Ubuntu20.04安装和编译运行lidar_align来联合标定lidar与imu的外参

---

一、编译运行lidar_align

这个标定文件得出的是：IMU到Lidar的外参

1.1 下载地址

github链接：链接: https://github.com/ethz-asl/lidar_align

1.2 编译

将源码放在src下，进行编译：

catkin_make

1.2.1 nlopt问题解决

出现如下问题：

解决办法：安装nlopt，但最新的(2.6.2) libnlopt-dev包在Ubuntu20中被破坏了，由于某种原因它被编译成了一个静态库(.so共享对象缺失)。快速的解决方法是从Github上下载NLOPT并自己编译，连接NLopt文档。

下载好NLopt，进行编译安装，流程如下：

mkdir build
cd build
cmake ..
make
sudo make install

那么在 /usr/local/lib/cmake 目录下出现 nlopt 文件。
然后，在lidar_align工程目录下，并在CMakeLists.txt里加上这样一句话：

list(APPEND CMAKE_FIND_ROOT_PATH ${CMAKE_SOURCE_DIR})
set (CMAKE_PREFIX_PATH "/usr/local/lib/cmake/nlopt")

以上为第一步操作，如果问题尚未解决，第二步操作如下：
将**/home/cupido/lidar-align/src/lidar_align/NLOPTConfig.cmake文件移动到/home/cupido/lidar-align/src**下(与lidar_align同级，注意自己的路径)。

1.2.2 c++问题解决

Ubuntu20.04用的应该是c++14，注意CMakeLists.txt的修改，如下：

或者set(CMAKE_CXX_STANDARD 14)

【注】：至此，编译应该没有问题了。

二、处理数据集

如果原始数据集话题内容较多，而且非常大，全部进行计算会导致系统内存不足而停止运算，报错如下：

针对park_dataset数据集，处理步骤如下：

//从park_dataset.bag中提取激光雷达点云和IMU数据，注意激光雷达点云数据格式：sensor_msgs/PointCloud2、IMU数据格式：sensor_msgs/Imu
rosbag filter park_dataset.bag cal.bag "topic == '/points_raw' or topic =='/imu_raw'"  // 生成cal.bag
//处于初步调试学习使用这个工具包的阶段，暂时要求能出结果就行。使用rosbag命令提取一个时间段内的数据进行计算就行。
rosbag filter cal.bag caltime.bag "t.to_sec() >1593996300.00 and t.to_sec() <= 1593996350.00" // 生成caltime.bag

处理后的caltime.bag包的数据量将会很小

参考博客：rosbag 时间和topic过滤

三、修改源码

3.1 修改loader.cpp

这个工具包原本是用来标定激光雷达和里程计的，所以需要改写IMU接口以替换里程计接口，改写部分，如下：

  // 找到odom部分注释掉，如下：
  std::vector<std::string> types;
  // 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());

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

  //在注释的位置粘贴如下代码
  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);
    }
  }

3.2 修改lidar_align.launch

主要是你数据集所在的路径：

四、运行

启动进行标定：

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

终端输出效果，如下：

计算完成后，在程序包的results文件夹里会出现一个.txt文件和一个.ply文件。

4.1 .txt查看

.txt文件查看标定结果，主要是变化矩阵，变化向量和四元数。

4.2 .ply查看

用pcl viewer打开，执行如下指令：

pcl_ply2pcd xxxxx.ply view.pcd
pcl_viewer view.pcd 

五、测试lidar_align的精度

这里展示拿lidar_align测出来的kitti的外参与kitti官方给的外参值对照图，如下：

由图片可以看出来，标得不是很准，考虑可能是准备的数据集有问题。

lidar_align测出来的kitti的外参即为：IMU到Lidar的外参值

六、参考博客

1、使用lidar_align联合标定lidar与imu的外参
2、激光雷达和IMU联合标定并运行LIOSAM