【SLAM-Vins_Fusion】代码总览

以双目+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);
    }
}
©著作权归作者所有,转载或内容合作请联系作者
  • 序言:七十年代末,一起剥皮案震惊了整个滨河市,随后出现的几起案子,更是在滨河造成了极大的恐慌,老刑警刘岩,带你破解...
    沈念sama阅读 201,552评论 5 474
  • 序言:滨河连续发生了三起死亡事件,死亡现场离奇诡异,居然都是意外死亡,警方通过查阅死者的电脑和手机,发现死者居然都...
    沈念sama阅读 84,666评论 2 377
  • 文/潘晓璐 我一进店门,熙熙楼的掌柜王于贵愁眉苦脸地迎上来,“玉大人,你说我怎么就摊上这事。” “怎么了?”我有些...
    开封第一讲书人阅读 148,519评论 0 334
  • 文/不坏的土叔 我叫张陵,是天一观的道长。 经常有香客问我,道长,这世上最难降的妖魔是什么? 我笑而不...
    开封第一讲书人阅读 54,180评论 1 272
  • 正文 为了忘掉前任,我火速办了婚礼,结果婚礼上,老公的妹妹穿的比我还像新娘。我一直安慰自己,他们只是感情好,可当我...
    茶点故事阅读 63,205评论 5 363
  • 文/花漫 我一把揭开白布。 她就那样静静地躺着,像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上,一...
    开封第一讲书人阅读 48,344评论 1 281
  • 那天,我揣着相机与录音,去河边找鬼。 笑死,一个胖子当着我的面吹牛,可吹牛的内容都是我干的。 我是一名探鬼主播,决...
    沈念sama阅读 37,781评论 3 393
  • 文/苍兰香墨 我猛地睁开眼,长吁一口气:“原来是场噩梦啊……” “哼!你这毒妇竟也来了?” 一声冷哼从身侧响起,我...
    开封第一讲书人阅读 36,449评论 0 256
  • 序言:老挝万荣一对情侣失踪,失踪者是张志新(化名)和其女友刘颖,没想到半个月后,有当地人在树林里发现了一具尸体,经...
    沈念sama阅读 40,635评论 1 295
  • 正文 独居荒郊野岭守林人离奇死亡,尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 35,467评论 2 317
  • 正文 我和宋清朗相恋三年,在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 37,515评论 1 329
  • 序言:一个原本活蹦乱跳的男人离奇死亡,死状恐怖,灵堂内的尸体忽然破棺而出,到底是诈尸还是另有隐情,我是刑警宁泽,带...
    沈念sama阅读 33,217评论 3 318
  • 正文 年R本政府宣布,位于F岛的核电站,受9级特大地震影响,放射性物质发生泄漏。R本人自食恶果不足惜,却给世界环境...
    茶点故事阅读 38,775评论 3 303
  • 文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹,春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 29,851评论 0 19
  • 文/苍兰香墨 我抬头看了看天上的太阳。三九已至,却和暖如春,着一层夹袄步出监牢的瞬间,已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 31,084评论 1 258
  • 我被黑心中介骗来泰国打工, 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留,地道东北人。 一个月前我还...
    沈念sama阅读 42,637评论 2 348
  • 正文 我出身青楼,却偏偏与公主长得像,于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子,可洞房花烛夜当晚...
    茶点故事阅读 42,204评论 2 341

推荐阅读更多精彩内容