直接法：

1. 作业描述：

1. 本次的作业为用直接线性方法来对机器人的里程计进行校正。

2. 本次作业使用的bag数据，路径为odom_ws/bag/odom.bag（太大了，没上传到github）。

3. 本次的作业中，需要实现三个函数，分别为：

•  main.cpp，第340行中的cal_delta_distance()函数，该函数的功能为给定两个里程计位姿，计算这两个位姿之间的位姿差。

•  Odom_Calib.cpp，第23行Add_Data()函数，该函数的功能为构建超定方程组Ax=b，具体参考PPT。

•  Odom_Calib.cpp，第44行 Solve()函数，该函数的功能为对2中构建的超定方程组进行求解。

2. 本次程序的运行过程为：

1. 运行launch文件:roslaunch calib_odom odomCalib.launch。

2. 在 1 正常的情况下，运行rviz，fix_frame选择为odom_frame。在Add选项卡中增加三条Path消息。一条订阅的topic为：odom_path_pub_;一条订阅的topic为:scan_path_pub_；最后一条为:calib_path_pub_。分别选择不同的颜色。

3. 进入到odom_ws/bag目录下，运行指令：rosbag play - -clock odom.bag。(双横线中间不空格)

4. 如果一切正常，则能看到运行矫正程序的终端会打印数据，并且rviz中可以看到两条路径。当打印的数据到达一个的数量之后，则可以开始矫正。

5. 矫正的命令为，在calib_flag的topic下发布一个数据：rostopic pub /calib_flag std_msgs/Empty “{}”。

6. 程序矫正完毕会输出对应的矫正矩阵，并且会在rviz中显示出第三条路径，即calib_path。可以观察里程计路径odom_path和矫正路径_calib_path区别来判断此次矫正的效果。

3. 结果

4. 标定结果：

correct_matrix:
    0.943991     0.133506 -0.000139669
  -0.0142447      13.4325  0.000335469
 0.000152577      40.2952  0.000234346
calibration over!!!!

5. 代码：

cal_delta_distence函数

//求解得到两帧数据之间的位姿差
//即求解当前位姿　在　上一时刻　坐标系中的坐标
Eigen::Vector3d cal_delta_distence(Eigen::Vector3d odom_pose)
{

    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));
    Eigen::Matrix3d trans=temp.matrix().inverse();
    d_pos = trans*d_pos;
    last_pos = now_pos; 
//方法二矩阵的方式
//    Eigen::Matrix3d trans_temp;
//    trans_temp<< cos(last_pos(2)), -sin(last_pos(2)), 0,
//            sin(last_pos(2)), cos(last_pos(2)),  0,
//            0,                           0,                       1;
//
//    d_pos = now_pos - last_pos;
//    d_pos = trans_temp.transpose()*d_pos;
//    last_pos = now_pos;

    //end of TODO:
    return d_pos;
}

Add_Data() 函数

构建最小二乘需要的超定方程组
Ax = b

*/
bool OdomCalib::Add_Data(Eigen::Vector3d Odom, Eigen::Vector3d scan)
{

    if (now_len < INT_MAX) {
        //TODO: 构建超定方程组

        Eigen::Matrix<double,3,9> trans_a;
        Eigen::Vector3d vector__b;
        trans_a << Odom(0), Odom(1),Odom(2),0,0,0,0,0,0,
                0,0,0,Odom(0), Odom(1),Odom(2),0,0,0,
                0,0,0,0,0,0,Odom(0), Odom(1),Odom(2);
        vector__b << scan(0),scan(1),scan(2);

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

Solve函数，一般用矩阵分解的方式，可以选svd或QR分解等不同方式，结果差别不大。

Eigen::Matrix3d OdomCalib::Solve()
{
    Eigen::Matrix3d correct_matrix;

    //TODO: 求解线性最小二乘
    // Eigen::Matrix<double, 9, 1> params;
    // // params = (A.transpose() * A).inverse() * A.transpose() * b;
    // params = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    // std::cout << params.transpose() << std::endl;
    // for (size_t i = 0; i < 3; i++) {
    //     correct_matrix.block<1, 3>(i, 0) = params.block<3, 1>(i * 3, 0).transpose();
    // }
    Eigen::Matrix<double,9,1> X_ = A.colPivHouseholderQr().solve(b);
    correct_matrix << X_(0),X_(1),X_(2),X_(3),X_(4),X_(5),X_(6),X_(7),X_(8);
    //end of TODO

    return correct_matrix;
}

基于模型的方法：

模型形式：

代码：

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <fstream>
#include <iostream>
#include <math.h>
#include <string>

using namespace std;

string scan_match_file = "./scan_match.txt";
string odom_file = "./odom.txt";

int main(int argc, char** argv)
{
    // 放置激光雷达的时间和匹配值 t_s s_x s_y s_th
    vector<vector<double>> s_data;
    // 放置轮速计的时间和左右轮角速度 t_r w_L w_R
    vector<vector<double>> r_data;

    ifstream fin_s(scan_match_file);
    ifstream fin_r(odom_file);
    if (!fin_s || !fin_r) {
        cerr << "请在有scan_match.txt和odom.txt的目录下运行此程序" << endl;
        return 1;
    }

    // 读取激光雷达的匹配值
    while (!fin_s.eof()) {
        double s_t, s_x, s_y, s_th;
        fin_s >> s_t >> s_x >> s_y >> s_th;
        s_data.push_back(vector<double>({ s_t, s_x, s_y, s_th }));
    }
    fin_s.close();

    // 读取两个轮子的角速度
    while (!fin_r.eof()) {
        double t_r, w_L, w_R;
        fin_r >> t_r >> w_L >> w_R;
        r_data.push_back(vector<double>({ t_r, w_L, w_R }));
    }
    fin_r.close();

    // 第一步：计算中间变量J_21和J_22
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    // 设置数据长度
    A.conservativeResize(5000, 2);
    b.conservativeResize(5000);
    A.setZero();
    b.setZero();

    size_t id_r = 0;
    size_t id_s = 0;
    double last_rt = r_data[0][0];
    double w_Lt = 0;
    double w_Rt = 0;
    while (id_s < 5000) {
        // 激光的匹配信息
        const double& s_t = s_data[id_s][0];
        const double& s_th = s_data[id_s][3];
        // 里程计信息
        const double& r_t = r_data[id_r][0];
        const double& w_L = r_data[id_r][1];
        const double& w_R = r_data[id_r][2];
        ++id_r;
        // 在2帧激光匹配时间内进行里程计角度积分
        if (r_t < s_t) {
            double dt = r_t - last_rt;
            w_Lt += w_L * dt;
            w_Rt += w_R * dt;
            last_rt = r_t;
        } else {
            double dt = s_t - last_rt;
            w_Lt += w_L * dt;
            w_Rt += w_R * dt;
            last_rt = s_t;
            // 填充A, b矩阵
            //TODO: (3~5 lines)
            A(id_s, 0) = w_Lt;
            A(id_s, 1) = w_Rt;
            b[id_s] = s_th;
            //end of TODO
            w_Lt = 0;
            w_Rt = 0;
            ++id_s;
        }
    }
    // 进行最小二乘求解
    Eigen::Vector2d J21J22;
    //TODO: (1~2 lines)
    J21J22 = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    // J21J22 = A.colPivHouseholderQr().solve(b);
    //end of TODO
    const double& J21 = J21J22(0);
    const double& J22 = J21J22(1);
    cout << "J21: " << J21 << endl;
    cout << "J22: " << J22 << endl;

    // 第二步，求解轮间距b
    Eigen::VectorXd C;
    Eigen::VectorXd S;
    // 设置数据长度
    C.conservativeResize(10000);
    S.conservativeResize(10000);
    C.setZero();
    S.setZero();

    id_r = 0;
    id_s = 0;
    last_rt = r_data[0][0];
    double th = 0;
    double cx = 0;
    double cy = 0;
    while (id_s < 5000) {
        // 激光的匹配信息
        const double& s_t = s_data[id_s][0];
        const double& s_x = s_data[id_s][1];
        const double& s_y = s_data[id_s][2];
        // 里程计信息
        const double& r_t = r_data[id_r][0];
        const double& w_L = r_data[id_r][1];
        const double& w_R = r_data[id_r][2];
        ++id_r;
        // 在2帧激光匹配时间内进行里程计位置积分
        if (r_t < s_t) {
            double dt = r_t - last_rt;
            cx += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * cos(th);
            cy += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * sin(th);
            th += (J21 * w_L + J22 * w_R) * dt;
            last_rt = r_t;
        } else {
            double dt = s_t - last_rt;
            cx += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * cos(th);
            cy += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * sin(th);
            th += (J21 * w_L + J22 * w_R) * dt;
            last_rt = s_t;
            // 填充C, S矩阵
            //TODO: (4~5 lines)
            C[id_s * 2] = cx;
            C[id_s * 2 + 1] = cy;
            S[id_s * 2] = s_x;
            S[id_s * 2 + 1] = s_y;
            //end of TODO
            cx = 0;
            cy = 0;
            th = 0;
            ++id_s;
        }
    }
    // 进行最小二乘求解，计算b, r_L, r_R
    double b_wheel;
    double r_L;
    double r_R;
    //TODO: (3~5 lines)
    b_wheel = C.colPivHouseholderQr().solve(S)[0];
    r_L = -J21 * b_wheel;
    r_R = J22 * b_wheel;
    //end of TODO
    cout << "b: " << b_wheel << endl;
    cout << "r_L: " << r_L << endl;
    cout << "r_R: " << r_R << endl;

    cout << "参考答案：轮间距b为0.6m左右，两轮半径为0.1m左右" << endl;

    return 0;
}

运行结果

J21: -0.163886
J22: 0.170575
b: 0.59796
r_L: 0.0979974
r_R: 0.101997
参考答案：轮间距b为0.6m左右，两轮半径为0.1m左右

cmakelist 和 odom.txt 和scan_match.txt 后续整理上传