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("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.header.stamp.toSec() < img_time - 0.002) // 500Hz imu
                continue;
            else
                break;
        }

        // time stamp difference still too large
        if (abs(odomCur.header.stamp.toSec() - img_time) > 0.05)
        {
            return odometry_channel;
        }

        // convert odometry rotation from lidar ROS frame to VINS camera frame (only rotation, assume lidar, camera, and IMU are close enough)
        tf::Quaternion q_odom_lidar;
        tf::quaternionMsgToTF(odomCur.pose.pose.orientation, q_odom_lidar);

        tf::Quaternion q_odom_cam = tf::createQuaternionFromRPY(0, 0, M_PI) * (q_odom_lidar * q_lidar_to_cam); // global rotate by pi // mark: camera - lidar
        tf::quaternionTFToMsg(q_odom_cam, odomCur.pose.pose.orientation);

        // convert odometry position from lidar ROS frame to VINS camera frame
        Eigen::Vector3d p_eigen(odomCur.pose.pose.position.x, odomCur.pose.pose.position.y, odomCur.pose.pose.position.z);
        Eigen::Vector3d v_eigen(odomCur.twist.twist.linear.x, odomCur.twist.twist.linear.y, odomCur.twist.twist.linear.z);
        Eigen::Vector3d p_eigen_new = q_lidar_to_cam_eigen * p_eigen;
        Eigen::Vector3d v_eigen_new = q_lidar_to_cam_eigen * v_eigen;

        odomCur.pose.pose.position.x = p_eigen_new.x();
        odomCur.pose.pose.position.y = p_eigen_new.y();
        odomCur.pose.pose.position.z = p_eigen_new.z();

        odomCur.twist.twist.linear.x = v_eigen_new.x();
        odomCur.twist.twist.linear.y = v_eigen_new.y();
        odomCur.twist.twist.linear.z = v_eigen_new.z();

        // odomCur.header.stamp = ros::Time().fromSec(img_time);
        // odomCur.header.frame_id = "vins_world";
        // odomCur.child_frame_id = "vins_body";
        // pub_latest_odometry.publish(odomCur);

        odometry_channel[0] = odomCur.pose.covariance[0];
        odometry_channel[1] = odomCur.pose.pose.position.x;
        odometry_channel[2] = odomCur.pose.pose.position.y;
        odometry_channel[3] = odomCur.pose.pose.position.z;
        odometry_channel[4] = odomCur.pose.pose.orientation.x;
        odometry_channel[5] = odomCur.pose.pose.orientation.y;
        odometry_channel[6] = odomCur.pose.pose.orientation.z;
        odometry_channel[7] = odomCur.pose.pose.orientation.w;
        odometry_channel[8]  = odomCur.twist.twist.linear.x;
        odometry_channel[9]  = odomCur.twist.twist.linear.y;
        odometry_channel[10] = odomCur.twist.twist.linear.z;
        odometry_channel[11] = odomCur.pose.covariance[1];
        odometry_channel[12] = odomCur.pose.covariance[2];
        odometry_channel[13] = odomCur.pose.covariance[3];
        odometry_channel[14] = odomCur.pose.covariance[4];
        odometry_channel[15] = odomCur.pose.covariance[5];
        odometry_channel[16] = odomCur.pose.covariance[6];
        odometry_channel[17] = odomCur.pose.covariance[7];

        return odometry_channel;
    }
};

头文件定义了两个类和一个函数。

B、函数主体

文件开头是利用相机的旋转来标定imu的bias的函数。

#include ……………

void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
	 //接受参数，包括帧、矩阵、滑窗内的帧、以及三维信息方位bgs，
    Matrix3d A;
    Vector3d b;
    Vector3d delta_bg;
    A.setZero();
    b.setZero();
    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    //下边是利用公式，根据滑窗和帧来计算，for循环对应了公式当中的求和部分。根据介绍的公式是对应了Ax=B
        
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)   
    {
        frame_j = next(frame_i);
        MatrixXd tmp_A(3, 3);
        tmp_A.setZero();
        VectorXd tmp_b(3);
        tmp_b.setZero();
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;

    }
    //A是一个矩阵，可以LDLT分解，这里的ldlt函数正是这个功能，利用了IDLT分解求解线性方程。
    delta_bg = A.ldlt().solve(b);
    //传递ROS信息流
    ROS_INFO_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());
	//给滑窗内的imu预积分加上角速度bias
    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;
	//重新计算预积分。
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = next(frame_i);
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
    }
}

利用gw的模长已知这个先验条件进一步优化gc0

说明：这里的模长就是gw我们所测得真实的重力大小，

void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
    Vector3d g0 = g.normalized() * G.norm();
    Vector3d lx, ly;
    //VectorXd x;
    int all_frame_count = all_image_frame.size();
    int n_state = all_frame_count * 3 + 2 + 1;

    MatrixXd A{n_state, n_state};
    A.setZero();
    VectorXd b{n_state};
    b.setZero();

    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    //一开始也是设置参数，接受容器，这里需要注意下这个n维的Vector，根据论文中的公式可知，这里是用来建立切向空间的：> 因此，我们在其切线空间上用两个变量重新参数化重力
    for(int k = 0; k < 4; k++)
    {
        MatrixXd lxly(3, 2);
        //建立切向空间的函数
        lxly = TangentBasis(g0);
        int i = 0;
      
        //代入公式，求解答案
        for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
        {
            frame_j = next(frame_i);

            MatrixXd tmp_A(6, 9);
            tmp_A.setZero();
            VectorXd tmp_b(6);
            tmp_b.setZero();

            double dt = frame_j->second.pre_integration->sum_dt;


            tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
            tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;
            tmp_A.block<3, 1>(0, 8) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;     
            tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] - frame_i->second.R.transpose() * dt * dt / 2 * g0;

            tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
            tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
            tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;
            tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;


            Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
            //cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
            //MatrixXd cov_inv = cov.inverse();
            cov_inv.setIdentity();

            MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
            VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;

            A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
            b.segment<6>(i * 3) += r_b.head<6>();

            A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>();
            b.tail<3>() += r_b.tail<3>();

            A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();
            A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();
        }
        //ldlt法求解线性方程，并得到最终优化结果
            A = A * 1000.0;
            b = b * 1000.0;
            x = A.ldlt().solve(b);
        
            VectorXd dg = x.segment<2>(n_state - 3);
            g0 = (g0 + lxly * dg).normalized() * G.norm();
            //double s = x(n_state - 1);
    }   
    g = g0;
}

这个函数利用imu的平移来估计重力、各个bk帧的速度以及尺度的函数。

bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{	//开头依旧是接受参数，设置容器
    int all_frame_count = all_image_frame.size();
    int n_state = all_frame_count * 3 + 3 + 1;

    MatrixXd A{n_state, n_state};
    A.setZero();
    VectorXd b{n_state};
    b.setZero();

    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    
    int i = 0;
    //下边的for循环也是利用公式来计算求解的形式
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
    {
        frame_j = next(frame_i);

        MatrixXd tmp_A(6, 10);
        tmp_A.setZero();
        VectorXd tmp_b(6);
        tmp_b.setZero();

        double dt = frame_j->second.pre_integration->sum_dt;

        tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
        tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
        tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;     
        tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];
        //cout << "delta_p   " << frame_j->second.pre_integration->delta_p.transpose() << endl;
        tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
        tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
        tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();
        tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;
        //cout << "delta_v   " << frame_j->second.pre_integration->delta_v.transpose() << endl;

        Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
        //cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
        //MatrixXd cov_inv = cov.inverse();
        cov_inv.setIdentity();

        MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
        VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;

        A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
        b.segment<6>(i * 3) += r_b.head<6>();

        A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();
        b.tail<4>() += r_b.tail<4>();

        A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();
        A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();
    }
    //为什么要乘上一千呢？
    A = A * 1000.0;
    b = b * 1000.0;
    //求解，依旧是LDLT分解
    x = A.ldlt().solve(b);
    //校正，校正这偏大的100？
    double s = x(n_state - 1) / 100.0;
    
    ROS_INFO("estimated scale: %f", s);
    //评估预测的重力是否正确，偏差大于一或是负数都表示计算出现了错误。
    g = x.segment<3>(n_state - 4);
    ROS_INFO_STREAM(" result g     " << g.norm() << " " << g.transpose());
    if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
    {
        return false;
    }

    RefineGravity(all_image_frame, g, x);
    s = (x.tail<1>())(0) / 100.0;
    (x.tail<1>())(0) = s;
    ROS_INFO_STREAM(" refine     " << g.norm() << " " << g.transpose());
    if(s < 0.0 )
        return false;   
    else
        return true;
}

这个是建立切向空间的函数，在重力优化时用到了。

MatrixXd TangentBasis(Vector3d &g0)
{
    Vector3d b, c;
    Vector3d a = g0.normalized();
    Vector3d tmp(0, 0, 1);
    if(a == tmp)
        tmp << 1, 0, 0;
    b = (tmp - a * (a.transpose() * tmp)).normalized();
    c = a.cross(b);
    MatrixXd bc(3, 2);
    bc.block<3, 1>(0, 0) = b;
    bc.block<3, 1>(0, 1) = c;
    return bc;
}

调用上述的函数，得到最终结果。

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

    if(LinearAlignment(all_image_frame, g, x))
        return true;
    else 
        return false;
}

四、问题及解答：

1、参数类型vector3，vector3d

ROS中的数据操作需要线性代数，Eigen库是C++中的线性代数计算库。
Eigen库独立于ROS，但是在ROS中可以使用。
Eigen库可以参见http://eigen.tuxfamily.org

可以理解成是python中的numpy。

其中Eigen下不仅有矩阵，还有向量。

向量是矩阵的特殊情况，也是用矩阵定义的。定义如下：

typedef Matrix Vector3f;
typedef Matrix RowVector2i;12

这里的Vector是几维的就是几d。

我们见到的vector3d是3维的。

而vector3是另一个包下的，有geometry_msgs/Vector3 Message。

2、 gw？gc0？有什么样的关系？

g表示的是gravity重力，w，c0都是我们之前提到的坐标系的名字，分别表示的是真实世界和参考c0.

我们在论文中介绍了重力精细化的步骤，表现在代码中就是RefineGravity这个函数。

3、Eigen常见函数，阅读代码需要

代码中涉及到Vector的地方有很多，不了解这些常用函数的话，看起来寸步难行，有必要学习一下。

读了文章之后有了初步的印象，把代码中出现的几个摘录下来：

C.setZero(rows,cols) // C = zeros(rows,cols) //全零矩阵

x.segment(i, n) // x(i+1 : i+n) //切片
x.segment(i) // x(i+1 : i+n) //切片

R.transpose() // R.’ or conj(R’) // 可读/写 转置

x.norm() // norm(x). //范数（注意：Eigen中没有norm®）

在Eigen中最基本的快操作运算是用.block()完成的。提取的子矩阵同样分为动态大小和固定大小。

x.cross(y) // cross(x, y) //交叉积，需要头文件 #include

五、参考资料：

VINS-Mono

VINS-Mono翻译_Pancheng1的博客-CSDN博客_vins-mono

Visual-Inertial Odometry - 知乎 (zhihu.com)

【SLAM】VINS-MONO解析——综述_iwanderu的博客-CSDN博客

从零手写VIO——（一）Introduction - 古月居 (guyuehome.com)

vi_sfm - ROS Wiki

详解SLAM中的两种常用的三角化求解地标点的方法_yg838457845的博客-CSDN博客_slam三角化

视觉SLAM中的对极约束、三角测量、PnP、ICP问题 - 古月居 (guyuehome.com)

例说姿态解算与导航18-IMU误差源 - 知乎 (zhihu.com)

SFM原理简介_lpj822的专栏-CSDN博客

[ROS中使用Eigen库不定期更新]_陈显森的博客-CSDN博客

EIgen:Matricx和vector类的定义和使用_飘零过客-CSDN博客

Eigen常用函数_yanzhiwen2的博客-CSDN博客

Eigen教程2----MatrixXd和VectorXd的用法_MaybeTnT的博客-CSDN博客