利用lidar_align来进行lidar和imu标定

下载并安装lidar_align

mkdir  -p  lidar_align/src
cd lidar_align/src
git clone https://github.com/ethz-asl/lidar_align.git

安装nlopt

git clone http://github.com/stevengj/nlopt
cd nlopt/
mkdir build
cd build
cmake ..
make
sudo make install

迁移NLOPTConfig.cmake

将 lidar_align/src/lidar_align/NLOPTConfig.cmake 文件移动到 lidar_align/src/ 下(与lidar_align同级)

修改loader.cpp文件

首先在该文件顶部添加 #include <sensor_msgs/Imu.h>，然后这个工具包原先是用来标定lidar和odom(里程计)，所以需要将里程计接口替换为imu接口：

将上图中标记部分的代码注释掉，在注释之后位置添加：

types.push_back(std::string("sensor_msgs/Imu"));rosbag::View view(bag, rosbag::TypeQuery(types));size_t imu_num = 0;double shiftX=0,shiftY=0,shiftZ=0,velX=0,velY=0,velZ=0;ros::Time time;double timeDiff,lastShiftX,lastShiftY,lastShiftZ;for (const rosbag::MessageInstance& m : view){std::cout <<"Loading imu: \e[1m"<< imu_num++<<"\e[0m from ros bag"<<'\r'<< std::flush;sensor_msgs::Imu imu=*(m.instantiate<sensor_msgs::Imu>());Timestamp stamp = imu.header.stamp.sec * 1000000ll +imu.header.stamp.nsec / 1000ll;if(imu_num==1){time=imu.header.stamp;Transform T(Transform::Translation(0,0,0),Transform::Rotation(1,0,0,0));odom->addTransformData(stamp, T);}else{timeDiff=(imu.header.stamp-time).toSec();time=imu.header.stamp;velX=velX+imu.linear_acceleration.x*timeDiff;velY=velX+imu.linear_acceleration.y*timeDiff;velZ=velZ+(imu.linear_acceleration.z-9.801)*timeDiff;lastShiftX=shiftX;lastShiftY=shiftY;lastShiftZ=shiftZ;shiftX=lastShiftX+velX*timeDiff+imu.linear_acceleration.x*timeDiff*timeDiff/2;shiftY=lastShiftY+velY*timeDiff+imu.linear_acceleration.y*timeDiff*timeDiff/2;shiftZ=lastShiftZ+velZ*timeDiff+(imu.linear_acceleration.z-9.801)*timeDiff*timeDiff/2;Transform T(Transform::Translation(shiftX,shiftY,shiftZ),Transform::Rotation(imu.orientation.w,imu.orientation.x,imu.orientation.y,imu.orientation.z));odom->addTransformData(stamp, T);}}

编译并运行

cd lidar_align
catkin_make
source ./devel/setup.bash
roslaunch lidar_align lidar_align.launch