李群、李代数在SLAM中的应用

文章目录

李群、李代数
- 李群、李代书与坐标变换的对应关系
- SE(3)上的李代数求导数
- - 左乘扰动、右乘扰动
  - 怎么选取用左or右扰动？
SLAM中的使用
重投影误差
- 误差项构建
- 对应C++代码
- 雅克比矩阵
- - frame_i的Ti2w从IMU坐标系变换到world坐标系
  - frame_j的Ti2w从IMU坐标系变换到world坐标系
  - Tc2i从camera坐标系变换到imu坐标系(相机外参)
  - frame_i中的逆深度inv_dep_i

李群、李代数

数学概念什么的不做介绍了，本人不是数学专业也解释不清楚。咱的目的是怎么即使不是很能理解还能够把这个东西理解下来，并且还能看懂和编程使用。就是像考试怎么不会还能把它做对！
反正下面是本人不知道对不对的理解方式。

数学概念相关链接

视觉SLAM十四讲第四讲
等等

李群、李代书与坐标变换的对应关系

SE(3)上的李代数求导数

首先需要补充一个运算符号^:将向量转换为反对称矩阵

就是说对扰动求导数，SLAM中的主要应用就是这个其他的也不用很懂，记住下面的左乘、右乘的公式其实就行了，然后用公式推导3-4个雅克比矩阵基本上就会用了。但是《视觉SLAM十四讲》中只有左乘扰动的推导过程，其他的地方咱也没有搜到合适的右乘扰动的讲解。

左乘扰动、右乘扰动

公式大致如下

怎么选取用左or右扰动？

大致就是说转换到world坐标系或者global坐标系这些固定不变的坐标系使用右乘扰动，其他的loacl坐标系下都是左乘扰动。
借鉴参考这个https://zhuanlan.zhihu.com/p/108478399

SLAM中的使用

这里以VINS_MONO中的重投影误差的约束为例解释使用。
projection_factor.cpp

重投影误差误差项构建

对应C++代码

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) 函数中

    TicToc tic_toc;
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); //共同观测的frame_i位姿
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); //共同观测的frame_j位姿
    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); //相机外参Tcamera2imu
    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    double inv_dep_i = parameters[3][0]; //相机逆深度

    // 将frame_i下的3d点投影到frame_j坐标系下
    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;                 //使用逆深度将frame_i的point归一化坐标恢复到真实3D坐标
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;             //将frame_i的point转化到imu坐标系
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;                      //将frame_i的point转化到world坐标系
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);          //将frame_i的point转化到frame_j的imu坐标系下
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); //将frame_i的point转化到frame_j的camera坐标系下
    Eigen::Map residual(residuals);

#ifdef UNIT_SPHERE_ERROR
    residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
    double dep_j = pts_camera_j.z();                               //投影到frame_j坐标系下point的深度
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); //归一化坐标的重投影误差
#endif

    residual = sqrt_info * residual; // sqrt_info = 焦距/1.5*I，信息矩阵，重投影误差的可靠性相等

雅克比矩阵

本质就是对误差项的各个变量求偏导数呗
咱也不知道为什么VINS_MONO的扰动都是右乘扰动。一开始拿《视觉SLAM14讲》的公式去推导怎么都不对QAQ

frame_i的Ti2w从IMU坐标系变换到world坐标系

公式推导
- 对应C++代码

        if (jacobians[0]) //共同观测的frame_i位姿
        {
            Eigen::Map> jacobian_pose_i(jacobians[0]);

            Eigen::Matrix jaco_i;
            jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
            jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);

            jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
            jacobian_pose_i.rightCols<1>().setZero();
        }

frame_j的Ti2w从IMU坐标系变换到world坐标系

公式推导
对应C++代码

        if (jacobians[1]) //共同观测的frame_j位姿
        {
            Eigen::Map> jacobian_pose_j(jacobians[1]);

            Eigen::Matrix jaco_j;
            jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
            jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);

            jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
            jacobian_pose_j.rightCols<1>().setZero();
        }

Tc2i从camera坐标系变换到imu坐标系(相机外参)

这个太多了，不想写了，搜索参考崔华坤《VINS论文推导及代码解析》很详细了。注意扰动的二阶项近似为0就是了。

对应C++代码

        if (jacobians[2]) //相机外参Tcamera2imu
        {
            Eigen::Map> jacobian_ex_pose(jacobians[2]);
            Eigen::Matrix jaco_ex;
            jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());
            Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;
            jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) +
                                     Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));
            jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
            jacobian_ex_pose.rightCols<1>().setZero();
        }

frame_i中的逆深度inv_dep_i

公式推导
不涉及李群、李代数，即1/inv_dep_i的导数为1/(inv_dep_i*inv_dep_i)。
对应C++代码

        if (jacobians[3]) //相机逆深度
        {
            Eigen::Map jacobian_feature(jacobians[3]);
#if 1
            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
#else
            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i;
#endif
        }

李群、李代数在SLAM中的应用

C/C++/C#相关栏目本月热门文章