R3LIVE源码解析(7) — R3LIVE中LiDAR_front_end.cpp文件_r3live_bag_ouster是什么

目录

1 LiDAR_front_end.cpp简介

2 LiDAR_front_end.cpp程序解析

1 LiDAR_front_end.cpp简介

激光点云首先在LiDAR_front_end节点中提取特征点，将处理完的信息通过/laser_cloud_flat完成节点的发送出去，与FAST-LIO2相同R3LIVE也只用到了面特征作为ESIKF融合。

• 首先主函数会根据不同的雷达类型运行不同的回调函数
• 接着通过give_feature()提取角点和面点
• 提取面点时，通过plane_judge()面点类型的判断
• 提取过程中，会进行边缘点判断，也就是LOAM论文中的进行异常点剔除
• 最后将提取的角点和面点发布出去

2 LiDAR_front_end.cpp程序解析

这部分内容和FAST-LIO2的内容类似，首先在最上面先定义了一系列enum的信息，通过enum选择不同的激光雷达类型，特征点特征等，并通过orgtype存放一些激光雷达点的一些其他属性。

// 枚举类型：表示支持的雷达类型
enum LID_TYPE
{
    AVIA = 1,
    VELO16,
    OUST64
}; //{1, 2, 3}
 
// 枚举类型：表示特征点的类型
enum Feature
{
    Nor,        // 正常点
    Poss_Plane, // 可能的平面点
    Real_Plane, // 确定的平面点
    Edge_Jump,  // 有跨越的边
    Edge_Plane, // 边上的平面点
    Wire,       // 线段 这个也许当了无效点？也就是空间中的小线段？
    ZeroPoint   // 无效点 程序中未使用
};
// 枚举类型：位置标识
enum Surround
{
    Prev, // 前一个
    Next  // 后一个
};
 
// 枚举类型：表示有跨越边的类型
enum E_jump
{
    Nr_nor,  // 正常
    Nr_zero, // 0
    Nr_180,  // 180
    Nr_inf,  // 无穷大 跳变较远？
    Nr_blind // 在盲区？
};
// orgtype类：用于存储激光雷达点的一些其他属性
struct orgtype
{
    double range; // 点云在xy平面离雷达中心的距离
    double dista; // 当前点与后一个点之间的距离
    //假设雷达原点为O 前一个点为M 当前点为A 后一个点为N
    double angle[2];  // 这个是角OAM和角OAN的cos值
    double intersect; // 这个是角MAN的cos值
    E_jump edj[2];    // 前后两点的类型
    Feature ftype;    // 点类型
 
    // 构造函数
    orgtype()
    {
        range = 0;
        edj[Prev] = Nr_nor;
        edj[Next] = Nr_nor;
        ftype = Nor; //默认为正常点
        intersect = 2;
    }
};

接着看文件的主函数：

• 首先会初始化ros节点，然后获取配置文件的参数
• 为了简便，设置一些会用到的全局变量
• 后面根据lidar_type的值进行lidar数据的订阅设置

int main( int argc, char **argv )
{
    /**
     * (1)初始化名为"feature_extract"的ros节点
     */ 
    ros::init( argc, argv, "feature_extract" );
    ros::NodeHandle n;
 
    /**
     * (2)从参数服务器上获取参数,如果获取失败则=默认值
     * @note NodeHandle::param(const std::string& param_name, T& param_val, const T& default_val)
     */ 
    n.param< int >( "Lidar_front_end/lidar_type", lidar_type, 0 );
    n.param< double >( "Lidar_front_end/blind", blind, 0.1 );
    n.param< double >( "Lidar_front_end/inf_bound", inf_bound, 4 );
    n.param< int >( "Lidar_front_end/N_SCANS", N_SCANS, 1 );
    n.param< int >( "Lidar_front_end/group_size", group_size, 8 );
    n.param< double >( "Lidar_front_end/disA", disA, 0.01 );
    n.param< double >( "Lidar_front_end/disB", disB, 0.1 );
    n.param< double >( "Lidar_front_end/p2l_ratio", p2l_ratio, 225 );
    n.param< double >( "Lidar_front_end/limit_maxmid", limit_maxmid, 6.25 );
    n.param< double >( "Lidar_front_end/limit_midmin", limit_midmin, 6.25 );
    n.param< double >( "Lidar_front_end/limit_maxmin", limit_maxmin, 3.24 );
    n.param< double >( "Lidar_front_end/jump_up_limit", jump_up_limit, 170.0 );
    n.param< double >( "Lidar_front_end/jump_down_limit", jump_down_limit, 8.0 );
    n.param< double >( "Lidar_front_end/cos160", cos160, 160.0 );
    n.param< double >( "Lidar_front_end/edgea", edgea, 2 );
    n.param< double >( "Lidar_front_end/edgeb", edgeb, 0.1 );
    n.param< double >( "Lidar_front_end/smallp_intersect", smallp_intersect, 172.5 );
    n.param< double >( "Lidar_front_end/smallp_ratio", smallp_ratio, 1.2 );
    n.param< int >( "Lidar_front_end/point_filter_num", point_filter_num, 1 );
    n.param< int >( "Lidar_front_end/point_step", g_LiDAR_sampling_point_step, 3 );
    n.param< int >( "Lidar_front_end/using_raw_point", g_if_using_raw_point, 1 );
 
    // 设置需要用到的全局变量 - cos(角度->弧度)
    jump_up_limit = cos( jump_up_limit / 180 * M_PI );      // cos(170度)
    jump_down_limit = cos( jump_down_limit / 180 * M_PI );  // cos(8度)
    cos160 = cos( cos160 / 180 * M_PI );                    // cos(160度)
    smallp_intersect = cos( smallp_intersect / 180 * M_PI );// cos(172.5度)
 
    ros::Subscriber sub_points;
 
    /**
     * (3)根据lidar_type的值进行lidar数据的订阅设置
     *      绑定订阅后的回调函数
     */ 
    switch ( lidar_type )
    {
    case MID:   // enum MID = 0 : 默认
        printf( "MID40\n" );
        // 订阅/livox/lidar原始数据,回调函数中转换数据(/laser_cloud),并提取角/面特征后发布出去(laser_cloud_sharp/laser_cloud_flat)
        sub_points = n.subscribe( "/livox/lidar", 1000, mid_handler, ros::TransportHints().tcpNoDelay() );
        break;
 
    case HORIZON:   // enum HORIZON = 1 : r3live_bag_ouster.launch中设置
        printf( "HORIZON\n" );
        sub_points = n.subscribe( "/livox/lidar", 1000, horizon_handler, ros::TransportHints().tcpNoDelay() );
        break;
 
    case VELO16:
        printf( "VELO16\n" )