Ubuntu20.04运行lidar_align标定激光雷达和IMU

---

Windows 10(64bits) + VMware 16 Pro + Ubuntu 20.04 + noetic

---

lidar_align基本原理：
1、读取rosbag的数据，通过时间戳匹配LiDAR每帧里面的每个点和IMU的坐标变换
2、通过上面的变换矩阵将利用位姿信息将LiDAR的每帧拼接成点云，迭代优化坐标变换使得距离最小。

一、源码及数据集下载

lidar_align源码：https://github.com/ethz-asl/lidar_align
nlopt 源码：github.com/stevengj/nlopt
Walking dataset：https://pan.baidu.com/s/1zbsYjkrhRaQ-feXRfuP9Yg

			提取码：zsk7

Park dataset：https://pan.baidu.com/s/1umjQ27ekUcSDTvK15Rhaeg

			提取码：bswp

二、安装依赖

主要是nlopt包，仅使用以下命令安装可能会报错。

sudo apt-get install libnlopt-dev

根据上面的链接下载nlopt源码进行安装。

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

三、处理数据集

原始数据集话题内容较多，而且非常大，全部进行计算会导致系统内存不足而停止运算。
针对Park dataset数据集：
从park_dataset.bag中提取激光雷达点云和IMU数据，注意激光雷达点云格式需要是sensor_msgs/PointCloud2

rosbag filter park_dataset.bag cal.bag "topic == '/points_raw' or topic =='/imu_raw'"

处于初步调试学习使用这个工具包的阶段，暂时要求能出结果就行。使用rosbag命令提取一个时间段内的数据进行计算就行。

rosbag filter cal.bag caltime.bag "t.to_sec() >1593996300.00 and t.to_sec() <= 1593996350.00"

四、修改源码

4.1 改写 loader.cpp

这个工具包原本是用来标定激光雷达和里程计的，所以需要改写IMU接口以替换里程计接口。以下代码可以直接复制粘贴使用。

#include 
#include 
#include 
#include 

#include "lidar_align/loader.h"
#include "lidar_align/transform.h"

#include 

namespace lidar_align {

Loader::Loader(const Config& config) : config_(config) {}

Loader::Config Loader::getConfig(ros::NodeHandle* nh) {
  Loader::Config config;
  nh->param("use_n_scans", config.use_n_scans, config.use_n_scans);
  return config;
}

void Loader::parsePointcloudMsg(const sensor_msgs::PointCloud2 msg,
                                LoaderPointcloud* pointcloud) {
  bool has_timing = false;
  bool has_intensity = false;
  for (const sensor_msgs::PointField& field : msg.fields) {
    if (field.name == "time_offset_us") {
      has_timing = true;
    } else if (field.name == "intensity") {
      has_intensity = true;
    }
  }

  if (has_timing) {
    pcl::fromROSMsg(msg, *pointcloud);
    return;
  } else if (has_intensity) {
    Pointcloud raw_pointcloud;
    pcl::fromROSMsg(msg, raw_pointcloud);

    for (const Point& raw_point : raw_pointcloud) {
      PointAllFields point;
      point.x = raw_point.x;
      point.y = raw_point.y;
      point.z = raw_point.z;
      point.intensity = raw_point.intensity;

      if (!std::isfinite(point.x) || !std::isfinite(point.y) ||
          !std::isfinite(point.z) || !std::isfinite(point.intensity)) {
        continue;
      }

      pointcloud->push_back(point);
    }
    pointcloud->header = raw_pointcloud.header;
  } else {
    pcl::PointCloud<pcl::PointXYZ> raw_pointcloud;
    pcl::fromROSMsg(msg, raw_pointcloud);

    for (const pcl::PointXYZ& raw_point : raw_pointcloud) {
      PointAllFields point;
      point.x = raw_point.x;
      point.y = raw_point.y;
      point.z = raw_point.z;

      if (!std::isfinite(point.x) || !std::isfinite(point.y) ||
          !std::isfinite(point.z)) {
        continue;
      }

      pointcloud->push_back(point);
    }
    pointcloud->header = raw_pointcloud.header;
  }
}

bool Loader::loadPointcloudFromROSBag(const std::string& bag_path,
                                      const Scan::Config& scan_config,
                                      Lidar* lidar) {
  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;
  types.push_back(std::string("sensor_msgs/PointCloud2"));
  rosbag::View view(bag, rosbag::TypeQuery(types));

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

    LoaderPointcloud pointcloud;
    parsePointcloudMsg(*(m.instantiate<sensor_msgs::PointCloud2>()),
                       &pointcloud);

    lidar->addPointcloud(pointcloud, scan_config);

    if (lidar->getNumberOfScans() >= config_.use_n_scans) {
      break;
    }
  }
  if (lidar->getTotalPoints() == 0) {
    ROS_ERROR_STREAM(
        "No points were loaded, verify that the bag contains populated "
        "messages of type sensor_msgs/PointCloud2");
    return false;
  }

  return true;
}

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

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

  return true;
}

bool Loader::loadTformFromMaplabCSV(const std::string& csv_path, Odom* odom) {
  std::ifstream file(csv_path, std::ifstream::in);

  size_t tform_num = 0;
  while (file.peek() != EOF) {
    std::cout << " Loading transform: \e[1m" << tform_num++
              << "\e[0m from csv file" << '\r' << std::flush;

    Timestamp stamp;
    Transform T;

    if (getNextCSVTransform(file, &stamp, &T)) {
      odom->addTransformData(stamp, T);
    }
  }

  return true;
}

// lots of potential failure cases not checked
bool Loader::getNextCSVTransform(std::istream& str, Timestamp* stamp,
                                 Transform* T) {
  std::string line;
  std::getline(str, line);

  // ignore comment lines
  if (line[0] == '#') {
    return false;
  }

  std::stringstream line_stream(line);
  std::string cell;

  std::vector<std::string> data;
  while (std::getline(line_stream, cell, ',')) {
    data.push_back(cell);
  }

  if (data.size() < 9) {
    return false;
  }

  constexpr size_t TIME = 0;
  constexpr size_t X = 2;
  constexpr size_t Y = 3;
  constexpr size_t Z = 4;
  constexpr size_t RW = 5;
  constexpr size_t RX = 6;
  constexpr size_t RY = 7;
  constexpr size_t RZ = 8;

  *stamp = std::stoll(data[TIME]) / 1000ll;
  *T = Transform(Transform::Translation(std::stod(data[X]), std::stod(data[Y]),
                                        std::stod(data[Z])),
                 Transform::Rotation(std::stod(data[RW]), std::stod(data[RX]),
                                     std::stod(data[RY]), std::stod(data[RZ])));

  return true;
}

}  // namespace lidar_align

4.2 更改 NLOPTConfig.cmake文件位置

将 lidar_align 源码包里的 NLOPTConfig.cmake 复制到 ROS工作空间lidar_align_ws/src 路径下面。

4.3 修改源码CMakeLists.txt文件

使用管理员权限用gedit编辑器打开CMakeLists.txt文件，添加以下代码。

set (CMAKE_PREFIX_PATH "/usr/local/lib/cmake/nlopt")
set(CMAKE_CXX_STANDARD 14)

4.4 修改launch文件

修改launch文件中的包路径就可以。

  <arg name="bag_file" default="/mnt/hgfs/H/caltime.bag"/>

五、运行代码

启动代码进行标定。

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

需要进行一段时间的等待。

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

5.1 查看标定结果

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

Active Transformation Vector (x,y,z,rx,ry,rz) from the Pose Sensor Frame to  the Lidar Frame:
[0.00354558, 0.00671555, -0.0260116, 1.15427, -1.11999, -1.50597]

Active Transformation Matrix from the Pose Sensor Frame to  the Lidar Frame:
 -0.154495   0.127539  -0.979727 0.00354558
 -0.974987  -0.180053   0.130309 0.00671555
 -0.159784   0.975354   0.152166 -0.0260116
         0          0          0          1

Active Translation Vector (x,y,z) from the Pose Sensor Frame to  the Lidar Frame:
[0.00354558, 0.00671555, -0.0260116]

Active Hamiltonen Quaternion (w,x,y,z) the Pose Sensor Frame to  the Lidar Frame:
[-0.452111, -0.467277, 0.453397, 0.609654]

Time offset that must be added to lidar timestamps in seconds:
0.045003

ROS Static TF Publisher: <node pkg="tf" type="static_transform_publisher" name="pose_lidar_broadcaster" args="0.00354558 0.00671555 -0.0260116 -0.467277 0.453397 0.609654 -0.452111 POSE_FRAME LIDAR_FRAME 100" />

5.2 查看.ply文件

5.2.1 用cloudcompare打开

会报错，不太懂，都点ok就行。

5.2.2 用Ubuntu的pcl viewer打开

第一次用需要安装pcl工具，输入以下指令报错时，按照提示安装即可。

pcl_ply2pcd caltime.ply 1.pcd
pcl_viewer 1.pcd 

5.2.3 转换成pcd格式后再用cloudcompare打开

转换成pcd格式后再用cloudcompare打开时，没有报错。感觉cloudcompare查看更加方便。

六、遇到的问题及解决方法

6.1 缺少头文件#include

报错内容

error: ‘Imu’ is not a member of ‘sensor_msgs’
     sensor_msgs::Imu imu=*(m.instantiate<sensor_msgs::Imu>());
                  ^~~
/home/ubuntu/Sensors_Calibration/lidar_align_ws/src/lidar_align/src/loader.cpp:134:23: error: ‘imu’ was not declared in this scope
     Timestamp stamp = imu.header.stamp.sec * 1000000ll +imu.header.stamp.nsec / 1000ll;
                       ^~~
/home/ubuntu/Sensors_Calibration/lidar_align_ws/src/lidar_align/src/loader.cpp:134:23: note: suggested alternative: ‘time’
     Timestamp stamp = imu.header.stamp.sec * 1000000ll +imu.header.stamp.nsec / 1000ll;
                       ^~~
                       time
lidar_align/CMakeFiles/lidar_align.dir/build.make:110: recipe for target 'lidar_align/CMakeFiles/lidar_align.dir/src/loader.cpp.o' failed
make[2]: *** [lidar_align/CMakeFiles/lidar_align.dir/src/loader.cpp.o] Error 1
CMakeFiles/Makefile2:510: recipe for target 'lidar_align/CMakeFiles/lidar_align.dir/all' failed
make[1]: *** [lidar_align/CMakeFiles/lidar_align.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8 -l8" failed

解决方法

见4.1，改写loader文件，添加头文件，

6.2 找不到nlopt编译文件

报错内容：

Could not find a package configuration file provided by "NLOPT"with any of the following names:
NLOPTConfig.cmake
nlopt-config.cmake

解决方法

见4.2，更改 NLOPTConfig.cmake文件位置。

6.3 pcl库报错

报错内容

包含以下内容的一大堆错误。这个报错很常见。

/usr/include/pcl-1.10/pcl/point_types.h:574:1: error: ‘plus’ is not a member of ‘pcl::traits’
  574 | POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::CPPFSignature,

/usr/include/pcl-1.10/pcl/point_types.h:588:1: error: ‘plusscalar’ is not a member of ‘pcl::traits’
  588 | POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PPFRGBSignature,

/usr/include/pcl-1.10/pcl/point_types.h:588:1: error: ‘type’ is not a member of ‘pcl::traits::datatype<pcl::PPFRGBSignature, pcl::fields::f4>::decomposed’ {aka ‘pcl::traits::decomposeArray<float>’}

/usr/include/pcl-1.10/pcl/point_types.h:588:1: error: ‘plusscalar’ is not a member of ‘pcl::traits’
  588 | POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PPFRGBSignature,

解决方法

见4.3，修改源码CMakeLists.txt文件，添加set(CMAKE_CXX_STANDARD 14)。

6.4 系统运存不足

报错内容

执行到“Performing Global Optimization”步骤终止。

解决方法

方法1：见三，处理数据集。
方法2：将sensors.h中keep_points_ratio的默认值0.01改成0.005，如果还报错继续往小改。

七、待解决问题

1、标定结果的使用
2、标定结果的精度
3、标定结果的ply文件不明白，感觉不对
4、LIO-SAM跑自己的数据集
5、清除Ubuntu占用内存