direct method ：

1. Job description ：

1. This job is to use the direct linear method to correct the odometer of the robot .

2. The... Used in this operation bag data , Path is odom_ws/bag/odom.bag（ It's too big , Not uploaded to github）.

3. In this assignment , You need to implement three functions , Respectively ：

•  main.cpp, The first 340 In a row cal_delta_distance() function , The function of this function is to give the position and attitude of two odometers , Calculate the pose difference between the two poses .

•  Odom_Calib.cpp, The first 23 That's ok Add_Data() function , The function of this function is to construct overdetermined equations Ax=b, Specific reference PPT.

•  Odom_Calib.cpp, The first 44 That's ok Solve() function , The function of this function is to 2 The overdetermined equations constructed in are solved .

2. The running process of this program is ：

1. function launch file :roslaunch calib_odom odomCalib.launch.

2. stay 1 Under normal circumstances , function rviz,fix_frame Choose as odom_frame. stay Add Add three items to the tab Path news . A subscription topic by ：odom_path_pub_; A subscription topic by :scan_path_pub_; The last one is :calib_path_pub_. Choose different colors .

3. Enter into odom_ws/bag Under the table of contents , Operation instruction ：rosbag play - -clock odom.bag.( There is no space in the middle of the double horizontal line )

4. If everything goes well , You can see that the terminal running the correction program will print data , also rviz You can see two paths . When the printed data reaches a certain amount , Then you can start to correct .

5. The order of correction is , stay calib_flag Of topic Next data release ：rostopic pub /calib_flag std_msgs/Empty “{}”.

6. After the program is corrected, the corresponding correction matrix will be output , And will be in rviz The third path is shown in , namely calib_path. You can observe the odometer path odom_path And correction path _calib_path To judge the effect of this correction .

3. result

4. Calibration results ：

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

5. Code ：

cal_delta_distence function

// Solve the pose difference between the two frames of data 
// That is to solve the current pose 　 stay 　 Last time 　 Coordinates in a coordinate system 
Eigen::Vector3d cal_delta_distence(Eigen::Vector3d odom_pose)
{

    Eigen::Vector3d d_pos; //return value
    now_pos = odom_pose;
    //TODO:
    // The way the vector rotates , Axis angle 
    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; 
// Matrix method II 
//    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() function

 The overdetermined equations needed to construct the least squares 
Ax = b

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

    if (now_len < INT_MAX) {
        //TODO:  Construct overdetermined equations 

        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);

        //  Give to the A and 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 function , Generally, matrix decomposition is used , You can choose svd or QR Decomposition and other different ways , The difference was not significant .

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

    //TODO:  Solve linear least squares 
    // 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;
}

A model-based approach ：

Model form ：

Code ：

#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)
{
    //  Time and matching value of placing lidar  t_s s_x s_y s_th
    vector<vector<double>> s_data;
    //  The time of placing the wheel speedometer and the angular speed of the left and right wheels  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 << " Please be there scan_match.txt and odom.txt Run this program in the directory of " << endl;
        return 1;
    }

    //  Read the matching value of lidar 
    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();

    //  Read the angular velocity of the two wheels 
    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();

    //  First step ： Calculate the intermediate variable J_21 and J_22
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    //  Set data length 
    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) {
        //  Laser matching information 
        const double& s_t = s_data[id_s][0];
        const double& s_th = s_data[id_s][3];
        //  Odometer information 
        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;
        //  stay 2 Carry out odometer angle integration within the frame laser matching time 
        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;
            //  fill A, b matrix 
            //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;
        }
    }
    //  Carry out the least square solution 
    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;

    //  The second step , Solving wheel spacing b
    Eigen::VectorXd C;
    Eigen::VectorXd S;
    //  Set data length 
    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) {
        //  Laser matching information 
        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];
        //  Odometer information 
        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;
        //  stay 2 Odometer position integration is performed within the frame laser matching time 
        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;
            //  fill C, S matrix 
            //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;
        }
    }
    //  Carry out the least square solution , Calculation 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 << " Refer to the answer ： Wheel spacing b by 0.6m about , The radius of the two wheels is 0.1m about " << endl;

    return 0;
}

Running results

J21: -0.163886
J22: 0.170575
b: 0.59796
r_L: 0.0979974
r_R: 0.101997
 Refer to the answer ： Wheel spacing b by 0.6m about , The radius of the two wheels is 0.1m about 

cmakelist and odom.txt and scan_match.txt Follow up sorting and uploading