1.固态激光雷达和机械激光雷达的区别

LOAM、Lego-LOAM、LIO-SAM使用的激光雷达Velodyne都是机械激光雷达。通过机械方式改变扫描方向。通俗点就是一个激光笔放在一个电动机上旋转，激光笔跟着电动机进行360度高速旋转。

工作原理参考：[科普]激光雷达LIDAR工作原理|英飞凌开发者技术社区

固定激光雷达（以livox为例）

通过阵列干涉或者是mems改变扫描的方向。说白了就是一个激光笔放在那不动，他可以自己改变激光射出的方向。如下就是livox激光线的示意图，“激光笔”是不动的，但是内部有阵列干涉，通过干涉、反射等操作，让“激光笔”发射的点打在不同的位置。如下图，就打出了一个类似雪花的的形状。看下图中的第1张图，蓝色的是起点，红色的终点。

原文链接：https://blog.csdn.net/weixin_40331125/article/details/106136136

传统激光雷达普遍采用机械扫描方式，扫描路径随时间重复。而Livox 激光雷达采用了独特的扫描⽅式，扫描路径不会重复。在非重复扫描方式中，视场中被激光照射到的区域面积会随时间增大，这意味着视场覆盖率随时间推移而显著提高，可减小视场内物体被漏检的概率，有助于探测视场中的更多细节。

2.LIO-SAM 适配MID360

LIO-SAM支持的激光雷达类型是机械激光雷达，其主要特点是点云数据以线束顺序进行排列，知道每个点在哪个线ring以及该点距离该帧点云起始时刻的时间间隔time，对于角特征和面特征的提取也是利用了这一特点。直接将Livox-Mid-360的点云数据放入LIO-SAM中是无法运行的，它的点云数据格式有三种

// 0 -- Livox pointcloud2(PointXYZRTLT) pointcloud format
float32 x               # X axis, unit:m
float32 y               # Y axis, unit:m
float32 z               # Z axis, unit:m
float32 intensity       # the value is reflectivity, 0.0~255.0
uint8   tag             # livox tag
uint8   line            # laser number in lidar
float64 timestamp       # Timestamp of point


// 1 -- Livox customized pointcloud format
std_msgs/Header header     # ROS standard message header
uint64          timebase   # The time of first point
uint32          point_num  # Total number of pointclouds
uint8           lidar_id   # Lidar device id number
uint8[3]        rsvd       # Reserved use
CustomPoint[]   points     # Pointcloud data
// CustomPoint具体的格式如下：
uint32  offset_time     # offset time relative to the base time
float32 x               # X axis, unit:m
float32 y               # Y axis, unit:m
float32 z               # Z axis, unit:m
uint8   reflectivity    # reflectivity, 0~255
uint8   tag             # livox tag
uint8   line            # laser number in lidar


// 2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS)

采用自定义的点云，在livox的ROS驱动包中，将Mid-360的点云数据等效于以线束顺序进行排列，这样就可以直接使用LIO-SAM。

2.1 MID-360坐标系

lidar坐标系IMU到坐标系的外参

// lidar和imu xyz方向相同，旋转矩阵是单位阵； 
[0.9999161, 0.0026676,  0.0126707, -0.011,
 -0.0025826, 0.9999741, -0.0067201, -0.0234,
 -0.0126883, 0.0066868,  0.9998971, 0.044,
  0.0,       0.0,        0.0,       1.0]

2.2 代码修改 （ROS2）

参考：nkymzsy/LIO-SAM-MID360

注：如果只是运行MID-360，参考上面链接代码即可。下面代码的修改同时兼容 Velodyne和Livox MID-360。

1.配置文件

    # Sensor Settings
    sensor: "livox"
    N_SCAN: 4
    Horizon_SCAN: 6000
    downsampleRate: 1
    lidarMinRange: 1.0                    # MID-360: 1~40 (m) 
    lidarMaxRange: 40.0                    


    # IMU Settings
    imuType: 0                         # Livox: 0 ; other: 1
    # 注意使用MID360内置的IMU数据，加速度单位为g，后续处理中会将这一值乘以重力加速度，恢复为m/s^2

    extrinsicTrans: [-0.011,  -0.0234, 0.044]
    extrinsicRot: 
      [ 1.0, 0.0, 0.0,
        0.0, 1.0, 0.0,
        0.0, 0.0, 1.0 ]
    extrinsicRPY:
      [ 1.0, 0.0, 0.0,
        0.0, 1.0, 0.0,
        0.0, 0.0, 1.0 ]

2.IMU数据处理

MID360内置的IMU数据加速度单位为g，后续处理中会将这一值乘以重力加速度，恢复为m/s^2

sensor_msgs::msg::Imu ParamServer::imuConverter(const sensor_msgs::msg::Imu &imu_in)
{
    sensor_msgs::msg::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);
    
    // livox 内置的六轴imu的加速度单位是g 这里要还原到m/s^2
    if(imuType == 0)  
        acc = acc * imuGravity;

    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 orientation
    Eigen::Quaterniond q_from(imu_in.orientation.w,
                              imu_in.orientation.x,
                              imu_in.orientation.y,
                              imu_in.orientation.z);
    Eigen::Quaterniond q_final;
    
    if(imuType == 0)
        q_final = extQRPY;
    else if(imuType == 1)
        q_final = q_from * extQRPY;
    else{
        RCLCPP_FATAL(get_logger(),"imu_type can only be one of 0 or 1");
        rclcpp::shutdown();
    }

    q_final.normalize();

    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();

    // check validity
    double norm = q_final.norm();
    if (norm < 0.1) {
        RCLCPP_ERROR(this->get_logger(), "Invalid quaternion, please use a 9-axis IMU!");
        rclcpp::shutdown();
    }

    return imu_out;
}

3.MID-360自定义数据对应的结构体

struct LiovxPointCustomMsg
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    float time;
    uint16_t ring;
    uint16_t tag;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (LiovxPointCustomMsg,
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (float, time, time)
    (uint16_t, ring, ring) (uint16_t, tag, tag)
)

4.

LIO-SAM将点云分为角点和面点，然后以自定义消息发布，因此只需要对 ImageProjection 和FeatureExtraction 两个类中的代码进行修改即可.

4.1 ImageProjection 

①订阅 livox 自定义点云的回调函数 cloudHandlerLivox()

subLaserCloud = create_subscription<livox_ros_driver2::msg::CustomMsg>(pointCloudTopic, qos_lidar,
            std::bind(&ImageProjection::cloudHandlerLivox, this, std::placeholders::_1),lidarOpt);

void ImageProjection::cloudHandlerLivox(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr& msg)
{
    if (!cachePointCloudLivox(*msg)) return;

    if (!deskewInfo()) return;

    projectPointCloud();

    cloudExtraction();

    publishClouds();

    resetParameters();
}

② 将 livox_ros_driver2::msg::CustomMsg 转为 LiovxPointCustomMsg 类型点云

void ImageProjection::moveFromCustomMsg(const livox_ros_driver2::msg::CustomMsg &Msg,
                                        pcl::PointCloud<LiovxPointCustomMsg> &cloud)
{
    cloud.clear();
    cloud.reserve(Msg.point_num);
    cloud.header.frame_id = Msg.header.frame_id;
    cloud.header.stamp = (uint64_t)((Msg.header.stamp.sec * 1e9 + Msg.header.stamp.nanosec) / 1000) ;

    LiovxPointCustomMsg point;
    
    for(uint i = 0; i < Msg.point_num; i++)
    // for(uint i = 0; i < Msg.point_num - 1; i++)
    {
        point.x = Msg.points[i].x; 
        point.y = Msg.points[i].y; 
        point.z = Msg.points[i].z; 
        point.intensity = Msg.points[i].reflectivity;      // reflectivity -> intensity
        point.tag = Msg.points[i].tag;                     
        point.time = Msg.points[i].offset_time * 1e-9;     // ns -> s
        point.ring = Msg.points[i].line;                   // line -> ring
        cloud.push_back(point);
    }
    cloud.width = cloud.size();
    cloud.height = 1;
    cloud.is_dense = false;
}

bool ImageProjection::cachePointCloudLivox(const livox_ros_driver2::msg::CustomMsg &msg)
{
    // cache point cloud
    cloudQueueLivox.push_back(msg);

    if (cloudQueueLivox.size() <= 2) return false;

    auto current = std::move(cloudQueueLivox.front());
    cloudQueueLivox.pop_front();

    // CustomMsg -> Livox PCL 点云
    moveFromCustomMsg(current, *livoxCloudIn);

    // get timestamp
    cloudHeader = current.header;
    timeScanCur = ROS_TIME(cloudHeader.stamp);
    timeScanEnd = timeScanCur + (livoxCloudIn->empty() ? 0.0 : livoxCloudIn->points.back().time);

    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*livoxCloudIn, *livoxCloudIn, indices);

    // check dense flag
    if (!livoxCloudIn->is_dense)
    {
        RCLCPP_ERROR(get_logger(), "Point cloud is not in dense format, please remove NaN points first!");
        rclcpp::shutdown();
    }

    return true;
}

③

size_t ImageProjection::currentCloudSize() const
{
    return isVelodyne() ? laserCloudIn->size() : livoxCloudIn->size();
}

void ImageProjection::readInputPoint(size_t i, PointType &thisPoint, int &rowIdn, float &relTime)
{
    if (isVelodyne()) {
        const auto &src = laserCloudIn->points[i];
        thisPoint.x = src.x; 
        thisPoint.y = src.y; 
        thisPoint.z = src.z; 
        thisPoint.intensity = src.intensity;
        rowIdn  = static_cast<int>(src.ring);
        relTime = src.time;          // s
    } else {
        const auto &src = livoxCloudIn->points[i];
        thisPoint.x = src.x; 
        thisPoint.y = src.y; 
        thisPoint.z = src.z; 
        thisPoint.intensity = src.intensity;
        rowIdn  = static_cast<int>(src.ring);
        relTime = src.time;          // 已在转换中做 ns->s
    }
}

void ImageProjection::projectPointCloud()
{
    // int cloudSize = laserCloudIn->points.size();
    const int cloudSize = static_cast<int>(currentCloudSize());
    
    #pragma omp parallel for num_threads(numberOfCores) 
    for (int i = 0; i < cloudSize; ++i)
    {
        PointType thisPoint;
        int rowIdn; float relTime;
        readInputPoint(i, thisPoint, rowIdn, relTime);

        float range = PointDistance(thisPoint);
        if (range < lidarMinRange || range > lidarMaxRange) continue;
            
        if (rowIdn < 0 || rowIdn >= N_SCAN) continue;
            
        if (rowIdn % downsampleRate != 0) continue;
            
        int columnIdn = -1;
        if (isVelodyne())
        {
            float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
            static float ang_res_x = 360.0 / float(Horizon_SCAN);
            columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN/2;
            if (columnIdn >= Horizon_SCAN)  columnIdn -= Horizon_SCAN;
        }else if(isLivox())
        {
            columnIdn = columnIdnCountVec[rowIdn];
            columnIdnCountVec[rowIdn] += 1;
        }
        
        if (columnIdn < 0 || columnIdn >= Horizon_SCAN) continue;
            
        if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)  continue;
            
        thisPoint = deskewPoint(&thisPoint, relTime);    // TODO

        rangeMat.at<float>(rowIdn, columnIdn) = range;

        int index = columnIdn  + rowIdn * Horizon_SCAN;
        fullCloud->points[index] = thisPoint;
    }
}

4.2 FeatureExtraction

① 计算曲率不同

void FeatureExtraction::calculateSmoothness()
{
    int cloudSize = extractedCloud->points.size();

    if(sensor == SensorType::VELODYNE)
    {
        for (int i = 5; i < cloudSize - 5; i++)
        {
            float diffRange = cloudInfo.point_range[i-5] + cloudInfo.point_range[i-4]
                        + cloudInfo.point_range[i-3] + cloudInfo.point_range[i-2]
                        + cloudInfo.point_range[i-1] - cloudInfo.point_range[i] * 10
                        + cloudInfo.point_range[i+1] + cloudInfo.point_range[i+2]
                        + cloudInfo.point_range[i+3] + cloudInfo.point_range[i+4]
                        + cloudInfo.point_range[i+5];            

            cloudCurvature[i] = diffRange * diffRange; 

            cloudNeighborPicked[i] = 0;
            cloudLabel[i] = 0;
        
            cloudSmoothness[i].value = cloudCurvature[i];
            cloudSmoothness[i].ind = i;
        }
    }else if(sensor == SensorType::LIVOX)
    {
        for (int i = 5; i < cloudSize - 5; i++)
        {
            float diffRange = 
                            cloudInfo.point_range[i-2]  + cloudInfo.point_range[i-1] - cloudInfo.point_range[i] * 4
                            + cloudInfo.point_range[i+1] + cloudInfo.point_range[i+2];    

            cloudCurvature[i] = diffRange*diffRange;

            cloudNeighborPicked[i] = 0;
            cloudLabel[i] = 0;
            // cloudSmoothness for sorting
            cloudSmoothness[i].value = cloudCurvature[i];
            cloudSmoothness[i].ind = i;
        }
    }
    
}

②标记属于遮挡、平行两种情况的点

void FeatureExtraction::markOccludedPoints()
{
    int cloudSize = extractedCloud->points.size();
    for (int i = 5; i < cloudSize - 6; ++i)
    {
        float depth1 = cloudInfo.point_range[i];
        float depth2 = cloudInfo.point_range[i+1];
        int columnDiff = std::abs(int(cloudInfo.point_col_ind[i+1] - cloudInfo.point_col_ind[i]));

        if (columnDiff < 10)
        {
            if (depth1 - depth2 > 0.3)
            {
                if (sensor == SensorType::VELODYNE)
                {
                    cloudNeighborPicked[i - 5] = 1;
                    cloudNeighborPicked[i - 4] = 1;
                    cloudNeighborPicked[i - 3] = 1;
                    cloudNeighborPicked[i - 2] = 1;
                    cloudNeighborPicked[i - 1] = 1;
                    cloudNeighborPicked[i] = 1;
                }else if(sensor == SensorType::LIVOX)
                {
                    cloudNeighborPicked[i - 1] = 1;
                    cloudNeighborPicked[i] = 1; 
                }
            }
            else if (depth2 - depth1 > 0.3)
            {
                if (sensor == SensorType::VELODYNE)
                {
                    cloudNeighborPicked[i + 1] = 1;
                    cloudNeighborPicked[i + 2] = 1;
                    cloudNeighborPicked[i + 3] = 1;
                    cloudNeighborPicked[i + 4] = 1;
                    cloudNeighborPicked[i + 5] = 1;
                cloudNeighborPicked[i + 6] = 1;
                }else if(sensor == SensorType::LIVOX)
                {
                    cloudNeighborPicked[i + 1] = 1;
                    cloudNeighborPicked[i + 2] = 1;
                }
            }
        }
        
        float diff1 = std::abs(float(cloudInfo.point_range[i-1] - cloudInfo.point_range[i]));
        float diff2 = std::abs(float(cloudInfo.point_range[i+1] - cloudInfo.point_range[i]));

        if (diff1 > 0.02 * cloudInfo.point_range[i] && diff2 > 0.02 * cloudInfo.point_range[i])
            cloudNeighborPicked[i] = 1;
    }
}