cartographer---第二篇（imu与lidar外参的自动标定）

设置lua脚本：

-- POSE_GRAPH.constraint_builder.log_matches = true
POSE_GRAPH.optimization_problem.log_solver_summary = true
POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d = true

执行launch后，结束在终端可以看到

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 7.196502e-07 <= 1.000000e-06)
[ INFO] [1624267333.284717482, 1624243912.593498410]: I0621 17:22:13.000000 30612 optimization_problem_3d.cc:578] Gravity was: 9.78914
[ INFO] [1624267333.284736916, 1624243912.593498410]: I0621 17:22:13.000000 30612 optimization_problem_3d.cc:580] IMU correction was: 0.48611 deg (0.999991, -0.00303204, 0.000971092, 0.0028034)
[ INFO] [1624267333.288219079, 1624243912.593498410]: I0621 17:22:13.000000 30612 pose_graph_3d.cc:851] Translational residuals histogram:
Count: 3114  Min: 0.00163128  Max: 0.196629  Mean: 0.0386912
[0.001631, 0.021131)                   #####    Count: 721 (23.1535%)    Total: 721 (23.1535%)
[0.021131, 0.040631)                ########    Count: 1307 (41.9717%)    Total: 2028 (65.1252%)
[0.040631, 0.060131)                    ####    Count: 627 (20.1349%)    Total: 2655 (85.2601%)
[0.060131, 0.079630)                      ##    Count: 234 (7.51445%)    Total: 2889 (92.7746%)
[0.079630, 0.099130)                       #    Count: 126 (4.04624%)    Total: 3015 (96.8208%)
[0.099130, 0.118630)                            Count: 51 (1.63776%)    Total: 3066 (98.4586%)
[0.118630, 0.138130)                            Count: 31 (0.995504%)    Total: 3097 (99.4541%)
[0.138130, 0.157630)                            Count: 11 (0.353243%)    Total: 3108 (99.8073%)
[0.157630, 0.177129)                            Count: 3 (0.0963391%)    Total: 3111 (99.9037%)
[0.177129, 0.196629]                            Count: 3 (0.0963391%)    Total: 3114 (100%)
[ INFO] [1624267333.288294483, 1624243912.593498410]: I0621 17:22:13.000000 30612 pose_graph_3d.cc:853] Rotational residuals histogram:
Count: 3114  Min: 0.000548045  Max: 0.0391761  Mean: 0.0135745
[0.000548, 0.004411)                      ##    Count: 265 (8.50996%)    Total: 265 (8.50996%)
[0.004411, 0.008274)                    ####    Count: 593 (19.043%)    Total: 858 (27.553%)
[0.008274, 0.012136)                   #####    Count: 796 (25.562%)    Total: 1654 (53.115%)
[0.012136, 0.015999)                     ###    Count: 396 (12.7168%)    Total: 2050 (65.8317%)
[0.015999, 0.019862)                     ###    Count: 417 (13.3911%)    Total: 2467 (79.2229%)
[0.019862, 0.023725)                      ##    Count: 254 (8.15671%)    Total: 2721 (87.3796%)
[0.023725, 0.027588)                       #    Count: 200 (6.42261%)    Total: 2921 (93.8022%)
[0.027588, 0.031450)                       #    Count: 114 (3.66089%)    Total: 3035 (97.4631%)
[0.031450, 0.035313)                            Count: 59 (1.89467%)    Total: 3094 (99.3577%)
[0.035313, 0.039176]                            Count: 20 (0.642261%)    Total: 3114 (100%)

 

代码：矫正imu的外参

#include<iostream>
#include<Eigen/Eigen>

int main(int argc, char** argv)
{

    Eigen::Quaterniond q_correction(0.999647,-0.0081411,-0.0055172,-0.024677);

    Eigen::Vector3d eulerAngle(3.1516,0.0,0.0);    //urdf设置rpy  初始化欧拉角(Z-Y-X，即RPY) Eigen::Vector3d eulerAngle(yaw, pitch, roll);
    Eigen::Quaterniond q_raw(
        Eigen::AngleAxisd(eulerAngle(2), Eigen::Vector3d::UnitX())
        *Eigen::AngleAxisd(eulerAngle(1), Eigen::Vector3d::UnitY())
        *Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d::UnitZ()));

    Eigen::Quaterniond q_final = q_correction*q_raw;

    Eigen::Vector3d euler = q_final.matrix().eulerAngles(2,1,0);
    //double d = 180 / 3.1415926;
    std::cout << euler << std::endl;
    return 0;
}

将矫正后的外参，更新urdf中的imu RPY。建议使用多组数据 多次矫正。

参考：

https://github.com/cartographer-project/cartographer_ros/issues/1566

https://github.com/cartographer-project/cartographer_ros/pull/1025

 https://github.com/cartographer-project/cartographer_ros/issues/1055