VINS-Fusion代码按执行顺序阅读(二)

    xiaoxiao2022-07-02  143

    main()函数的最后一句,std::thread sync_thread{sync_process}; 可以看出,只有time0在time1前后0.03s内的两幅图片,才会被发布estimator.inputImage(time, image0, image1);。

    // extract images with same timestamp from two topics void sync_process() { while(1) { if(STEREO) // STEREO 取值为1 表示双目,为0 表示单目 { cv::Mat image0, image1; std_msgs::Header header; double time = 0; m_buf.lock(); if (!img0_buf.empty() && !img1_buf.empty()) { double time0 = img0_buf.front()->header.stamp.toSec(); double time1 = img1_buf.front()->header.stamp.toSec(); // 0.003s sync tolerance if(time0 < time1 - 0.003) { img0_buf.pop(); printf("throw img0\n"); } else if(time0 > time1 + 0.003) { img1_buf.pop(); printf("throw img1\n"); } else { time = img0_buf.front()->header.stamp.toSec(); header = img0_buf.front()->header; image0 = getImageFromMsg(img0_buf.front()); img0_buf.pop(); image1 = getImageFromMsg(img1_buf.front()); img1_buf.pop(); //printf("find img0 and img1\n"); } } m_buf.unlock(); if(!image0.empty()) estimator.inputImage(time, image0, image1); } std::chrono::milliseconds dura(2); // 2ms std::this_thread::sleep_for(dura); } }

    进入estimator类中查看inputImage()类成员函数: int inputImageCnt;是一个类数据成员 对于双目调用featureFrame = featureTracker.trackImage(t, _img, _img1);,前面阅读的代码派上用场了!这一句话完成了解析上13页(四、前端视觉处理)的部分。

    SHOW_TRACK变量的含义?如果为真,则发布消息pubTrackImage(imgTrack, t); imgTrack为FeatureTracker类的数据成员。

    queue<pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > > featureBuf; 为Estimator类的一个数据成员,是一个队列。

    进入到Estimator类的一个函数成员void processMeasurements();

    void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1) { inputImageCnt++; map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame; TicToc featureTrackerTime; if(_img1.empty()) featureFrame = featureTracker.trackImage(t, _img); else featureFrame = featureTracker.trackImage(t, _img, _img1); //printf("featureTracker time: %f\n", featureTrackerTime.toc()); if (SHOW_TRACK) { cv::Mat imgTrack = featureTracker.getTrackImage(); pubTrackImage(imgTrack, t); } if(MULTIPLE_THREAD) { if(inputImageCnt % 2 == 0) { mBuf.lock(); featureBuf.push(make_pair(t, featureFrame)); mBuf.unlock(); } } else { mBuf.lock(); featureBuf.push(make_pair(t, featureFrame)); mBuf.unlock(); TicToc processTime; processMeasurements(); printf("process time: %f\n", processTime.toc()); } }

    进入该函数会一直在此处循环,通过break方式退出的条件是 USE_IMU的含义?推测此处的取值为true

    下面进入Estimator类的另一个成员函数,getIMUInterval(),bool Estimator::getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector, vector<pair<double, Eigen::Vector3d>> &gyrVector) 该函数的目的很明确,将时间区间[t0,t1]之间的陀螺仪、加速度计输出的数据从accBuf和gyrBu取出存放在accVector和gyrVector两个vector中。

    如果还未初始化,即initFirstPoseFlag为false,则首先进行初始化。 感觉这个地方使用了双矢量定姿(一个是加速度计输出的平均值,一个是重力矢量)。还有些细节的地方没有理解(例如yaw用来干什么?),暂时略过。 最后,初始姿态保存为了Rs[0],是Estimator的一个类数据成员Matrix3d Rs[(WINDOW_SIZE + 1)];

    接下来调用了processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);,完成了之前介绍过的解析4页(二、IMU预积分)的当前时刻及帧间PVQ的求取,还有PVQ增量的误差、协方差及Jacobian。

    接下来调用了processImage(feature.second, feature.first);,有一种感觉,这里就是处理的核心了!去功能模块仔细阅读。

    void Estimator::processMeasurements() { while (1) { //printf("process measurments\n"); pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature; vector<pair<double, Eigen::Vector3d>> accVector, gyrVector; if(!featureBuf.empty()) { feature = featureBuf.front(); curTime = feature.first + td; while(1) { if ((!USE_IMU || IMUAvailable(feature.first + td))) break; else { printf("wait for imu ... \n"); if (! MULTIPLE_THREAD) return; std::chrono::milliseconds dura(5); std::this_thread::sleep_for(dura); } } mBuf.lock(); if(USE_IMU) getIMUInterval(prevTime, curTime, accVector, gyrVector); featureBuf.pop(); mBuf.unlock(); if(USE_IMU) { if(!initFirstPoseFlag) initFirstIMUPose(accVector); for(size_t i = 0; i < accVector.size(); i++) { double dt; if(i == 0) dt = accVector[i].first - prevTime; else if (i == accVector.size() - 1) dt = curTime - accVector[i - 1].first; else dt = accVector[i].first - accVector[i - 1].first; processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second); } } mProcess.lock(); processImage(feature.second, feature.first); prevTime = curTime; printStatistics(*this, 0); std_msgs::Header header; header.frame_id = "world"; header.stamp = ros::Time(feature.first); pubOdometry(*this, header); pubKeyPoses(*this, header); pubCameraPose(*this, header); pubPointCloud(*this, header); pubKeyframe(*this); pubTF(*this, header); mProcess.unlock(); } if (! MULTIPLE_THREAD) break; std::chrono::milliseconds dura(2); std::this_thread::sleep_for(dura); } }
    最新回复(0)