mkdir -p ~/calib_odom_ws/src
cd ~/calib_odom_ws/src
git clone https://github.com/KOTOKORURU/odometry_calibration
cd ..
catkin_make
错误1：
odom_calib/src/odometry_calibration/src/main.cpp:5:40: fatal error: nav_core/recovery_behavior.h: 没有那个文件或目录
解决办法：打开odom_calib/src/odometry_calibration/src/main.cpp，注释#nav_core/recovery_behavior.h。
错误2：
odom_calib/src/odometry_calibration/src/main.cpp:28:25: fatal error: csm/csm_all.h: 没有那个文件或目录
解决办法：安装ros-kinetic-csm。
sudo apt install ros-kinetic-csm
cd ~/calib_odom_ws/
source /devel/setup.bash
roslaunch calib_odom odomCalib.launch
注意：保证没有任何ROS节点在运行，roscore也要关闭。
1.打开rviz
rosrun rviz rviz
2.配置rviz
fix_frame选择为odom。在Add选项卡中增加三条Path消息，订阅的topic分别为：
分别选择不同的颜色，用来区分路径。
可以将此rviz的配置保存为odom_calib.rviz文件，然后在odomCalib.launch文件中添加：
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find calib_odom)/rviz_cfg/odom_calib.rviz" />
这样，在启动launch文件时，就自动打开了配置好的rviz。
首先，解压odom.bag.tar.gz：
cd ~/calib_odom_ws/src/odometry_calibration/bag
tar zxvf odom.bag.tar.gz
然后，播放bag：
rosbag play –clock odom.bag
如果一切正常，则能看到运行矫正程序的终端会打印数据，并且rviz中可以看到两条路径。
当打印的数据到达一定的数量后，则可以开始矫正。这里，需要给calib_flag话题发布一个数据。
rostopic pub /calib_flag std_msgs/Empty "{}"
程序矫正完毕会输出对应的矫正矩阵，并且会在rviz中显示出第三条路径，即calib_path。可以观察里程计路径odom_path和矫正路径calib_path区别来判断此次矫正的效果。
如下图所示，激光雷达的pose好像有点问题。。
/*
* 输入:里程计和激光数据
* 构建最小二乘需要的超定方程组
* Ax = b
*/
typedef Eigen::Matrix<double, 1, 9> Vector9d_T;
typedef Eigen::Matrix<double, 9, 1> Vector9d;
bool OdomCalib::Add_Data(Eigen::Vector3d Odom, Eigen::Vector3d scan)
{
if(now_len < INT_MAX)
{
Eigen::Matrix<double, 3, 9> A_tmp;
Eigen::Matrix<double, 3, 1> b_tmp;
A_tmp.setZero();
b_tmp.setZero();
b_tmp = scan;
for(int i = 0; i < 3; i++){
Vector9d_T tmp;
tmp.setZero();
tmp.block<1, 3>(0, i * 3) = Odom.transpose();
A_tmp.block<1, 9>(i, 0) = tmp;
}
A.block<3, 9>(now_len * 3, 0) = A_tmp;
b.block<3, 1>(now_len * 3, 0) = b_tmp;
now_len++;
return true;
}
else
return false;
}
/*
* 输入:里程计和激光数据
* 构建最小二乘需要的超定方程组
* Ax = b
*/
typedef Eigen::Matrix<double, 1, 9> Vector9d_T;
typedef Eigen::Matrix<double, 9, 1> Vector9d;
bool OdomCalib::Add_Data(Eigen::Vector3d Odom, Eigen::Vector3d scan)
{
if(now_len < INT_MAX)
{
Eigen::Matrix<double, 3, 9> A_tmp;
Eigen::Matrix<double, 3, 1> b_tmp;
A_tmp.setZero(); b_tmp.setZero();
b_tmp = scan;
for(int i = 0; i < 3; i++)
{
Vector9d_T tmp;
tmp.setZero();
tmp.block<1, 3>(0, i * 3) = Odom.transpose();
A_tmp.block<1, 9>(i, 0) = tmp; }
A.block<3, 9>(now_len * 3, 0) = A_tmp;
b.block<3, 1>(now_len * 3, 0) = b_tmp;
now_len++;
return true;
}
else return false;
}
/*
* 求解线性最小二乘Ax=b(A^TAx^* = A^Tb)
* 返回得到的矫正矩阵
*/
Eigen::Matrix3d OdomCalib::Solve()
{
Eigen::Matrix3d correct_matrix;
Vector9d x;
Eigen::MatrixXd A_tmp;
Eigen::VectorXd b_tmp;
A_tmp = A.transpose() * A;
b_tmp = A.transpose() * b;
x = A_tmp.ldlt().solve(b_tmp); // Cholesky decomposition
correct_matrix << x(0), x(1), x(2),
x(3), x(4), x(5),
x(6), x(7), x(8);
return correct_matrix;
}
Eigen::Vector3d cal_delta_distance(Eigen::Vector3d odom_pose)
{
static Eigen::Vector3d now_pos, last_pos;
Eigen::Vector3d d_pos; // return value
now_pos = odom_pose;
Eigen::Matrix3d R_last;
R_last << cos(now_pos(2)), -sin(now_pos(2)), 0,
sin(now_pos(2)), cos(now_pos(2)), 0,
0, 0, 1;
d_pos = R_last.transpose() * (now_pos - last_pos);
// d_pos = R_last.inverse() * (now_pos - last_pos); // 根据航迹推算公式，应该是逆啊
last_pos = now_pos;
return d_pos;
}
输出bag文件的信息：
其中，/odom话题为里程计的pose，/sick_scan为2D激光雷达的数据，同时还发布了TF。激光雷达的pose是通过PI-ICP算法计算的，而里程计是直接从bag发布的。
获取里程计pose：
bool Scan2::getOdomPose(Eigen::Vector3d& pose, const ros::Time& t)
{
// Get the robot's pose
tf::Stamped<tf::Pose> ident(tf::Transform(tf::createQuaternionFromRPY(0,0,0),
tf::Vector3(0,0,0)), t, base_frame_);
tf::Stamped<tf::Transform> odom_pose;
try
{
tf_.transformPose(odom_frame_, ident, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
double yaw = tf::getYaw(odom_pose.getRotation());
pose << odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(),
yaw;
//pub_msg(pose, path_odom, odom_path_pub_);
return true;
}
bool Scan2::getOdomPose(Eigen::Vector3d& pose, const ros::Time& t)
{
// Get the robot's pose
tf::Stamped<tf::Pose> ident(tf::Transform(tf::createQuaternionFromRPY(0,0,0),
tf::Vector3(0,0,0)), t, base_frame_);
tf::Stamped<tf::Transform> odom_pose;
try
{
tf_.transformPose(odom_frame_, ident, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
double yaw = tf::getYaw(odom_pose.getRotation());
pose << odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(),
yaw;
//pub_msg(pose, path_odom, odom_path_pub_);
return true;
}
理论上，相邻的两个时刻，激光雷达和里程计的位姿变换是相同的，而实际的估计值和真实值呈线性关系，因此此可以构建基于直接线性方法的最小二乘问题。
目前此项目，只是演示如何使用激光雷达纠正里程计的误差，属于后处理，并不适用于实际应用。
我觉得，真正意义上得里程计标定可以参考：https://github.com/MegviiRobot/OdomLaserCalibraTool 。