激光slam第二章作业三个函数的笔记

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

里程计标定作业回顾

初学小白，主要为了理解作业中需要自己编写代码的每一句的含义，该笔记借鉴了CSDN上许多博客，就不做一一注释了，如有侵权，联系我加注释或者删除。

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、main.cpp中cal_delta_distance函数

cal_delta_distance代码如下：

Eigen::Vector3d  cal_delta_distance(Eigen::Vector3d odom_pose)
{
    static Eigen::Vector3d now_pos,last_pos;//定义当前位姿和上一次的位姿   Vector3d默认定义一个列向量（3*1）
    Eigen::Vector3d d_pos;  //return value  //返回值为当前位姿-上一次位姿
    now_pos = odom_pose;    //现在位姿由里程计获取

    //TODO:需要编写的代码区域
	d_pos = now_pos - last_pos; 
    Eigen::AngleAxisd temp(last_pos(2),Eigen::Vector3d(0,0,1)); //定义一个旋转向量（3*1）last_pos(2)为旋转角  Eigen::Vector3d(0,0,1)为旋转轴
    Eigen::Matrix3d trans=temp.matrix().inverse();//定义一个Matrix3d（三阶方阵） matrix()函数将temp（旋转向量）转化旋转矩阵 inverse()求逆
    d_pos = trans*d_pos;//直接相减求位姿差是在世界坐标系中的结果，而激光雷达的scan-matching的位姿差需要以上一时刻的位姿作为参考系，因此需要将直接相减的结果做一次旋转。

    //end of TODO:
    
    last_pos = now_pos;
    return d_pos;
}

d_pos=now_pos - last_pos； 解释：上图可看出
d_pos = trans*d_pos; 解释：直接相减求位姿差是在世界坐标系中的结果，而激光雷达的scan-matching的位姿差需要以上一时刻的位姿作为参考系，因此需要将直接相减的结果做一次旋转。使用AngleAxis，可以使用这个它乘向量实现旋转操作（因为定义了运算符重载）
Eigen::Matrix3d trans=temp.matrix().inverse()； 解释：

二、Odom_Calib.cpp中Add_Data函数

Ax=b
构建最小二乘需要的超定方程组A
A为下图中红圈中的矩阵（3*9）

/*
输入:里程计和激光数据
构建最小二乘需要的超定方程组Ax = b
*/
bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{ //Odom为里程计数据   scan为激光数据
    if(now_len<INT_MAX)
    {
		//直接线性方法  Ax=b A为里程计数据   b为激光数据
        //TODO: 构建超定方程组
        Eigen::Matrix<double,3,9> temp_A;//定义一个矩阵（3*9）
        Eigen::Vector3d temp_b;   //定义一个三维列向量（3*1）
        temp_A << Odom(0), Odom(1),Odom(2),0,0,0,0,0,0,   //Odom(0)Odom(1)Odom(2)分别是里程计的三个数据
            0,0,0,Odom(0), Odom(1),Odom(2),0,0,0,         //构成矩阵A
            0,0,0,0,0,0,Odom(0), Odom(1),Odom(2);
        temp_b << scan(0),scan(1),scan(2);  // scan为激光数据保存在temp_b矩阵中

        // 给到A和B
        A.block<3,9>(3*now_len,0) = temp_A; //
        b.block<3,1>(3*now_len,0) = temp_b; //
        //end of TODO
        now_len++;
        return true;
    }
    else
    {
        return false;
    }
}

三、Odom_Calib.cpp中Solve函数

矩阵分解

矩阵分解 (decomposition, factorization)是将矩阵拆解为数个矩阵的乘积，可分为三角分解、满秩分解、QR分解、Jordan分解和SVD（奇异值）分解等，常见的有三种：1)三角分解法 (Triangular Factorization)，2)QR 分解法 (QR Factorization)，3)奇异值分解法 (Singular Value Decompostion)。
QR分解法：是将矩阵分解成一个正规正交矩阵与上三角形矩阵,所以称为QR分解法,与此正规正交矩阵的通用符号Q有关。此处使用了colPivHouseholderQr().solve()函数
Add_Data代码如下：

/*
 * TODO:
 * 求解线性最小二乘Ax=b
 * 返回得到的矫正矩阵
*/
Eigen::Matrix3d OdomCalib::Solve()
{
    Eigen::Matrix3d correct_matrix;
    //TODO:求解线性最小二乘

	Eigen::Matrix<double,9,1> X_ = A.colPivHouseholderQr().solve(b);//进行QR分解线性方程组   colPivHouseholderQr().solve()
    correct_matrix << X_(0),X_(1),X_(2),X_(3),X_(4),X_(5),X_(6),X_(7),X_(8);//将得到的解存入correct_matrix中

    //end of TODO
    return correct_matrix;
}

总结

再次回顾了第二章作业的代码，对实现一个直接线性里程计的过程有了更深刻的认识，学习了Eigen库的一些常用类型和方法。