深藍學院《從零開始手寫VIO》作業七

• 深藍學院《從零開始手寫VIO》作業七

深藍學院《從零開始手寫VIO》作業七

將第二講中的仿真數據（視覺特征，imu數據）接入VINS系統，并運行出軌跡結果

第一步：將IMU數據傳入VINS系統
這部分的代碼實現在run_euroc.cpp中的PubSimImuData部分，如下：

void PubSimImuData() {string sImu_data_file = sConfig_path + "imu_pose_noise.txt"; cout << "1 PubImuData start sImu_data_file: " << sImu_data_file << endl;ifstream fsImu;fsImu.open(sImu_data_file.c_str());if (!fsImu.is_open()){cerr << "Failed to open imu file! " << sImu_data_file << endl;return;}std::string sImu_line;double dStampNSec = 0.0;double tmp;Vector3d vAcc;Vector3d vGyr;while (std::getline(fsImu, sImu_line) && !sImu_line.empty()) // read imu data{std::istringstream ssImuData(sImu_line);ssImuData >> dStampNSec;for(int i=0;i<7;i++)ssImuData>>tmp; ssImuData>>vGyr.x() >> vGyr.y() >> vGyr.z() >> vAcc.x() >> vAcc.y() >> vAcc.z(); pSystem->PubImuData(dStampNSec, vGyr, vAcc);usleep(5000*nDelayTimes);}fsImu.close(); }

第二步：將特征數據傳入VINS系統
讀入特征數據的代碼實現在run_euroc.cpp中的PubSimImageData部分，如下：

void PubSimImageData() {string sImage_file = sConfig_path + "cam_pose.txt"; //包含時間戳的文件cout << "1 PubImageData start sImage_file: " << sImage_file << endl;ifstream fsImage;fsImage.open(sImage_file.c_str());if (!fsImage.is_open()){cerr << "Failed to open image file! " << sImage_file << endl;return;}std::string sImage_line;double dStampNSec;string sImgFileName;int n=0;while (std::getline(fsImage, sImage_line) && !sImage_line.empty()){std::istringstream ssImgData(sImage_line);ssImgData >> dStampNSec; //讀入時間戳cout<<"cam time: "<<fixed<<dStampNSec<<endl;string all_points_file_name = sPointData_path + "all_points_" + to_string(n)+ ".txt"; //第n個相機對應的觀測數據的文件名cout<<"points_file: "<<all_points_file_name<<endl;pSystem->PubSimImageData(dStampNSec, all_points_file_name);usleep(50000*nDelayTimes);n++;}fsImage.close(); }

第三步：修改System.cpp
通過函數loadPointData傳入double時間戳和string文件名，如下：

TicToc t_r;//trackerData[0].readImage(img, dStampSec);trackerData[0].loadPointData(points_file_name,dStampSec);

第四步：修改feature_tracker.cpp
由于沒有圖像，并且所有的特征點都在每一個相機中被成功跟蹤，所以就不用光流函數追蹤。直接讀入當前時刻相機對應的觀測值并存入forw_pts即可，并將每個特征點的track_cnt++。對于初始時刻，由于cur_pts是空的，需要對該時刻的相機觀測值forw_pts進行初始化，將該時刻相機對特征點的觀測值（像素坐標）存入n_pts中，然后調用函數addPoints來完成forw_pts,ids,track_cnt的初始化。最后，將forw_pts賦值給cur_pts。并通過函數undistoredSimPoints來計算像素點的速度，如下：

void FeatureTracker::undistortedSimPoints() {cur_un_pts.clear();cur_un_pts_map.clear();//cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());for (unsigned int i = 0; i < cur_pts.size(); i++){Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);cur_un_pts.push_back(cv::Point2f(cur_pts[i].x,cur_pts[i].y));cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(cur_pts[i].x,cur_pts[i].y)));//printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);}// caculate points velocityif (!prev_un_pts_map.empty()){double dt = cur_time - prev_time;pts_velocity.clear();for (unsigned int i = 0; i < cur_un_pts.size(); i++){if (ids[i] != -1){std::map<int, cv::Point2f>::iterator it;it = prev_un_pts_map.find(ids[i]);if (it != prev_un_pts_map.end()){double v_x = (cur_un_pts[i].x - it->second.x) / dt;double v_y = (cur_un_pts[i].y - it->second.y) / dt;pts_velocity.push_back(cv::Point2f(v_x, v_y));}elsepts_velocity.push_back(cv::Point2f(0, 0));}else{pts_velocity.push_back(cv::Point2f(0, 0));}}}else{for (unsigned int i = 0; i < cur_pts.size(); i++){pts_velocity.push_back(cv::Point2f(0, 0));}}prev_un_pts_map = cur_un_pts_map; }

實驗結果
(1)無噪聲仿真數據

（2）有噪聲數據

其中加速度計的bias隨機游走的方差為0.0001，高斯噪聲的方差為0.019; 陀螺儀的bias隨機游走的方差為為1e-5,高斯噪聲的方差為0.015

（3）實驗結果對比
a. 帶噪聲數據

b.不帶噪聲數據


結論：帶噪聲的數據比不帶噪聲的數據效果聲好。

總結

以上是生活随笔為你收集整理的深蓝学院《从零开始手写VIO》作业七的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。