LIO-SAM源码阅读笔记(一)---配置文件

最新推荐文章于 2022-12-03 10:04:51 发布

AutoMan-USTC

最新推荐文章于 2022-12-03 10:04:51 发布

阅读量2.1k

点赞数 5

分类专栏：激光SLAM

本文链接：https://blog.csdn.net/Zlp19970106/article/details/108555220

版权

激光SLAM 专栏收录该内容

19 篇文章 13 订阅

订阅专栏

文章目录

代码：
https://github.com/TixiaoShan/LIO-SAM
针对每个人手头的不同的雷达或者IMU等，只需要在utility.h和params.yaml两个文件中进行主要的修改，在imageProjection.cpp中区分了velodyne和Ouster的雷达，也需要根据实际情况进行部分代码的注释与取消注释。

lidar、IMU：
在这里插入图片描述
System architecture：

在这里插入图片描述

1. params.yaml

params.yaml文件是对于每个人不同的传感器等修改配置的主要部分。

# Topics
  pointCloudTopic: "points_raw"               # Point cloud data 点云话题 根据自己的修改
  imuTopic: "imu_raw"                         # IMU data
  odomTopic: "odometry/imu"                   # IMU pre-preintegration odometry, same frequency as IMU
  gpsTopic: "odometry/gpsz"                   # GPS odometry topic from navsat, see module_navsat.launch file
                                                   
  # Frames  坐标系这块根据自己发的改 大部分可以默认用这个配置
  lidarFrame: "base_link"
  baselinkFrame: "base_link"
  odometryFrame: "odom"
  mapFrame: "map"

  # GPS Settings  
  # LIO-SAM里将GPS作为一种非必要的约束 想用这个进行优化前确认信号状态足够好
  useImuHeadingInitialization: true           # if using GPS data, set to "true"
  useGpsElevation: false                      # if GPS elevation is bad, set to "false"
  #算法判读是否加入GPS Factor的条件就是对协方差矩阵与阈值的对比  如果GPS信号好可以改下阈值让GPS多参与进来
  gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
  poseCovThreshold: 25.0                      # m^2, threshold for using GPS data
  
  # Export settings
  #保存点云地图 这个比较好用 不用单独开终端  
  savePCD: false                              # https://github.com/TixiaoShan/LIO-SAM/issues/3
  savePCDDirectory: "/Downloads/LOAM/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

  # Sensor Settings
  # 雷达的参数这块很重要 否则影响后续点云处理
  # 作者给出的是velo和Ouster的雷达配置 
  # 其他雷达 需要单独根据雷达的参数改一下配置
  # timeField这个参数比较搞  如果不是velo或ouster的雷达可能就没有这个field会有一些问题，这里先默认
  N_SCAN: 16                                  # number of lidar channel (i.e., 16, 32, 64, 128)
  Horizon_SCAN: 1800                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
  timeField: "time"                           # point timestamp field, Velodyne - "time", Ouster - "t"
  downsampleRate: 1                           # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1 

  # IMU Settings  作者的IMU的参数  如果追求更好的效果 建议用自己的传感器参数代替 
  imuAccNoise: 3.9939570888238808e-03
  imuGyrNoise: 1.5636343949698187e-03
  imuAccBiasN: 6.4356659353532566e-05
  imuGyrBiasN: 3.5640318696367613e-05
  imuGravity: 9.80511

  # Extrinsics (lidar -> IMU)
  #这块是个重点  作者上一行写的lidar->imu其实容易造成理解的偏差，通过后续操作可知其实是把imu数据转到雷达坐标系下
  #标定这块 平移矩阵没啥好说的 其实这个影响不是特别大 重点是旋转
  extrinsicTrans: [0.0, 0.0, 0.0]
  #这里的旋转比较关键 需要配合utility.h中相关操作以及作者提供的图理解
  #作者的IMU与雷达的坐标系以及IMU提供的欧拉角的方向跟常用的有区别 特别注意IMU 的yaw角是与常见的反向的
  #所以才将Rot和RPY的旋转变换分开写，其实常见的两个矩阵都是一样的
  #一定要根据你的传感器安装位置以及默认坐标轴的方向进行标定 同时追踪extrinsicRot这个值在utility.h文件中的处理 
  extrinsicRot: [-1, 0, 0,
                  0, 1, 0,
                  0, 0, -1]
  
  extrinsicRPY: [0,  1, 0,
                 -1, 0, 0,
                  0, 0, 1]
  # extrinsicRot: [1, 0, 0,
  #                 0, 1, 0,
  #                 0, 0, 1]
  # extrinsicRPY: [1, 0, 0,
  #                 0, 1, 0,
  #                 0, 0, 1]

params.yaml文件其他的参数可以暂时默认，后期验证其他效果可以再改。

2. utility.h

注意params.yaml文件修改了可以直接运行，utility.h则需要编译再运行才会起作用。

外参读取的部分，可以看到对于欧拉角的变换被转成了四元数，这里也是需要关注的，后续用的都是extRot extQRPY extTrans.

 nh.param<vector<double>>("lio_sam/extrinsicRot", extRotV, vector<double>());
 nh.param<vector<double>>("lio_sam/extrinsicRPY", extRPYV, vector<double>());
 nh.param<vector<double>>("lio_sam/extrinsicTrans", extTransV, vector<double>());
 extRot = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRotV.data(), 3, 3);
 extRPY = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRPYV.data(), 3, 3);
 extTrans = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransV.data(), 3, 1);
 extQRPY = Eigen::Quaterniond(extRPY);

利用变换参数将数据从imu坐标系转到了雷达坐标系，这里建议对比下自己的imu数据是弧度还是角度值，以及加速度是原始单位还是一个0-1的系数乘9.8，这里区别也是挺大的。imuConverter这个函数这里没有调用，在其他文件中被调用。

sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
    {
        sensor_msgs::Imu imu_out = imu_in;
        // rotate acceleration
        Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
        acc = extRot * acc;
        imu_out.linear_acceleration.x = acc.x();
        imu_out.linear_acceleration.y = acc.y();
        imu_out.linear_acceleration.z = acc.z();
        // rotate gyroscope
        Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
        gyr = extRot * gyr;
        imu_out.angular_velocity.x = gyr.x();
        imu_out.angular_velocity.y = gyr.y();
        imu_out.angular_velocity.z = gyr.z();
        // rotate roll pitch yaw
        Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
        Eigen::Quaterniond q_final = q_from * extQRPY;
        imu_out.orientation.x = q_final.x();
        imu_out.orientation.y = q_final.y();
        imu_out.orientation.z = q_final.z();
        imu_out.orientation.w = q_final.w();

        if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
        {
            ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
            ros::shutdown();
        }

        return imu_out;
    }

一些提前定义的关于IMU数据的转换函数，类似于一种工具，在其他文件中被调用，这里其实可以做一些初始补偿等工作。


template<typename T>
void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
{
    *angular_x = thisImuMsg->angular_velocity.x;
    *angular_y = thisImuMsg->angular_velocity.y;
    *angular_z = thisImuMsg->angular_velocity.z;
}


template<typename T>
void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z)
{
    *acc_x = thisImuMsg->linear_acceleration.x;
    *acc_y = thisImuMsg->linear_acceleration.y;
    *acc_z = thisImuMsg->linear_acceleration.z;
}


template<typename T>
void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
{
    double imuRoll, imuPitch, imuYaw;
    tf::Quaternion orientation;
    tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
    tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);

    *rosRoll = imuRoll;
    *rosPitch = imuPitch;
    *rosYaw = imuYaw;
}


float pointDistance(PointType p)
{
    return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}


float pointDistance(PointType p1, PointType p2)
{
    return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z));
}

3. imageProjection.cpp

在个人的数据集或者传感器配置中还有最后一部分工作在imageProjection.cpp文件中，需要根据你是velodyne还是ouster雷达在开头就选择雷达数据的结构体，其他雷达也可以仿照修改。

// Velodyne
struct PointXYZIRT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    uint16_t ring;
    float time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRT,  
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
    (uint16_t, ring, ring) (float, time, time)
)

// Ouster
// struct PointXYZIRT {
//     PCL_ADD_POINT4D;
//     float intensity;
//     uint32_t t;
//     uint16_t reflectivity;
//     uint8_t ring;
//     uint16_t noise;
//     uint32_t range;
//     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// }EIGEN_ALIGN16;

// POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT,
//     (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
//     (uint32_t, t, t) (uint16_t, reflectivity, reflectivity)
//     (uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range)
// )