Ubuntu18.04下 用lidar_align实现激光雷达VLP-16和Imu联合标定（未完成）

注意：制作某个算法所需 rosbag 前，先在已提供包下查看你所需数据类型等信息

rosbag info ***.bag

一、实现所用设备

系统环境：Ubuntu 18.04
激光雷达：Velodyne VLP-16
Imu : 3DM-GX4-25

二、安装依赖、配环境

2.1 依赖

sudo apt-get install libnlopt-dev

2.2 下载 Lidar + IMU 标定工具：lidar_align ，编译

链接有问题的请点击这里进行手动下载

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

2.3 出现问题如下

解决：将 lidar_align 文件夹下的 NLOPTConfig.cmake 复制到 ROS工作空间lidar_align_ws/src路径下面，再次编译catkin_make即可

2.3 改写IMU

1.需将里程计接口改写为 IMU 接口，未必能精确标定激光雷达和imu

2.在如上图文件中找到此块 odom 代码内容前后加/* */注释掉或者直接删除

/*types.push_back(std::string("geometry_msgs/TransformStamped"));
  rosbag::View view(bag, rosbag::TypeQuery(types));

  size_t tform_num = 0;
  for (const rosbag::MessageInstance& m : view) {
    std::cout << " Loading transform: \e[1m" << tform_num++
              << "\e[0m from ros bag" << '\r' << std::flush;

    geometry_msgs::TransformStamped transform_msg =
        *(m.instantiate<geometry_msgs::TransformStamped>());

    Timestamp stamp = transform_msg.header.stamp.sec * 1000000ll +
                      transform_msg.header.stamp.nsec / 1000ll;

    Transform T(Transform::Translation(transform_msg.transform.translation.x,
                                       transform_msg.transform.translation.y,
                                       transform_msg.transform.translation.z),
                Transform::Rotation(transform_msg.transform.rotation.w,
                                    transform_msg.transform.rotation.x,
                                    transform_msg.transform.rotation.y,
                                    transform_msg.transform.rotation.z));
    odom->addTransformData(stamp, T);
  }
  */

3.将上部分替换为以下 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);
	 }
	}

4.文件开头添加头文件

#include <sensor_msgs/Imu.h>

5.点击保存，关闭，再次编译catkin_make

三、录制数据