十五.激光和惯导LIO-SLAM框架学习之惯导与雷达外参标定（1）

专栏系列文章如下：

一：Tixiao Shan最新力作LVI-SAM(Lio-SAM Vins-Mono)，基于视觉-激光-惯导里程计的SLAM框架、环境建设和运行过程_goldqiu的博客-CSDN博客

二.激光SLAM框架学习之A-LOAM框架介绍_goldqiu的博客-CSDN博客

三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）_goldqiu的博客-CSDN博客

四.激光SLAM框架学习之A-LOAM框架-项目代码介绍-2.scanRegistration.cpp--前端雷达处理和特征提取_goldqiu的博客-CSDN博客

五.激光SLAM框架学习之A-LOAM框架-项目代码介绍-3.laserOdometry.cpp--前端雷达里程计和位置粗估计_goldqiu的博客-CSDN博客

六.激光SLAM框架学习之A-LOAM框架-项目代码介绍-4.laserMapping.cpp--估计(优化)后端建图和帧位姿势_goldqiu的博客-CSDN博客

七.激光SLAM框架学习之A-LOAM框架---速腾Robosense-室内建图16线雷达_goldqiu的博客-CSDN博客

八.激光SLAM框架学习之LeGO-LOAM框架-框架介绍和操作演示_goldqiu的博客-CSDN博客

九.激光SLAM框架学习之LeGO-LOAM框架---速腾Robosense-与其他框架相比，16线雷达的室外图纸被记录和保存_goldqiu的博客-CSDN博客

十.激光SLAM框架学习之LeGO-LOAM框架-算法原理和改进_goldqiu的博客-CSDN博客

十一.激光惯导LIO-SLAM框架学习之LIO-SAM框架---框架介绍和运行演示

十二.激光SLAM框架学习之livox-loam框架安装和运行数据集_goldqiu的博客-CSDN博客_livox 数据集

十三.激光SLAM框架学习之livox-Mid-70雷达和实时室外运行框架_goldqiu的博客-CSDN博客

十四.激光和惯导LIO-SLAM框架学习习惯导内参标定_goldqiu的博客-CSDN博客

即确认惯导和雷达的旋转转换矩阵

平移向量影响不大，可以直接从结构图中获得，即惯导原点到雷达原点的向量。旋转矩阵对构图有很大的影响。首先，我们必须确认理论旋转矩阵，然后进行校准。

旋转矩阵的认理论的方法：

将imageProjection.cpp回调函数中的注释打开。

  void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)     {         sensor_msgs::Imu thisImu = imuConverter(*imuMsg);   // 对imu做坐标转换         // 添加线程锁，把imu将数据保存到队列中         std::lock_guard<std::mutex> lock1(imuLock);         imuQueue.push_back(thisImu);          // debug IMU data         // cout << std::setprecision(6);         // cout << "IMU acc: " << endl;         // cout << "x: " << thisImu.linear_acceleration.x <<          //       ", y: " << thisImu.linear_acceleration.y <<          //       ", z: " << thisImu.linear_acceleration.z << endl;         // cout << "IMU gyro: " << endl;         // cout << "x: " << thisImu.angular_velocity.x <<          //       ", y: " << thisImu.angular_velocity.y <<          //       ", z: " << thisImu.angular_velocity.z << endl;         // double imuRoll, imuPitch, imuYaw;         // tf::Quaternion orientation;         // tf::quaternionMsgToTF(thisImu.orientation, orientation);         // tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);         // cout << "IMU roll pitch yaw: " << endl;         // cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;     } 

参考此视频https://youtu.be/BOUK8LYQhHs

确认惯导输出是否正常。如果Z轴的加速度为负，则Z轴反过来检查

LIO-SAM中config文件夹中的配置yaml文件：

这两个矩阵需要调整。根据与视频现象的比较，发现单位阵可以达到Z轴加速度为9.8(重力加速度)。

Z轴已经在这里确定，我们暂时无法确定X轴和Y轴的朝向，可以参考后面标定的旋转矩阵。

其中extrinsicRot表示imu->lidar坐标变换， 用于旋转imu坐标系下的加速度和角速度lidar坐标系下, extrinsicRPY则用于旋转imu欧拉角到坐标系下lidar坐标下, 由于lio-sam作者使用的imu欧拉角旋转方向与lidar坐标系不一致(即按旋转顺序旋转)， 因此，个旋转是不同的， 但大多数设备的两个旋转应该设置为相同，我们也可以设置为相同。

以下校准：

第一个标定软件(不可行):

下载标定工具

mkdir -p catkin_ws/src    cd catkin_ws/src git clone https://github.com/chennuo0125-HIT/lidar_imu_calib.git cd .. catkin_make -DCATKIN_WHITELIST_PACKAGES="ndt_omp;lidar_imu_calib"

第二种标定软件(目前可行):

https://github.com/stevengj/nlopt.git

github下载nlopt 并cmake编译

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

github下载源码ROS编译

编译时出现Could not find NLOPTConfig.cmake

解决方案：找到文件并将其放入工程目录中CMakeLists.txt加上这样一句话：

list(APPEND CMAKE_FIND_ROOT_PATH ${PROJECT_SOURCE_DIR})

因为这个标定软件没有IMU因此，重写数据接口http://loader.cc

改写如下：

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

录制话题数据，旋转三个轴，XY轴不要大幅度旋转

rosbag record -o 20211117.bag out /velodyne_points /imu_raw

修改标定软件包launch文件中的数据包路径，然后运行launch文件，等待漫长迭代优化时间。

最后输出数据：

这里发现标定矩阵类似于单位阵，说明单位阵是理论外参，而标定后的矩阵是考虑了小角度误差后的外参。

把旋转矩阵复制到LIO-SAM中config文件夹中的配置yaml文件中，更改这两个矩阵。

最后室外测试效果：