首页 > 其他分享 >Vins-Mono 阅读笔记——estimator

Vins-Mono 阅读笔记——estimator

时间：2023-04-08 23:15:20浏览次数：49  
标签：Mono frame feature estimator imu msg Vins buf dt

vins_estimator 概述

基本上VINS里面绝大部分功能都在这个package下面，包括IMU数据的处理(前端)，初始化(我觉得可能属于是前端)，滑动窗口(后端)，非线性优化(后端)，关键帧的选取(部分内容)(前端)。我第一次看的时候，总是抱有一个疑问，就是为什么把这么多内容全都放在这一个node里面。为了回答这个问题，那么首先先搞清楚vins_estimator里面分别具体都是什么，为什么要有这些数据结构/函数，这些函数是怎样工作的。

ROS package组成

这个package下面主要以下文件：
factor——主要用于非线性优化对各个参数块和残差块的定义，VINS采用的是ceres，所以这部分需要对一些状态量和因子进行继承和重写。
initial——主要用于初始化，VINS采用的初始化策略是先SfM进行视觉初始化，再与IMU进行松耦合。
estimator.cpp——vins_estimator需要的所有函数都放在这里，是一个鸿篇巨制。
estimator_node.cpp——vins_estimator的入口，是一个ROS的node，实际上运行的是这个cpp文件。
feature_manager.cpp——负责管理滑窗内的所有特征点。
parameters.cpp——读取参数，是一个辅助函数。
utility——里面放着用于可视化的函数和tictok计时器。

然后就是要注意CmakeLists.txt和package.xml文件的写法，这两个文件也是相当于套公式，写错了就不能实现正常的ROS功能了。
image.png

main()入口

image.png

1、ROS初始化，设置句柄

    // 1、ROS初始化、设置句柄
    ros::init(argc, argv, "vins_estimator");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

2、读取参数，设置状态估计器参数

// 2、读取参数，设置状态估计器参数
    readParameters(n);
    estimator.setParameter();

3、发布用于RVIZ显示的topic

registerPub(n);

4、订阅imu_topic，执行回调imu_callback

ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    if (imu_msg->header.stamp.toSec() <= last_imu_t)
    {
        ROS_WARN("imu message in disorder!");
        return;
    }
    last_imu_t = imu_msg->header.stamp.toSec();
    m_buf.lock();
    imu_buf.push(imu_msg);
    m_buf.unlock();
    con.notify_one();
    last_imu_t = imu_msg->header.stamp.toSec();
    {
        std::lock_guard<std::mutex> lg(m_state);
        predict(imu_msg);
        std_msgs::Header header = imu_msg->header;
        header.frame_id = "world";
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
    }
}

该回调主要干了3件事情：

  • 1、往imu_buf里面放imu数据
    • queue<sensor_msgs::ImuConstPtr> imu_buf;
  • 2、IMU预积分得到当前时刻的PVQ
    • predict(imu_msg)
  • 3、如果当前处于非线性优化阶段的话，需要把第二件事计算得到的PVQ发布到rviz里去，见utility/visualization.cpp的pubLatestOdometry()函数

5、订阅/feature_tracker/feature，执行feature_callback

ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);

// feature回调函数，将feature_msg放入feature_buf
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
    if (!init_feature)
    {
        //skip the first detected feature, which doesn't contain optical flow speed
        init_feature = 1;
        return;
    }
    m_buf.lock();
    feature_buf.push(feature_msg);
    m_buf.unlock();
    con.notify_one();
}

这一部分接收的是feature_tracker_node发布的在cur帧的所有特征点的信息。
feature_callback就只干了一件事，就是把cur帧的所有特征点放到feature_buf里，同样需要上锁。注意，cur帧的所有特征点都是整合在一个数据里的，也就是sensor_msgs::PointCloudConstPtr &feature_msg。

6、订阅/feature_tracker/restart，执行restart_callback

ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);

7、订阅/pose_graph/match_points，执行relocalization_callback

ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);

8、创建主线程process

std::thread measurement_process{process};

这一部分是最重要的，包含了VINS绝大部分内容和最难的内容。

8.1 对imu和图像数据进行对齐
 std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
              
        std::unique_lock<std::mutex> lk(m_buf);
        con.wait(lk, [&]{return (measurements = getMeasurements()).size() != 0; });
        lk.unlock();

:::success
数据结构: measurements
1、首先，measurements他自己就是一个vector；
2、对于measurements中的每一个measurement，又由2部分组成；
3、第一部分，由sensor_msgs::ImuConstPtr组成的vector；
4、第二部分，一个sensor_msgs::PointCloudConstPtr；
5、这两个sensor_msgs见3.1-6部分介绍。
6、为什么要这样配对(一个PointCloudConstPtr配上若干个ImuConstPtr)？
因为IMU的频率比视觉帧的发布频率要高，所以说在这里，需要把一个视觉帧和之前的一串IMU帧的数据配对起来。

std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;

:::
容器measurements有了，接下来就是配对IMU和视觉帧的数据，放到容器里去。配对过程也需要上锁，而且是一个条件锁。作者在这里用了一个lambda表达式，也就是说，在return里面的部分是false时，保持上锁状态，继续配对数据；如果return里面时true，说明配对完成，释放锁，measurements完成了，以供后续使用。

接下来分析一下measurements()具体的功能：

// 对imu和图像数据进行对齐并组合（根据时间戳，挑选当前帧和上一帧的IMU数据，用于后续IMU积分）
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
{
    std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;

    while (true)
    {
        if (imu_buf.empty() || feature_buf.empty())
            return measurements;
        // imu   *******
        // image          *****
        // 这就是imu还没来
        if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            //ROS_WARN("wait for imu, only should happen at the beginning");
            sum_of_wait++;
            return measurements;
        }
        // imu        ****
        // image    ******
        // 这种只能扔掉一些image帧
        if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            ROS_WARN("throw img, only should happen at the beginning");
            feature_buf.pop();
            continue;
        }
        // 此时就保证了图像前一定有imu数据
        sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
        feature_buf.pop();
        // 一般第一帧不会严格对齐，但是后面就都会对齐，当然第一帧也不会用到
        std::vector<sensor_msgs::ImuConstPtr> IMUs;
        //imu     *****
        //image       ***
        //图像前一定有IMU数据
        while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
        {
            IMUs.emplace_back(imu_buf.front());
            imu_buf.pop();
        }
        // 保留图像时间戳后一个imu数据，但不会从buffer中扔掉
        // imu    *   *
        // image    *
        IMUs.emplace_back(imu_buf.front());
        if (IMUs.empty())
            ROS_WARN("no imu between two image");
        //每一项的格式
        // imu  ******   *
        // image       *
        measurements.emplace_back(IMUs, img_msg);  
    }
    return measurements;
}

image.png

8.2 对IMU数据进行处理

        // 2、对measurements中的每一个measurement(IMUs,IMG)组合进行操作
        for (auto &measurement : measurements)
        {
            auto img_msg = measurement.second;
            double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
            // 遍历imu
            for (auto &imu_msg : measurement.first)
            {
                double t = imu_msg->header.stamp.toSec();
                double img_t = img_msg->header.stamp.toSec() + estimator.td;
                if (t <= img_t)
                { 
                    if (current_time < 0)
                        current_time = t;
                    double dt = t - current_time; // 当前imu信息与上一个imu信息之间的时间差
                    ROS_ASSERT(dt >= 0);
                    current_time = t;
                    dx = imu_msg->linear_acceleration.x;
                    dy = imu_msg->linear_acceleration.y;
                    dz = imu_msg->linear_acceleration.z;
                    rx = imu_msg->angular_velocity.x;
                    ry = imu_msg->angular_velocity.y;
                    rz = imu_msg->angular_velocity.z;
                    // 时间差和imu数据送进去
                    // 2.1、对于measurement中的每一个imu_msg，计算dt并执行processIMU()。
                    //processIMU()实现了IMU的预积分，通过中值积分得到当前PQV作为优化初值
                    estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
                    //printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);

                }
                else    // 这就是针对最后一个imu数据，需要做一个简单的线性插值
                {
                    double dt_1 = img_t - current_time;
                    double dt_2 = t - img_t;
                    current_time = img_t;
                    ROS_ASSERT(dt_1 >= 0);
                    ROS_ASSERT(dt_2 >= 0);
                    ROS_ASSERT(dt_1 + dt_2 > 0);
                    double w1 = dt_2 / (dt_1 + dt_2);
                    double w2 = dt_1 / (dt_1 + dt_2);
                    dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
                    dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
                    dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
                    rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
                    ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
                    rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
                    estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
                    //printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
                }
            }

processIMU()

:::success
/这一段作用就是就是给以下数据提供初始值/初始化：

pre_integrations[frame_count]
dt_buf[frame_count]
linear_acceleration_buf[frame_count]
angular_velocity_buf[frame_count]
Rs[frame_count]
PS[frame_count]
Vs[frame_count]

  • TODO 关于frame_count的更新，目前只在process_img里的solver_flag == INITIAL这里看到?

*/
:::

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
    if (!first_imu)
    {
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }
    // 滑窗中保留11帧，frame_count表示现在处理第几帧，一般处理到第11帧时就保持不变了
    // 由于预积分是帧间约束，因此第1个预积分量实际上是用不到的
    if (!pre_integrations[frame_count])
    {
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }
    // 所以只有大于0才处理
    if (frame_count != 0)
    {
        //pre_integrations[frame_count]、tmp_pre_integration[frame_count]的初始化
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

        //dt、加速度、角速度放入buf中
        dt_buf[frame_count].push_back(dt);
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

        
        // 又是一个中值积分，更新滑窗中状态量，本质是给非线性优化提供可信的初始值
        int j = frame_count;       
        
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;   //a0=Q(a^-ba)-g     上一帧速度

        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];    //w=1/2(w0+w1)-bg

        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();       //更新R

        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g; //a1   当前帧速度

        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);          //中值积分a=0.5*(a0+a1)

        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;           //Pk+1=Pk+v*t+1/2*a*t^2
        
        Vs[j] += dt * un_acc;                                   //Vk+1=Vk+a*t
    }
    //当frame_count==0时表示华东窗口内还没有图像帧数据，不需要进行预积分，只进行加速度和角速度初始值的更新
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

:::success
数据结构:
1、Rs[frame_count]，Ps[frame_count]，Vs[frame_count]:是从IMU系转到world系的PVQ，数据是由IMU预积分得到的，目前在这里存放的是没有用bias修正过的值。
2、frame_count:这个值让我很疑惑，它只在processImage()里有过++操作，而且在estimator.hpp声明的时候，没有加上static关键字。它是在h文件中声明，在cpp文件里初始化的，后续需要再关注一下。
3、dt_buf，linear_acceleration_buf，angular_velocity_buf：帧数和IMU测量值的缓存，而且它们是对齐的。
3、pre_integrations[frame_count]，它是IntegrationBase的一个实例，在factor/integration_base.h中定义，它保存着frame_count帧中所有跟IMU预积分相关的量，包括F矩阵，Q矩阵，J矩阵等。
:::

新的理解

这里processIMU后面的中值积分部分其实就是当前时刻PVQ的中值离散形式，它表示了从第i个IMU时刻到i+1个IMU时刻的积分过程，代码中的j就是i+1时刻，IMU积分出来的第j时刻的物理量可以作为第j帧图像的视觉估计初值（本质就是提供了一个初值）
注意这里的Rs[j]、Ps[j]，Vs[j]并不是预积分，而是当前时刻的离散形式的中值积分
image.png

预积分是两帧之间的PVQ增量形式，与这里的当前时刻不同

8.3 重定位和回环检测

回到estimator_node.cpp的process()函数。接下来的代码的作用是，在relo_buf中取出最后一个重定位帧，拿出其中的信息并执行setReloFrame()

        // set relocalization frame
        sensor_msgs::PointCloudConstPtr relo_msg = NULL;
   //2.2、在relo_buf中取出最后一个重定位帧，拿出其中的信息并执行setReloFrame()
        while (!relo_buf.empty())
        {
            relo_msg = relo_buf.front();
            relo_buf.pop();
        }
        
        if (relo_msg != NULL)
        {
            vector<Vector3d> match_points;
            double frame_stamp = relo_msg->header.stamp.toSec();
            for (unsigned int i = 0; i < relo_msg->points.size(); i++)
            {
                Vector3d u_v_id;
                u_v_id.x() = relo_msg->points[i].x;
                u_v_id.y() = relo_msg->points[i].y;
                u_v_id.z() = relo_msg->points[i].z;
                match_points.push_back(u_v_id);
            }
            Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
            Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
            Matrix3d relo_r = relo_q.toRotationMatrix();
            int frame_index;
            frame_index = relo_msg->channels[0].values[7];
            estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);//设置重定位帧
        }

8.4 对image信息进行处理

(1)一开始，就定义了一个新的数据结构，解释一下：

map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;

:::success
数据结构: map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image
1、虽然它叫image，但是这个容器里面存放的信息是每一个特征点的！
2、索引值是feature_id；
3、value值是一个vector，如果系统是多目的，那么同一个特征点在不同摄像头下会有不同的观测信息，那么这个vector，就是存储着某个特征点在所有摄像头上的信息。对于VINS-mono来说，value它不是vector，仅仅是一个pair，其实是可以的。
4、接下来看这个vector里面的每一pair。int对应的是camera_id，告诉我们这些数据是当前特征点在哪个摄像头上获得的。
5、Matrix<double, 7, 1>是一个7维向量，依次存放着当前feature_id的特征点在camera_id的相机中的归一化坐标，像素坐标和像素运动速度，这些信息都是在feature_tracker_node.cpp中获得的。

:::
(2)这个数据结构定义完之后，接下来就是往这个容器中放数据。

//遍历img_msg里面的每一个特征点的归一化坐标
            for (unsigned int i = 0; i < img_msg->points.size(); i++)
            {
                //把img的信息提取出来放在image容器里去，通过这里，可以理解img信息里面装的都是些什么
                int v = img_msg->channels[0].values[i] + 0.5; //channels[0].values[i]==id_of_point
               //hash
                int feature_id = v / NUM_OF_CAM;
                int camera_id = v % NUM_OF_CAM;
           
                double x = img_msg->points[i].x;
                double y = img_msg->points[i].y;
                double z = img_msg->points[i].z;
                double p_u = img_msg->channels[1].values[i];
                double p_v = img_msg->channels[2].values[i];
                double velocity_x = img_msg->channels[3].values[i];
                double velocity_y = img_msg->channels[4].values[i];
           
                Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
                xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
                image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
            }


(3) 处理图像processImage() (核心！)

estimator.processImage(image, img_msg->header);//处理图像帧：初始化，紧耦合的非线性优化

8.5 可视化

向RVIZ发布里程计信息、关键位姿、相机位姿、点云和TF关系，这些函数都定义在中utility/visualization.cpp里，都是ROS相关代码。

pubOdometry(estimator, header);
pubKeyPoses(estimator, header);
pubCameraPose(estimator, header);
pubPointCloud(estimator, header);
pubTF(estimator, header);
pubKeyframe(estimator);
if (relo_msg != NULL)
    pubRelocalization(estimator);

8.6 IMU的PVQ信息更新

更新IMU参数[P,Q,V,ba,bg,a,g]，需要上锁，注意线程安全。这里对应的知识点是4.1.1最后一个公式。

标签：Mono,frame,feature,estimator,imu,msg,Vins,buf,dt
From： https://www.cnblogs.com/hongweijiang/p/17299491.html

相关文章

  • Vins-Mono 阅读笔记——前端
    1、前端流程概述VINS-Mono的前端整个封装成了一个ROS节点其订阅的topic是：相机或者数据集发来的图片其发布topic是：由pub_img发布的"feature"，发布的是当前帧的特征点，特征点分装成了sensor_msgs::PointCloudPtr类型，里面包括了每个特征点的（1）归一化平面上的坐标（2）归一化平面上的......
  • monodb4.4 index《一》
    1.索引简介(1).准备基础数据for(i=0;i<1000000;i++){db.users.insertOne({"i":i,username:"user"+i,age:Math.floor(Math.random()*120),created:newDate()})}(2).然后随机查找一个用户wang>db.users.find({"username":"user101"}).explai......
  • monodb4.4 index《二》
    1.使用复合索引(1).索引的方向2个字段都按照升序{"age":1,"username":1}age按照升序，username按照降序{"age":1,"username":-­1}以下2个索引是一样的{"age":1,"username":­-1}{"age":-­1,"username":1}(2).......
  • Convex Analysis and Monotone Operator Theory in Hilbert Spaces 3.1-3.2 总结材料
    拓扑空间基本概念  集合是数学中最基本的概念之一，我们最常见的集合便是\(\mathbb{R}\)。\(\mathbb{R}\)中的元素有大小关系，即\(\mathbb{R}\)上有序结构；\(\mathbb{R......
  • Vins 前端中高效的去畸变的方式解析
    Vins前端中高效的去畸变的方式解析1.0畸变是如何产生的\(\quad\)我们先来想想3D点是如何投影到图像平面的：世界坐标点经过一个外参矩阵得到相机坐标系下的位置，由于我们......
  • pymonodb 批量更新数据
      frompymongoimportUpdateOneaction=list()withopen("/Users/wyc/work/geneSearch/server/search/userdatagene.fixed.txt","r")......
  • SREWorks前端低代码组件生态演进：monorepo架构重构和远程组件加载实践
    作者：王威（地谦）文章结构项目背景演进分析monorepo架构演进Webpack与Rollup如何平滑迁移构建优化组件的可扩展与可插拔演进总结版本动态项目背景SREWorks是一个面向企业级复杂......
  • K8s:Monokle Desktop 一个集Yaml资源编写、项目管理、集群管理的 K8s IDE
    写在前面MonokleDesktop是kubeshop推出的一个开源的K8sIDE相关项目还有MonokleCLI和MonokleCloud相比其他的k8s管理工具，MonokleDesktop功能较全面，涉及......
  • 使用Mono.Cecil实现IL代码注入
    Target新建项目CecilTest创建待注入类Target.cspublicclassTarget{publicstringGetInputIntReturnIntString(inti){Console.WriteLine("Get......
  • monodepth2学习
    monodepth2学习相关网站​​https://www.cnblogs.com/blackworld-sp/category/2205853.html​​​​https://blog.csdn.net/avideointerfaces/article/details/105925104​......

赞助商

阅读排行

网站主页关于我们联系我们网站地图

本网站内容转载自其他媒体，侵权联系[admin##ips99.com]。

Copyright © 2020-2023 IPS99 版权所有 IPS99