【MSFL】2：LIO-SAM：从安装到测试

1、标定IMU+LiDAR

1、录制ROS包

• 录制rosbag，仅需包含imu和lidar数据，具体/topic需要自行修改：

rosbag record /imu /velodyne_points -O data/ROSBAG/lidar_imu.bag

• 保存的效果如下：

zhao@zhao:~/WS/msfl_ws$ rosbag info data/ROSBAG/lidar_imu.bag 
path:        data/ROSBAG/lidar_imu.bag
version:     2.0
duration:    3:17s (197s)
start:       Jan 01 1970 08:00:03.46 (3.46)
end:         Jan 01 1970 08:03:20.57 (200.57)
size:        384.0 MB
messages:    21684
compression: none [475/475 chunks]
types:       sensor_msgs/Imu         [6a62c6daae103f4ff57a132d6f95cec2]
             sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
topics:      /imu               19713 msgs    : sensor_msgs/Imu        
             /velodyne_points    1971 msgs    : sensor_msgs/PointCloud2

2、安装依赖项

• nlop包安装：

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

这里可能会遇到在cmake ..的时候有fail现象，提示一些包未找到，为了保证后续运行顺利，我这边还是安装了：

sudo apt-get install guile-3.0 #这里我选择的是3.0版本
sudo apt-get install guile-3.0-dev
sudo apt-get install octave
sudo apt-get install liboctave-dev
sudo apt-get install swig

3、修改源码

• 修改 loader.cpp

#include <geometry_msgs/TransformStamped.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Imu.h>

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

#include <sensor_msgs/Imu.h>

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

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

• 修改NLOPTConfig.cmake文件位置

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

• 修改源码CMakeLists.txt文件，添加以下代码：

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

• 修改launch文件中的包路径

<arg name="bag_file" default="/home/zhao/WS/msfl_ws/data/ROSBAG/lidar_imu.bag"/>

4、运行代码

启动代码进行标定

catkin_make #此处建议先删除之前的build/ devel/
source devel/setup.bash
roslaunch lidar_align lidar_align.launch

运行结果如下

zhao@zhao:~/WS/msfl_ws$ roslaunch lidar_align lidar_align.launch 
... logging to /home/zhao/.ros/log/24f28d9c-c332-11ef-a81b-25d9e051409d/roslaunch-zhao-83450.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://zhao:41241/

SUMMARY
========

PARAMETERS
 * /lidar_align/input_bag_path: /home/zhao/WS/msf...
 * /lidar_align/input_csv_path: PATH/TO/YOUR.csv
 * /lidar_align/output_calibration_path: /home/zhao/WS/msf...
 * /lidar_align/output_pointcloud_path: /home/zhao/WS/msf...
 * /lidar_align/transforms_from_csv: False
 * /rosdistro: noetic
 * /rosversion: 1.17.0

NODES
  /
    lidar_align (lidar_align/lidar_align_node)

ROS_MASTER_URI=http://localhost:11311

process[lidar_align-1]: started with pid [83464]
[ INFO] [1735198105.395046732]: Loading Pointcloud Data...
[ INFO] [1735198107.504482765]: Loading Transformation Data...                                
[ INFO] [1735198107.640942368]: Interpolating Transformation Data...                          
[ INFO] [1735198109.247437244]: Performing Global Optimization...                             
[ INFO] [1735198300.233199926]: Performing Local Optimization...                                
[ INFO] [1735198309.992561542]: Saving Aligned Pointcloud...                                     eration: 204
[ INFO] [1735198310.543539562]: Saving Calibration File...                                
[ INFO] [1735198310.543604514]: Final Calibration:                                
Active Transformation Vector (x,y,z,rx,ry,rz) from the Pose Sensor Frame to  the Lidar Frame:
[0, 0, 0, -1.39626, 0.465421, 0]

Active Transformation Matrix from the Pose Sensor Frame to  the Lidar Frame:
 0.909884 -0.270347  0.314679         0
-0.270347  0.188959  0.944038         0
-0.314679 -0.944038 0.0988438         0
        0         0         0         1

Active Translation Vector (x,y,z) from the Pose Sensor Frame to  the Lidar Frame:
[0, 0, 0]

Active Hamiltonen Quaternion (w,x,y,z) the Pose Sensor Frame to  the Lidar Frame:
[0.74123, -0.636805, 0.212268, 0]

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

ROS Static TF Publisher: <node pkg="tf" type="static_transform_publisher" name="pose_lidar_broadcaster" args="0 0 0 -0.636805 0.212268 0 0.74123 POSE_FRAME LIDAR_FRAME 100" />
[lidar_align-1] process has finished cleanly
log file: /home/zhao/.ros/log/24f28d9c-c332-11ef-a81b-25d9e051409d/lidar_align-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

2、对比调试数据发布信息

1、检查rosbag中的topic

检查LIO-SAM开源数据包与个人实际录制数据包中的topic信息，发现imu这一消息类型中，个人录制数据包中四元数错误

此图为walking_dataset.bag中/imu_raw话题的输出结果，可以看到orientation这一个四元数是正确的

此图为msfl.bag中的/imu话题的输出结果，可以看到orientation这个四元数是错误的

2、中继发布/imu数据

3、注意到IMU的内参也要标定

• 参考链接：

  https://blog.csdn.net/qq_43112859/article/details/132879387