R3LIVE源码解析(7) — R3LIVE中LiDAR_front_end.cpp文件

news2024/12/23 17:11:25

目录

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" );   // enum VELO16 = 2 : TODO
        sub_points = n.subscribe( "/velodyne_points", 1000, velo16_handler, ros::TransportHints().tcpNoDelay() );
        break;

    case OUST64:
        printf( "OUST64\n" );   // enum OUST64 = 3 : TODO
        sub_points = n.subscribe( "/os_cloud_node/points", 1000, oust64_handler, ros::TransportHints().tcpNoDelay() );
        break;

    default:
        printf( "Lidar type is wrong.\n" );
        exit( 0 );
        break;
    }

    /**
     * (4)设置需要发布的消息格式
     *      回调函数接收到原始点云后,将其转换为指定格式,并提取角点特征和面特征
     *      然后通过pub_full,pub_surf,pub_corn将消息发布出去
     */ 
    pub_full = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud", 100 );  //发布转换后的消息
    pub_surf = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud_flat", 100 ); //发布面特征消息
    pub_corn = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud_sharp", 100 );//发布角点特征消息

    ros::spin();
    return 0;
}

文章以livox雷达为例,进入到mid_handler回调函数。mid_handler回调函数接收/livox/lidar原始数据,回调函数中转换数据类型(/laser_cloud),并提取角/面特征后发布出去(laser_cloud_sharp/laser_cloud_flat) 

  • 首先会进行消息类型的格式转换,通过PointCloud2格式的msg转换为PointXYZINormal格式。
  • 接着遍历每一个点云,将点云保存为自定义的vector< orgtype >格式,并设置最后一个点的range信息。
  • 接着提取角点和面点
  • 最后将提取的角点和面点发布出去
/**
 * @note LiDAR_front_end的回调函数
 *      接收/livox/lidar原始数据,回调函数中转换数据(/laser_cloud),
 *      并提取角/面特征后发布出去(laser_cloud_sharp/laser_cloud_flat)
 */ 
double vx, vy, vz;
void   mid_handler( const sensor_msgs::PointCloud2::ConstPtr &msg )
{
    // typedef pcl::PointXYZINormal PointType;
    pcl::PointCloud< PointType > pl;    // 存放Lidar点的一维数组
    pcl::fromROSMsg( *msg, pl ); // 通过PointCloud2格式的msg初始化PointXYZINormal的pl

    pcl::PointCloud< PointType > pl_corn, pl_surf;  // 保存提取的角点和平面点
    vector< orgtype >            types; // 自定义的结构提 : TODO
    uint                         plsize = pl.size() - 1;    // Lidar点数量
    pl_corn.reserve( plsize );  // 预留size
    pl_surf.reserve( plsize );  // 预留size
    types.resize( plsize + 1 );

    // 遍历msg中的前n-1个点,按照保存在types中(关键内容:range和dista)
    for ( uint i = 0; i < plsize; i++ )
    {
        types[ i ].range = pl[ i ].x;   // 
        vx = pl[ i ].x - pl[ i + 1 ].x; // 相邻两点做差
        vy = pl[ i ].y - pl[ i + 1 ].y; // 得到两点构成
        vz = pl[ i ].z - pl[ i + 1 ].z; // 的向量
        types[ i ].dista = vx * vx + vy * vy + vz * vz;// 向量模长(少开根号)
    }
    // 设置最后一个点的range信息 : =最后一点到原点的距离
    types[ plsize ].range = sqrt( pl[ plsize ].x * pl[ plsize ].x + pl[ plsize ].y * pl[ plsize ].y );

    // 提取角点特征(pl_corn)和面特征(pl_surf)
    give_feature( pl, types, pl_corn, pl_surf );

    // 将转换后的pl,提取到的pl_surf,pl_corn发布出去.
    ros::Time ct( ros::Time::now() );
    pub_func( pl, pub_full, msg->header.stamp );
    pub_func( pl_surf, pub_surf, msg->header.stamp );
    pub_func( pl_corn, pub_corn, msg->header.stamp );
}

give_feature()函数主要是特征提取,下面我们对该函数进行学习和了解,对于每条line的点云提取特征 

/**
 * @note 提取Lidar帧的点特征和面特征
 * @param pl : Lidar帧的原始点容器
 * @param types : 对Lidar原始点计算了相邻两点距离后的点容器
 * @param pl_corn : 存放提取到的点特征
 * @param pl_surf : 存放提取到的面特征
 * ***注意*** : 默认没有提取点特征,且只以间隔3个点的形式赋值了面特征
 *             可能是为了速度吧~~~
 */ 
void give_feature( pcl::PointCloud< PointType > &pl, vector< orgtype > &types, 
                   pcl::PointCloud< PointType > &pl_corn,
                   pcl::PointCloud< PointType > &pl_surf )
{
    /*
     * step1:预先判断点数量和点的距离,并定义相关变量
     */
    uint plsize = pl.size();   // 单条线的点数
    uint plsize2;
    if ( plsize == 0 )  // 判断点数量
    {
        printf( "something wrong\n" );
        return;
    }
    uint head = 0;
    while ( types[ head ].range < blind )   // blind = 0.1 : default
    {           // types[i].range = pl[i].x
        head++; // 因此猜测这里判断lidar帧中点的距离
    }           // 保证后续处理的起点在一定距离外,防止干扰点

    // Surf : group_size = 8 : 点数大于8,则后续处理范围为 head -> 点数-8,
    // 否则为0, 因该是只计算head到倒数第8点之间的中间点.
    plsize2 = ( plsize > group_size ) ? ( plsize - group_size ) : 0;

    Eigen::Vector3d curr_direct( Eigen::Vector3d::Zero() ); // 当前平面的法向量
    Eigen::Vector3d last_direct( Eigen::Vector3d::Zero() ); //上一个平面的法向量

    uint i_nex = 0, i2;  // i2为当前点的下一个点
    uint last_i = 0;    // last_i为上一个点的保存的索引
    uint last_i_nex = 0;     // last_i_nex为上一个点的下一个点的索引
    int  last_state = 0;     //为1代表上个状态为平面 否则为0
    int  plane_type;

    PointType ap;// 充当中间交换量的临时容器

    /*
     * step2:对head到倒数第8点之间的中间点进行计算
     */
    for ( uint i = head; i < plsize2; i += g_LiDAR_sampling_point_step )    // g_LiDAR_sampling_point_step = 3
    {
        // 在盲区范围内的点不做处理
        if ( types[ i ].range > blind )
        {
            ap.x = pl[ i ].x;
            ap.y = pl[ i ].y;
            ap.z = pl[ i ].z;
            ap.curvature = pl[ i ].curvature;
            pl_surf.push_back( ap );
        }
        if ( g_if_using_raw_point )
        {
            continue;
        }
        //更新i2
        i2 = i;
        //求得平面,并返回类型0 1 2
        plane_type = plane_judge( pl, types, i, i_nex, curr_direct );
        //返回1:一般默认是平面
        if ( plane_type == 1 )
        {
            for ( uint j = i; j <= i_nex; j++ )
            {
                if ( j != i && j != i_nex )
                {
                    //把起始点和终止点之间的所有点设置为确定的平面点
                    types[ j ].ftype = Real_Plane;
                }
                else
                {
                    //把起始点和终止点设置为可能的平面点
                    types[ j ].ftype = Poss_Plane;
                }
            }

            // 最开始last_state=0直接跳过, 之后last_state=1
            // //如果之前状态是平面则判断当前点是处于两平面边缘的点还是较为平坦的平面的点
            if ( last_state == 1 && last_direct.norm() > 0.1 )
            {
                double mod = last_direct.transpose() * curr_direct;
                if ( mod > -0.707 && mod < 0.707 )
                {
                    //修改ftype,两个面法向量夹角在45度和135度之间 认为是两平面边缘上的点
                    types[ i ].ftype = Edge_Plane;
                }
                else
                {
                    //否则认为是真正的平面点
                    types[ i ].ftype = Real_Plane;
                }
            }

            i = i_nex - 1;
            last_state = 1;
        }
        else if ( plane_type == 2 )
        {
            // plane_type=2的时候
            i = i_nex;
            last_state = 0; // 设置为不是平面点
        }
        else if ( plane_type == 0 )
        {
            // plane_type=2的时候
            // 为1代表上个状态为平面
            if ( last_state == 1 )
            {
                uint i_nex_tem;
                uint j;
                // 遍历区间中的点重新进行判断
                for ( j = last_i + 1; j <= last_i_nex; j++ )
                {
                    uint            i_nex_tem2 = i_nex_tem;
                    Eigen::Vector3d curr_direct2;

                    uint ttem = plane_judge( pl, types, j, i_nex_tem, curr_direct2 );

                    if ( ttem != 1 )
                    {
                        i_nex_tem = i_nex_tem2;
                        break;
                    }
                    curr_direct = curr_direct2;
                }

                if ( j == last_i + 1 )
                {
                    last_state = 0;
                }
                else
                {
                    for ( uint k = last_i_nex; k <= i_nex_tem; k++ )
                    {
                        if ( k != i_nex_tem )
                        {
                            //把起始点和终止点之间的所有点设置为确定的平面点
                            types[ k ].ftype = Real_Plane;
                        }
                        else
                        {
                            //把起始点和终止点设置为可能的平面点
                            types[ k ].ftype = Poss_Plane;
                        }
                    }
                    i = i_nex_tem - 1;
                    i_nex = i_nex_tem;
                    i2 = j - 1;
                    last_state = 1;
                }
            }
        }
        // 更新变量
        last_i = i2;
        last_i_nex = i_nex;
        last_direct = curr_direct;
    }
    if ( g_if_using_raw_point )
    {
        return;
    }

   // 下面进行边缘点的提取
    plsize2 = plsize > 3 ? plsize - 3 : 0; //如果剩下的点数小于3则不判断边缘点,否则计算哪些点是边缘点
    for ( uint i = head + 3; i < plsize2; i++ )
    {
        //点不能在盲区或者点必须属于正常点和可能的平面点,否则退出
        if ( types[ i ].range < blind || types[ i ].ftype >= Real_Plane )
        {
            continue;
        }
        //该点与前后点的距离不能挨的太近
        if ( types[ i - 1 ].dista < 1e-16 || types[ i ].dista < 1e-16 )
        {
            continue;
        }

        Eigen::Vector3d vec_a( pl[ i ].x, pl[ i ].y, pl[ i ].z );   //当前点坐标组成的向量
        Eigen::Vector3d vecs[ 2 ];

        for ( int j = 0; j < 2; j++ )
        {
            int m = -1;
            if ( j == 1 )
            {
                m = 1;
            }
            //若当前的前/后一个点在盲区内(4m)
            if ( types[ i + m ].range < blind )
            {
                if ( types[ i ].range > inf_bound )
                {
                    //若其大于10m
                    //赋予该点Nr_blind(在盲区)
                    types[ i ].edj[ j ] = Nr_inf;
                }
                else
                {
                    //赋予该点Nr_blind(在盲区)
                    types[ i ].edj[ j ] = Nr_blind;
                }
                continue;
            }

            vecs[ j ] = Eigen::Vector3d( pl[ i + m ].x, pl[ i + m ].y, pl[ i + m ].z );
            vecs[ j ] = vecs[ j ] - vec_a; //前、后点指向当前点的向量

            //若雷达坐标系原点为O,当前点为A,前、后一点为M和N
            //则下面OA点乘MA/(|OA|*|MA|),得到的是cos角OAM的大小
            types[ i ].angle[ j ] = vec_a.dot( vecs[ j ] ) / vec_a.norm() / vecs[ j ].norm();
            if ( types[ i ].angle[ j ] < jump_up_limit )
            {
                // M在OA延长线上
                types[ i ].edj[ j ] = Nr_180;
            }
            else if ( types[ i ].angle[ j ] > jump_down_limit )
            {
                // M在OA上
                types[ i ].edj[ j ] = Nr_zero;
            }
        }

        types[ i ].intersect = vecs[ Prev ].dot( vecs[ Next ] ) / vecs[ Prev ].norm() / vecs[ Next ].norm();
        if ( types[ i ].edj[ Prev ] == Nr_nor && types[ i ].edj[ Next ] == Nr_zero && types[ i ].dista > 0.0225 &&
             types[ i ].dista > 4 * types[ i - 1 ].dista )
        {
            //角MAN要小于160度,不然就平行于激光了
            if ( types[ i ].intersect > cos160 )
            {
                if ( edge_jump_judge( pl, types, i, Prev ) )
                {
                    types[ i ].ftype = Edge_Jump;
                }
            }
        }
        // 前一个点在激光束上 && 后一个点正常 && 前一个点与当前点的距离大于0.0225m && 前一个点与当前点的距离大于当前点与后一个点距离的四倍
        else if ( types[ i ].edj[ Prev ] == Nr_zero && types[ i ].edj[ Next ] == Nr_nor && types[ i - 1 ].dista > 0.0225 &&
                  types[ i - 1 ].dista > 4 * types[ i ].dista )
        {
            if ( types[ i ].intersect > cos160 )
            {
                if ( edge_jump_judge( pl, types, i, Next ) )
                {
                    types[ i ].ftype = Edge_Jump;
                }
            }
        }
        // 前面的是正常点 && (当前点到中心距离>10m并且后点在盲区)
        else if ( types[ i ].edj[ Prev ] == Nr_nor && types[ i ].edj[ Next ] == Nr_inf )
        {
            if ( edge_jump_judge( pl, types, i, Prev ) )
            {
                types[ i ].ftype = Edge_Jump;
            }
        }
        // (当前点到中心距离>10m并且前点在盲区) && 后面的是正常点
        else if ( types[ i ].edj[ Prev ] == Nr_inf && types[ i ].edj[ Next ] == Nr_nor )
        {
            if ( edge_jump_judge( pl, types, i, Next ) )
            {
                types[ i ].ftype = Edge_Jump;
            }
        }
        // 前后点都不是正常点
        else if ( types[ i ].edj[ Prev ] > Nr_nor && types[ i ].edj[ Next ] > Nr_nor )
        {
            if ( types[ i ].ftype == Nor )
            {
                types[ i ].ftype = Wire;
            }
        }
    }

    plsize2 = plsize - 1;
    double ratio;
    //继续找平面点
    for ( uint i = head + 1; i < plsize2; i++ )
    {
        //前面、当前、之后三个点都需要不在盲区内
        if ( types[ i ].range < blind || types[ i - 1 ].range < blind || types[ i + 1 ].range < blind )
        {
            continue;
        }
        //前面和当前、当前和之后的点与点之间距离都不能太近
        if ( types[ i - 1 ].dista < 1e-8 || types[ i ].dista < 1e-8 )
        {
            continue;
        }

        //还剩下来一些正常点继续找平面点
        if ( types[ i ].ftype == Nor )
        {
            //求点与点间距的比例,大间距/小间距
            if ( types[ i - 1 ].dista > types[ i ].dista )
            {
                ratio = types[ i - 1 ].dista / types[ i ].dista;
            }
            else
            {
                ratio = types[ i ].dista / types[ i - 1 ].dista;
            }
            //如果夹角大于172.5度 && 间距比例<1.2
            if ( types[ i ].intersect < smallp_intersect && ratio < smallp_ratio )
            {
                // 前后三个点认为是平面点
                if ( types[ i - 1 ].ftype == Nor )
                {
                    types[ i - 1 ].ftype = Real_Plane;
                }
                if ( types[ i + 1 ].ftype == Nor )
                {
                    types[ i + 1 ].ftype = Real_Plane;
                }
                types[ i ].ftype = Real_Plane;
            }
        }
    }
    //存储平面点
    int last_surface = -1;
    for ( uint j = head; j < plsize; j++ )
    {
        //可能的平面点和确定的平面点
        if ( types[ j ].ftype == Poss_Plane || types[ j ].ftype == Real_Plane )
        {
            if ( last_surface == -1 )
            {
                last_surface = j;
            }
            //通常连着好几个都是面点
            //必须在采样间隔上的平面点才使用(这里的是无差别滤波 从每次新找到面点开始每几个点才取一个)
            if ( j == uint( last_surface + point_filter_num - 1 ) )
            {
                PointType ap;
                ap.x = pl[ j ].x;
                ap.y = pl[ j ].y;
                ap.z = pl[ j ].z;
                ap.curvature = pl[ j ].curvature;
                pl_surf.push_back( ap );

                last_surface = -1;
            }
        }
        else
        {   
            //跳变较大的边缘边的点和位于平面边缘的点
            if ( types[ j ].ftype == Edge_Jump || types[ j ].ftype == Edge_Plane )
            {
                pl_corn.push_back( pl[ j ] );
            }
            //假如上次找到的面点被无差别滤波掉了,而此时已经到了边缘
            if ( last_surface != -1 )
            {
                PointType ap;
                //取上次面点到此次边缘线之间的所有点的重心当作一个面点存储进去
                for ( uint k = last_surface; k < j; k++ )
                {
                    ap.x += pl[ k ].x;
                    ap.y += pl[ k ].y;
                    ap.z += pl[ k ].z;
                    ap.curvature += pl[ k ].curvature;
                }
                ap.x /= ( j - last_surface );
                ap.y /= ( j - last_surface );
                ap.z /= ( j - last_surface );
                ap.curvature /= ( j - last_surface );
                pl_surf.push_back( ap );
            }
            last_surface = -1;
        }
    }
}

通过plane_judge()面点类型的判断,面特征主要是主要是依据点与点之间的距离来拟合成一个向量,并通过乘积来计算出面特征的提取,这里的面特征提取和边缘特征提取和LOAM的类似。

LOAM特征提取方法:LOAM提出了一种简单而高效的特征点提取方式:根据点云点的曲率提取特征点。即把特别尖锐的边线点与特别平坦的平面点作为特征点。公式看起来比较复杂,实际上就是同一条扫描线上的取目标点左右两侧各5个点,分别与目标点的坐标作差,得到的结果就是目标点的曲率。当目标点处在棱或角的位置时,自然与周围点的差值较大,得到的曲率较大;反之当目标点在平面上时,周围点与目标点的坐标相近,得到的曲率自然较小。

/*
* 平面类型判断
*/
int plane_judge( const pcl::PointCloud< PointType > &pl, vector< orgtype > &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct )
{
    // 0.01*sqrt(x^2+y^2)+0.1,基本上可以近似看成是0.1,100m的时候才到0.2
    double group_dis = disA * types[ i_cur ].range + disB;
    group_dis = group_dis * group_dis;
    // i_nex = i_cur;

    double           two_dis;
    vector< double > disarr;    // 前后点距离数组
    disarr.reserve( 20 );

    for ( i_nex = i_cur; i_nex < i_cur + group_size; i_nex++ )
    {
        if ( types[ i_nex ].range < blind )
        {
            // 距离雷达原点太小了将法向量设置为零向量
            curr_direct.setZero();
            return 2;
        }
        // 存储当前点与后一个点的距离
        disarr.push_back( types[ i_nex ].dista );
    }

    //看看后续的点有没有满足条件的
    for ( ;; )
    {
        //索引超出所有点的个数直接break
        if ( ( i_cur >= pl.size() ) || ( i_nex >= pl.size() ) )
            break;

        if ( types[ i_nex ].range < blind )
        {
            // 距离雷达原点太小了将法向量设置为零向量
            curr_direct.setZero();
            return 2;
        }
        //最后的i_nex点到i_cur点的距离
        vx = pl[ i_nex ].x - pl[ i_cur ].x;
        vy = pl[ i_nex ].y - pl[ i_cur ].y;
        vz = pl[ i_nex ].z - pl[ i_cur ].z;
        two_dis = vx * vx + vy * vy + vz * vz;

        //距离i_cur点太远了就直接break
        if ( two_dis >= group_dis )
        {
            break;
        }
        disarr.push_back( types[ i_nex ].dista );   //存储当前点与后一个点的距离
        i_nex++;
    }

    double leng_wid = 0;
    double v1[ 3 ], v2[ 3 ];
    for ( uint j = i_cur + 1; j < i_nex; j++ )
    {
        if ( ( j >= pl.size() ) || ( i_cur >= pl.size() ) )
            break;
        //假设i_cur点为A,j点为B i_nex点为C
        //向量AB
        v1[ 0 ] = pl[ j ].x - pl[ i_cur ].x;
        v1[ 1 ] = pl[ j ].y - pl[ i_cur ].y;
        v1[ 2 ] = pl[ j ].z - pl[ i_cur ].z;
        //向量AB叉乘向量AC
        v2[ 0 ] = v1[ 1 ] * vz - vy * v1[ 2 ];
        v2[ 1 ] = v1[ 2 ] * vx - v1[ 0 ] * vz;
        v2[ 2 ] = v1[ 0 ] * vy - vx * v1[ 1 ];

        //物理意义是组成的ABC组成的平行四边形面积的平方(为|AC|*h,其中为B到线AC的距离)
        double lw = v2[ 0 ] * v2[ 0 ] + v2[ 1 ] * v2[ 1 ] + v2[ 2 ] * v2[ 2 ];
        if ( lw > leng_wid )
        {
            //寻找最大面积的平方,也就是寻找距离AC最远的B
            leng_wid = lw;
        }
    }

    //|AC|*|AC|/(|AC|*|AC|*h*h)<225
    //也就是h>1/15 B点到AC的距离要大于0.06667m
    //太近了不好拟合一个平面
    if ( ( two_dis * two_dis / leng_wid ) < p2l_ratio )
    {
        //太近了法向量直接设置为0
        curr_direct.setZero();
        return 0;
    }

    //把两点之间的距离,按从大到小排个顺序
    uint disarrsize = disarr.size();
    for ( uint j = 0; j < disarrsize - 1; j++ )
    {
        for ( uint k = j + 1; k < disarrsize; k++ )
        {
            if ( disarr[ j ] < disarr[ k ] )
            {
                leng_wid = disarr[ j ];
                disarr[ j ] = disarr[ k ];
                disarr[ k ] = leng_wid;
            }
        }
    }

    //如果两点间的距离太近了
    if ( disarr[ disarr.size() - 2 ] < 1e-16 )
    {
        //太近了法向量直接设置为0
        curr_direct.setZero();
        return 0;
    }

    if ( lidar_type == MID || lidar_type == HORIZON )
    {
        //点与点之间距离变化太大的时候,可能与激光束是平行的,就也舍弃了
        double dismax_mid = disarr[ 0 ] / disarr[ disarrsize / 2 ];
        double dismid_min = disarr[ disarrsize / 2 ] / disarr[ disarrsize - 2 ];

        if ( dismax_mid >= limit_maxmid || dismid_min >= limit_midmin )
        {
            //太近或太远,法向量直接设置为0
            curr_direct.setZero();
            return 0;
        }
    }
    else
    {
        double dismax_min = disarr[ 0 ] / disarr[ disarrsize - 2 ];
        if ( dismax_min >= limit_maxmin )
        {
            curr_direct.setZero();
            return 0;
        }
    }

    curr_direct << vx, vy, vz;
    curr_direct.normalize();    //法向量归一化
    return 1;
}

边界特征主要是为了防止下面的两种情况所加入的判断。在LOAM中也有体现,在A-LOAM代码中没有体现异常点剔除,FAST-LIO2中进行了异常点剔除的判断。

LOAM论文:如图所示(a)实线段代表局部表面块。点A位于与激光束有一定角度的表面上(橙色虚线段)。点B位于与激光束大致平行的表面上。我们认为B是一个不可靠的特征点,不选择它作为特征点。(b)实线段是激光可观测的物体。点A在被遮挡区域的边界上(橙色虚线段),可以检测到边缘点。然而,如果从不同的角度观察,被遮挡的区域可以改变并变得可观察到。我们不把A作为显著边点,也不选择A可靠的特征点。

/*
* 边缘点判断,也就是LOAM论文中的进行异常点剔除
* 条件1:当前点前两个点不能在盲区
* 条件2:当前点后两个点不能在盲区
*/
bool edge_jump_judge( const pcl::PointCloud< PointType > &pl, vector< orgtype > &types, uint i, Surround nor_dir )
{
    if ( nor_dir == 0 )
    {
        //前两个点不能在盲区
        if ( types[ i - 1 ].range < blind || types[ i - 2 ].range < blind )
        {
            return false;
        }
    }
    else if ( nor_dir == 1 )
    {
        //后两个点不能在盲区
        if ( types[ i + 1 ].range < blind || types[ i + 2 ].range < blind )
        {
            return false;
        }
    }

    // 下面分别对(i-2,i-1)和(i,i+1)两种情况时点与点间距进行了判断
    double d1 = types[ i + nor_dir - 1 ].dista;
    double d2 = types[ i + 3 * nor_dir - 2 ].dista;
    double d;

    if ( d1 < d2 )
    {
        //将大小间距进行调换,大在前小在后
        d = d1;
        d1 = d2;
        d2 = d;
    }

    d1 = sqrt( d1 );
    d2 = sqrt( d2 );

    if ( d1 > edgea * d2 || ( d1 - d2 ) > edgeb )
    {
        //假如间距太大 可能是被遮挡,就不把它当作边缘点
        return false;
    }

    return true;
}

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/963016.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

在windows上配置ninja环境

ninja使用并行任务来编译工程&#xff0c;比cmake编译快了一个数量级&#xff0c;是谷歌在2010年为了提高cmake的编译速度而开发一款编译工具。下面介绍在windows上配置ninja环境。 1 下载ninja ninja官网地址&#xff1a; https://github.com/ninja-build/ninja/releases   …

【OpenCV入门】第七部分——图像的几何变换

文章结构 缩放dsize参数实现缩放fx参数和fy参数实现缩放 翻转仿射变换平移旋转倾斜 透视cmath模块 缩放 通过resize()方法可以随意更改图像的大小比例&#xff1a; dst cv2.resize(src, dsize, fx, fy, interpolation)src&#xff1a; 原始图像dsize&#xff1a; 输出图像的…

Leetcode---360周赛

题目列表 2833. 距离原点最远的点 2834. 找出美丽数组的最小和 2835. 使子序列的和等于目标的最少操作次数 2836. 在传球游戏中最大化函数值 一、距离原点最远的点 这题主要是理解题意&#xff0c;遇到L往左走&#xff0c;遇到R往右走&#xff0c;遇到_左右都可以走&#x…

bazel外部依赖管理

前面我们主要分析了基于 Action 的增量构建&#xff0c;缓存和远程执行机制。现在让我们看看 Bazel 是如何管理外部依赖的。 大部分项目都没法避免引入第三方的依赖项。构建系统通常提供了下载第三方依赖的能力。为了避免重复下载&#xff0c;Bazel 要求在声明外部依赖的时候&…

美客多(mercadolibre)测评下单技术(养号环境搭建详解)

MercadoLibre&#xff08;美客多&#xff09;是拉丁美洲的一个网购平台。该公司为其客户提供电子商务交易的购买&#xff0c;出售&#xff0c;支付和收集机制。目前全球第十大电商市场——巴西是MercadoLibre的主要市场&#xff0c;占据近60%的平台营收&#xff0c;接着是阿根廷…

关于购买AirPods,现在是否为最佳时机?

我们不需要解释你为什么想要AirPods。苹果对真正的无线耳机的采用彻底改变了市场&#xff0c;并从那时起大量销售。你总是在记者、同事和名人的耳朵里看到它们——尤其是在我们这个远程工作和Zoom会议的时代。 真正的问题是&#xff0c;你应该现在就买一个&#xff0c;还是在几…

2024年java面试--多线程(1)

系列文章目录 2024年java面试&#xff08;一&#xff09;–spring篇2024年java面试&#xff08;二&#xff09;–spring篇2024年java面试&#xff08;三&#xff09;–spring篇2024年java面试&#xff08;四&#xff09;–spring篇 文章目录 系列文章目录线程调度线程五种状态…

spring boot项目生成容器并运行

一个安静的周末&#xff0c;shigen又睡懒觉了&#xff0c;上次说的拖延症的惩罚来了&#xff1a;早晚各100个健腹轮练习&#xff0c;早上的已经完成了。今天的文章来的有点晚&#xff0c;但是依旧保持质量。 springboot项目生成容器并运行 背景 将springboot项目打包成jar包&…

2021年03月 C/C++(六级)真题解析#中国电子学会#全国青少年软件编程等级考试

第1题&#xff1a;生日相同 2.0 在一个有180人的大班级中&#xff0c;存在两个人生日相同的概率非常大&#xff0c;现给出每个学生的名字&#xff0c;出生月日。试找出所有生日相同的学生。 时间限制&#xff1a;1000 内存限制&#xff1a;65536 输入 第一行为整数n&#xff0c…

Linux系统中u-boot启动流程分析(详解)

大家好&#xff0c;我是ST小智&#xff0c;今天给大家分享一下&#xff0c;u-boot的启动流程。 今天给大家全面的分析一下u-boot启动流程。整理这篇文章花费时间较长&#xff0c;中间很长时间未更新&#xff0c;希望这篇文章对大家有所帮助。 本章主要是详细的分析一下uboot的…

框架分析(8)-React Native

框架分析&#xff08;8&#xff09;-React Native 专栏介绍React Native特性和优势跨平台开发&#xff1a;热更新原生性能组件化开发第三方库支持社区支持 限制和挑战性能问题第三方库兼容性学习曲线 总结 专栏介绍 link 主要对目前市面上常见的框架进行分析和总结&#xff0c…

Node基础and包管理工具

Node基础 fs 模块 fs 全称为 file system&#xff0c;称之为 文件系统&#xff0c;是 Node.js 中的 内置模块&#xff0c;可以对计算机中的磁盘进行操作。 本章节会介绍如下几个操作&#xff1a; 1. 文件写入 2. 文件读取 3. 文件移动与重命名 4. 文件删除 5. 文件夹操作 6. …

安圭拉变成AI领域的数字金矿?

这个小小的岛国今年的域名销售额可能达到其GDP的10%&#xff01; 安圭拉a小小的英国岛屿领土在加勒比海地区&#xff0c;由于其“可再生能源”&#xff0c;今年可能带来高达3000万美元的收入。ai”域名&#xff0c;报告彭博在周四发表的一篇文章中说。在过去的一年里&#xff0…

应届生面试指南:如何在缺乏经验的情况下脱颖而出

&#x1f337;&#x1f341; 博主猫头虎&#xff08;&#x1f405;&#x1f43e;&#xff09;带您 Go to New World✨&#x1f341; &#x1f984; 博客首页——&#x1f405;&#x1f43e;猫头虎的博客&#x1f390; &#x1f433; 《面试题大全专栏》 &#x1f995; 文章图文…

IMU姿态计算

总述 IMU即惯性测量单元&#xff0c;主要用于对机体的加速度与角速度的测算&#xff0c;使用场景很多&#xff0c;例如: 平衡车、惯性导航等等 姿态 姿态角&#xff08;Euler angles&#xff09;是用于描述物体在三维空间中的旋转姿态的一种表示方法。它由三个角度组成&…

微机原理 || 第3次测试:第八章-常用接口芯片82558253(测试题+手写解析)

&#xff08;一&#xff09;知识点总结 一直没有学明白8253和8255芯片&#xff0c;觉得后面难&#xff0c;其实看懂后&#xff0c;就是照着表格去对应填写&#xff0c; 知道地址怎么回事就没问题哒~ 相信你&#x1f618; 一、8255芯片&#xff08;不是偷懒&#xff0c;真的就…

PMD代码检查:避免抛出原始的异常类型(AvoidThrowingRawExceptionTypes)

https://docs.pmd-code.org/pmd-doc-6.55.0/pmd_rules_java_design.html#avoidthrowingrawexceptiontypes 避免抛出RuntimeException、Throwable、Exception、Error这样原始的异常类型&#xff0c;而应该抛出它们的子类。 例如下面的使用方法PMD检查报违反项&#xff1a; 在…

动态规划之连续乘积最大子数组 连续和最大子数组

一. 连续和最大子数组 给你一个整数数组 nums &#xff0c;请你找出一个具有最大和的连续子数组&#xff08;子数组最少包含一个元素&#xff09;&#xff0c;返回其最大和。 子数组 是数组中的一个连续部分。 示例 1&#xff1a; 输入&#xff1a;nums [-2,1,-3,4,-1,2,1,-5,…

23062day6

作业&#xff1a;将dict.txt导入到数据库中。 方法1&#xff1a;创建shell脚本&#xff0c; 调用指令创建数据库和表格&#xff0c;使用循环在循环中用数组存储dict.txt的内容并插入表格中。 方法2&#xff1a;在终端创建数据库和表格&#xff0c;将dict.txt中的内容手动输入…

OB Cloud 初体验⎮用户之声

OceanBase 作为国内一款优秀的分布式数据库&#xff0c;这些年来发展很快&#xff0c;在金融、电商、政企、物流等行业得到广泛应用。之前多为私有化部署&#xff0c;去年也推出云版本并于最近进行重大升级。受朋友之约&#xff0c;对其云产品做了个简单测试。因时间所限&#…