以双目+IMU为例 Stereo cameras + IMU
github中给出运行命令为
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag
//查看catkin_ws/src/VINS-Fusion/vins_estimator/CMakeLists.txt
add_executable(vins_node src/rosNodeTest.cpp)
程序入口：VINS-Fusion/vins-estimator/src/rosNodeTest.cpp
接下来从src/rosNodeTest.cpp文件开始入手
1. vins_estimator:: rosNodeTest.cpp中
首先 全局estimator
Estimator estimator;
Estimator类成员内部有两个重要的自定义类成员：
FeatureManager f_manager; //用来对滑动窗口内所有特征点的管理。
FeatureTracker featureTracker; //用来对原始图像进行畸变校正，特征点采集，光流跟踪
和 全局Buf队列
queue<sensor_msgs::ImuConstPtr> imu_buf;
queue<sensor_msgs::PointCloudConstPtr> feature_buf;
queue<sensor_msgs::ImageConstPtr> img0_buf;
queue<sensor_msgs::ImageConstPtr> img1_buf;
然后 main（）函数中：
a.读取配置文件参数 readParameter()
b.设置参数 estimator.setParameter()
//设置相机参数 开启滑动窗口估计的一个新线程Estimator::processMeasurements
c.订阅了四个话题，分别是imu消息，两个相机图像，和feature_tracker所提供的跟踪光流点
回调函数主要就是把左右img和imu数据存储到相应的缓冲区内
d.开启了一个新线程sync_process。
线程的作用：如果图像buffer里面有数据的话,读入数据并且添加到estimator中。
2.线程Estimator::processMeasurements()
在estimator.cpp 文件 Estimator::setParameter()中
//catkin_ws/src/VINS-Fusion/vins_estimator/src/estimator/estimator.cpp
void Estimator::setParameter()
{
...
{
initThreadFlag = true;
processThread = std::thread(&Estimator::processMeasurements, this);
}
...
}
Estimator::processMeasurements() 中
void Estimator::processMeasurements()
{
...
if(!initFirstPoseFlag)
initFirstIMUPose(accVector);
...
//对IMU进行预积分
processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
//对图像进行处理
processImage(feature.second, feature.first);
...
//处理各个Buf队列中的数据
pubOdometry(*this, header);
pubKeyPoses(*this, header);
pubCameraPose(*this, header);
pubPointCloud(*this, header);
pubKeyframe(*this);
pubTF(*this, header);
processIMU（）： 对IMU进行预积分
processIMage（）：对图像进行处理
pub VIO的各种话题，包括里程计信息，tf变换，相机姿态，点云信息，并且发布关键帧。
3.线程void sync_process()
在sync_process是对buf中数据进行处理
读入数据
如果图像buffer里面有数据的话,读入数据并且添加到estimator中
当判断为双目的时候，首先判断img0_buf、img1_buf缓冲区和时标是否异常，正常的话，通过
3.1 getImageFromMsg（）函数获取图片，
该函数主要功能就是把ros中sensor_msgs::ImageConstPtr类型的图片转换成opencv中的标准图像格式cv::Mat输出。
3.2 estimator.inputImage（）
最后把两个cv::Mat格式的图片通过estimator.inputImage（）函数传入，之后便进行特征提取。
（需要注意，VINS-Mono里面feature_tracker和estimator是完全分开的，而这里feature_tracker包含在estimator里，唯一的差别在于减少了前端跟踪和后端之间的rostopic的发布和订阅）
———————————————
原文链接：https://blog.csdn.net/weixin_41843971/article/details/86537228
其中 3.2estimator.inputImage（）
estimator.inputImage(time, image0, image1)
{
...
featureFrame = featureTracker.trackImage(t, _img, _img1);
...
featureBuf.push(make_pair(t, featureFrame));
...
processMeasurements();
...
}
提取特征
暂时略过不深究
3.2.1函数featureTracker.trackImage(t, _img, _img1) 跟踪特征
在catkin_ws/src/VINS-Fusion/vins_estimator/src/featureTracker/feature_tracker.cpp 中
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>
FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1)
{
// 接受参数 当前相机图像
// 构造并返回featureFrame 特征容器
...
...
return featureFrame;
}
跟踪完成后，构建一个特征容器
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame
第一个参数int为feature的id，
第二个参数vector里面的int为相机id（0为左图，1为右图），
参数Eigen::Matrix类型里面包含该特征点在该相机下的信息
分别为:归一化平面坐标（x,y,z=1），像素坐标（u,v），像素移动速度（v_x,v_y），共七维。
然后将featureFrame加入到featurebuf中，传到后端进行图像处理。
void Estimator::processMeasurements()函数对featureBuf中特征进行处理(就是后端内容啦？？）
//catkin_ws/src/VINS-Fusion/vins_estimator/src/estimator/estimator.cpp
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;
...
}
}
}
后端Estimator::processMeasurements()
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;
// 对IMU进行处理 预积分
processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
}
}
mProcess.lock();
// processImage
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);
}
}
