标定激光雷达与IMU的外参，使用lidar_align，报错记录

苏黎世联邦理工大学开源标定工具

github链接:

https://github.com/ethz-asl/lidar_align

1 安装非线性优化库nlopt

sudo apt-get install libnlopt-dev

2 编译

打开终端

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

2.1 catkin_make后报错:

CMake Error at lidar_align/CMakeLists.txt:18 (find_package):
  By not providing "FindNLOPT.cmake" in CMAKE_MODULE_PATH this project has
  asked CMake to find a package configuration file provided by "NLOPT", but
  CMake did not find one.

  Could not find a package configuration file provided by "NLOPT" with any of
  the following names:

    NLOPTConfig.cmake
    nlopt-config.cmake

  Add the installation prefix of "NLOPT" to CMAKE_PREFIX_PATH or set
  "NLOPT_DIR" to a directory containing one of the above files.  If "NLOPT"
  provides a separate development package or SDK, be sure it has been
  installed.

 解决办法:

将/calibration_ws/src/lidar_align/NLOPTConfig.cmake文件放入/calibration_ws/src/目录下.

再打开/calibration_ws/src/lidar_align/CMakeLists.txt文件,设置路径如下:

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

重新编译catkin_make即可通过.

3 运行

修改launch文件中的bag文件路径,再运行launch

注：bag与csv文件有一个即可；bag需包含点云话题和imu话题，且尽量在结构清晰无动态物体的室内录制；

source devel/setup.bash
roslaunch lidar_align lidar_align.launch

3.1 报错:No odom message found

 解决办法:

因为程序里只有odom的数据,没有加入imu的数据,需要修改loader.cpp中的Loader::loadTformFromROSBag(const std::string& bag_path, Odom* odom)函数.

注释掉原程序部分,加入imu数据部分代码,具体如下:

bool Loader::loadTformFromROSBag(const std::string& bag_path, Odom* odom) {
  rosbag::Bag bag;
  try {
    bag.open(bag_path, rosbag::bagmode::Read);
  } catch (rosbag::BagException e) {
    ROS_ERROR_STREAM("LOADING BAG FAILED: " << e.what());
    return false;
  }

  std::vector<std::string> types;

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


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

  if (odom->empty()) {
    ROS_ERROR_STREAM("No odom messages found!");
    return false;
  }

  return true;
}

3.1 再编译发现报错:sensor_msgs没有成员Imu

error: ‘Imu’ is not a member of ‘sensor_msgs’
     sensor_msgs::Imu imu=*(m.instantiate<sensor_msgs::Imu>());

解决办法:

这是因为Imu消息未在头文件定义,在loader.h中加入就好.

#include <sensor_msgs/Imu.h>

再编译即可成功.

重新运行,如下显示,则正在标定,耐心等待即可.

 

 4 查看结果

接上图,加载完毕之后有:

 results文件中会保存标定的结果.如下: