Table of contents
1 Introduction to LiDAR_front_end.cpp
2 LiDAR_front_end.cpp program analysis
1 Introduction to LiDAR_front_end.cpp
The laser point cloud first extracts feature points in the LiDAR_front_end node, and sends the processed information through the /laser_cloud_flatcompletion node. Like FAST-LIO2, R3LIVE only uses surface features as ESIKF fusion.
- First, the main function will run different callback functions according to different radar types
- Then extract corner points and face points through give_feature()
- When extracting noodles, judge the type of noodles by plane_judge()
- During the extraction process, the edge point judgment will be carried out, that is, the abnormal point removal in the LOAM paper
- Finally, publish the extracted corner points and face points.
2 LiDAR_front_end.cpp program analysis
This part of the content is similar to that of FAST-LIO2. First, a series of enum information is defined at the top. Different lidar types, feature point characteristics, etc. are selected through enum, and some other attributes of some lidar points are stored through 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;
}
};Then look at the main function of the file:
- First, the ros node will be initialized, and then the parameters of the configuration file will be obtained
- For simplicity, set some global variables that will be used
- Subscription settings for lidar data will be made later based on the value of lidar_type
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;
}The article takes the livox radar as an example and enters the mid_handler callback function. The mid_handler callback function receives /livox/lidar original data, converts the data type in the callback function (/laser_cloud), and extracts the corner/face features and publishes them (laser_cloud_sharp/laser_cloud_flat)
- First, the format conversion of the message type will be performed, and the msg in the PointCloud2 format will be converted into the PointXYZINormal format.
- Then traverse each point cloud, save the point cloud as a custom vector<orgtype> format, and set the range information of the last point.
- Then extract corner points and face points
- Finally, publish the extracted corner points and face points.
/**
* @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 );
}The give_feature() function is mainly for feature extraction. Next, we will learn and understand this function. For the point cloud extraction feature of each 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;
}
}
}By judging the surface point type with plane_judge(), the surface feature is mainly fitted into a vector based on the distance between points, and the surface feature extraction is calculated through the product. The surface feature extraction and edge feature extraction here are Similar to LOAM.
LOAM feature extraction method: LOAM proposes a simple and efficient feature point extraction method: feature points are extracted based on the curvature of point cloud points. That is, particularly sharp edge points and particularly flat plane points are used as feature points. The formula looks complicated. In fact, it is to take 5 points on the left and right sides of the target point on the same scanning line, and make differences with the coordinates of the target point respectively. The result is the curvature of the target point. When the target point is at an edge or corner, the difference between it and the surrounding points is naturally larger, and the resulting curvature is larger; conversely, when the target point is on a plane, the coordinates of the surrounding points and the target point are close, and the resulting curvature is natural. smaller.
/*
* 平面类型判断
*/
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;
}The boundary feature is mainly a judgment added to prevent the following two situations. It is also reflected in LOAM. The outlier elimination is not reflected in the A-LOAM code. The judgment of outlier elimination is made in FAST-LIO2.
LOAM paper: As shown in the figure (a) the solid line segment represents the local surface patch. Point A is located on the surface at an angle to the laser beam (orange dashed line segment). Point B is located on a surface approximately parallel to the laser beam. We believe that B is an unreliable feature point and do not select it as a feature point. (b) The solid line segment is a laser-observable object. Point A is on the boundary of the occluded area (orange dotted line segment), and the edge point can be detected. However, occluded areas can change and become observable if viewed from different angles. We do not regard A as a significant edge point, nor do we select reliable feature points of 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;
}