ROS-3DSLAM（7）：视觉部分visual-estimator第一节：initial初步分析

2021@SDUSC

2021年11月2日星期二——2021年11月4日星期四

2021年11月10日星期三——2021年11月11日星期四

附：发烧导致学习进度中断。

一、学习背景：

经过前几周对于雷达部分的学习，我对于ros以及LVI-SLAM包有了初步的认识。我们小组的下一个目标就是视觉部分，经过讨论和对于信息流的分析，我们决定按照visual_estimator，visual_feature和visual_loop的顺序来学习。

因为LVI-SAM的视觉部分是建立在VINS-MONO的基础之上的，而后者的包的分析资料网上有很多，所以我们可以以此作为参考来学习。但是同时也要重点注意LVI-SAM是融合了雷达和视觉系统的存在，在查阅资料时要注意区分不同点。

根据对于整个项目的规划，我计划利用十篇文章左右的篇幅来介绍这个visual_estimator。

在visual-estimator包下，我的第一个任务是对于initial部分的分析。

在学习的过程中，我发现在阅读代码时经常会陷入不知道在解释什么的困境，于是决定回到VIN-Mono这篇论文中去，通过阅读论文来为后续的学习铺垫基础。因此，在继续这部分的学习之前，我先阅读一下VIN-Mono的论文。

二、文件概述：

Visual_estimator:

在visual_estimator下，主要有三个文件夹：factor，initial和utility及3个头文件与4个cpp文件。

• initial：初始化。
• factor：非线性优化。
• utility：辅助设备？可能是存放着辅助其他文件完成功能的函数。
• estimator.cpp：主角，所有的函数都在其中。
• estimator_node.cpp：ros节点，也是上一个函数的入口。
• feature_manager.cpp：特征点管理
• parameters.cpp：读取参数的函数，并不是参数

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ERHJtNew-1636788956482)(C:\Users\Fan luke\Desktop\image-20211102103333149.png)]

initial

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-XvtC7tCi-1636788956486)(C:\Users\Fan luke\Desktop\image-20211110151329327.png)]

文件夹内部一共有四个cpp文件和四个与之对应的头文件。

三、initial内

1、初始化原理

这里参考了古月局上大佬的分析图，出处附在文末，文章中给出了详细的数学推导过程。

我正是因为卡在了这个地方，所以4号的时候不得不去看了看论文的讲解，并把学习的内容记录在了上一篇博客之中，这里就不再赘述了。

2、第一个函数：initial_alignment

A、头文件：

using namespace Eigen;
using namespace std;

class ImageFrame
{
   
    public:
        ImageFrame(){
   };
        ImageFrame(const map<int, vector<pair<int, Eigen::Matrix<double, 8, 1>>>>& _points, 
                   const vector<float> &_lidar_initialization_info,
                   double _t):
        t{
   _t}, is_key_frame{
   false}, reset_id{
   -1}, gravity{
   9.805}
        {
   
            points = _points;
            
            // reset id in case lidar odometry relocate
            reset_id = (int)round(_lidar_initialization_info[0]);
            // Pose
            T.x() = _lidar_initialization_info[1];
            T.y() = _lidar_initialization_info[2];
            T.z() = _lidar_initialization_info[3];
            // Rotation
            Eigen::Quaterniond Q = Eigen::Quaterniond(_lidar_initialization_info[7],
                                                      _lidar_initialization_info[4],
                                                      _lidar_initialization_info[5],
                                                      _lidar_initialization_info[6]);
            R = Q.normalized().toRotationMatrix();
            // Velocity
            V.x() = _lidar_initialization_info[8];
            V.y() = _lidar_initialization_info[9];
            V.z() = _lidar_initialization_info[10];
            // Acceleration bias
            Ba.x() = _lidar_initialization_info[11];
            Ba.y() = _lidar_initialization_info[12];
            Ba.z() = _lidar_initialization_info[13];
            // Gyroscope bias
            Bg.x() = _lidar_initialization_info[14];
            Bg.y() = _lidar_initialization_info[15];
            Bg.z() = _lidar_initialization_info[16];
            // Gravity
            gravity = _lidar_initialization_info[17];
        };

        map<int, vector<pair<int, Eigen::Matrix<double, 8, 1>> > > points;
        double t;
        
        IntegrationBase *pre_integration;
        bool is_key_frame;

        // Lidar odometry info
        int reset_id;
        Vector3d T;
        Matrix3d R;
        Vector3d V;
        Vector3d Ba;
        Vector3d Bg;
        double gravity;
};


bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x);


class odometryRegister
{
   
public:

    ros::NodeHandle n;
    tf::Quaternion q_lidar_to_cam;
    Eigen::Quaterniond q_lidar_to_cam_eigen;

    ros::Publisher pub_latest_odometry; 

    odometryRegister(ros::NodeHandle n_in):
    n(n_in)
    {
   
        q_lidar_to_cam = tf::Quaternion(0, 1, 0, 0); // rotate orientation // mark: camera - lidar
        q_lidar_to_cam_eigen = Eigen::Quaterniond(0, 0, 0, 1); // rotate position by pi, (w, x, y, z) // mark: camera - lidar
        // pub_latest_odometry = n.advertise<nav_msgs::Odometry>("odometry/test", 1000);
    }

    // convert odometry from ROS Lidar frame to VINS camera frame
    vector<float> getOdometry(deque<nav_msgs::Odometry>& odomQueue, double img_time)
    {
   
        vector<float> odometry_channel;
        odometry_channel.resize(18, -1); // reset id(1), P(3), Q(4), V(3), Ba(3), Bg(3), gravity(1)

        nav_msgs::Odometry odomCur;
        
        // pop old odometry msg
        while (!odomQueue.empty()) 
        {
   
            if (odomQueue.front().header.stamp.toSec() < img_time - 0.05)
                odomQueue.pop_front();
            else
                break;
        }

        if (odomQueue.empty())
        {
   
            return odometry_channel;
        }

        // find the odometry time that is the closest to image time
        for (int i = 0; i < (int)odomQueue.size(); ++i)
        {
   
            odomCur = odomQueue[i];

            if (odomCur<