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

news/2024/12/24 0:46:58/

目录

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, // 0Nr_180,  // 180Nr_inf,  // 无穷大 跳变较远?Nr_blind // 在盲区?
};
// orgtype类:用于存储激光雷达点的一些其他属性
struct orgtype
{double range; // 点云在xy平面离雷达中心的距离double dista; // 当前点与后一个点之间的距离//假设雷达原点为O 前一个点为M 当前点为A 后一个点为Ndouble 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 : TODOsub_points = n.subscribe( "/velodyne_points", 1000, velo16_handler, ros::TransportHints().tcpNoDelay() );break;case OUST64:printf( "OUST64\n" );   // enum OUST64 = 3 : TODOsub_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的plpcl::PointCloud< PointType > pl_corn, pl_surf;  // 保存提取的角点和平面点vector< orgtype >            types; // 自定义的结构提 : TODOuint                         plsize = pl.size() - 1;    // Lidar点数量pl_corn.reserve( plsize );  // 预留sizepl_surf.reserve( plsize );  // 预留sizetypes.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].xhead++; // 因此猜测这里判断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代表上个状态为平面 否则为0int  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;}//更新i2i2 = i;//求得平面,并返回类型0 1 2plane_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.2if ( 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.2double 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 ( ;; ){//索引超出所有点的个数直接breakif ( ( 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点太远了就直接breakif ( 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//向量ABv1[ 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叉乘向量ACv2[ 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最远的Bleng_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 ){//太近了法向量直接设置为0curr_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 ){//太近了法向量直接设置为0curr_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 ){//太近或太远,法向量直接设置为0curr_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.ppmy.cn/news/1080712.html

相关文章

JavaScript安全性最佳实践:如何保护你的Web应用程序

在如今的数字时代&#xff0c;网页应用程序的安全性至关重要。恶意攻击者不断寻找机会来入侵你的应用程序。为了帮助你保护你的网页应用程序&#xff0c;我们将介绍一些关键的JS安全性知识点&#xff0c;并提供示例代码来演示如何实施它们。 1. 跨站点脚本 XSS攻击是一种常见…

Qt树形控件QTreeView使用——复选框的设置(setCheckable)(setTristate)(关联根的三态子节点两态)

参考代码 关于子节点 : 选中 /不选中 会影响 根节点的状态 关于根节点: 全选/部分全选/不选 会影响子节点的状态 根节点有三态setTristate ,qt5.6版本建议舍去,可通过判断item是否存在父节点判断当前点击是否是根节点,变相的判断是否是三态 demo下载地址&#xff1a;https://…

天地图pc端显示正常,app不能显示

问题&#xff1a;天地图在pc端显示正常&#xff0c;但是app端不能显示 原因&#xff1a;天地图官网区分服务端key和客户端key 更换一下key就可以了 app端需要申请服务端key, pc端和h5使用客户端key

Bert和LSTM:情绪分类中的表现

一、说明 这篇文章的目的是评估和比较 2 种深度学习算法&#xff08;BERT 和 LSTM&#xff09;在情感分析中进行二元分类的性能。评估将侧重于两个关键指标&#xff1a;准确性&#xff08;衡量整体分类性能&#xff09;和训练时间&#xff08;评估每种算法的效率&#xff09;。…

Vue组件之间进行传值的两种方式

在 Vue 中&#xff0c;组件之间传值是一个常见的操作&#xff0c;通常有两种方式来传递数据&#xff1a;props 和事件&#xff08;Event Bus&#xff09;。下面我将详细介绍这两种方式。 1. 通过 Props 传递数据 Props 是一种用于从父组件向子组件传递数据的方式。在子组件中…

ZMTP协议

ZoreMQ Transport Protocol是一个传输层协议&#xff0c;用于ZMQ的连接的信息交互&#xff0c;本文档描述的是3.0协议&#xff0c;主要分析基于NULL Security Mechanism 协议语法 ZMTP由三部分组成&#xff0c;分别是 greeting、handshake、traffic 部分描述构成greeting描述…

滑动窗口实例3(最大连续1的个数Ⅲ)

题目&#xff1a; 给定一个二进制数组 nums 和一个整数 k&#xff0c;如果可以翻转最多 k 个 0 &#xff0c;则返回 数组中连续 1 的最大个数 。 示例 1&#xff1a; 输入&#xff1a;nums [1,1,1,0,0,0,1,1,1,1,0], K 2 输出&#xff1a;6 解释&#xff1a;[1,1,1,0,0,1,1…

01_es安装和入门体验

01_es安装和入门体验 概述Elasticsearch 是什么全文搜索引擎 1. 安装环境介绍单机 & 集群集群 Cluster节点 Node 1.1 linux 单机安装1.2 集群安装1.3 window下安装 2. postMan 体验3.java客户端体验3.1 资源链接和关闭3.2 索引相关操作3.3 文档基本操作3.4 查询相关操作 概…