该博客详细介绍了基于Livox激光雷达的点云处理流程，包括特征提取、点云分割、花瓣划分、点类型标记（角点、平面点）以及发布特征点云。首先，通过ROS节点接收点云数据，然后利用Laser_feature类进行特征提取，根据点云的曲率和视角判断点类型。接着，通过projection_scan_3d_2d函数将点云划分为多个花瓣，并去除无效点。之后，计算每个花瓣的特征，如角点、平面点，并进行体素滤波。最后，将处理后的特征点云发布到ROS话题。整个过程涉及点云的预处理、特征计算和发布，旨在提高点云数据的可用性和准确性。
一 特征提取
1、laser_feature_extractor.cpp
1、主函数构造scanRegistration节点，调用Laser_feature类中的laser_feature函数
ros::init( argc, argv, "scanRegistration" ); Laser_feature laser_feature;
2、laser_feature_extractor.hpp
class Laser_feature的定义，主要包含一下几个函数
-
get_ros_paramter 获取ROS参数
-
int int_ros_env() 相当于主函数
-
~Laser_feature() 析构函数，无实际作用
-
Laser_feature() 调用了int_ros_env() 函数
-
void removeClosedPointCloud 去除距离激光雷达近的点
-
void laserCloudHandler 特征提取的重要函数，回调函数
-
void init_livox_lidar_para 初始化雷达参数
1、进入init_ros_env()函数
1）调用init_livox_lidar_para判断雷达类型，对m_lidar_type 进行赋值；
2）调用get_ros_parameter函数，获取雷达的基本参数
double livox_corners, livox_surface, minimum_view_angle; get_ros_parameter<double>(nh, "feature_extraction/corner_curvature", livox_corners, 0.05 ); get_ros_parameter<double>(nh, "feature_extraction/surface_curvature", livox_surface, 0.01 ); get_ros_parameter<double>(nh, "feature_extraction/minimum_view_angle", minimum_view_angle, 10 ); m_livox.thr_corner_curvature = livox_corners;//角点曲率 m_livox.thr_surface_curvature = livox_surface;//面点曲率 m_livox.minimum_view_angle = minimum_view_angle;
3）调用回调函数laserCloudHandler
for(int i = 0 ; i< m_maximum_input_lidar_pointcloud; i++)
{ m_input_lidar_topic_name_vec.push_back(string("laser_points_").append(std::to_string(i)));
m_sub_input_laser_cloud_vec.push_back( nh.subscribe<sensor_msgs::PointCloud2>( m_input_lidar_topic_name_vec.back(), 10000, boost::bind( &Laser_feature::laserCloudHandler, this, _1, m_input_lidar_topic_name_vec.back()) ) );
m_map_pointcloud_full_vec_vec[i].resize(m_piecewise_number);
m_map_pointcloud_surface_vec_vec[i].resize(m_piecewise_number);
m_map_pointcloud_corner_vec_vec[i].resize(m_piecewise_number);
}
4）订阅节点信息进行发布
m_pub_pc_livox_corners = nh.advertise<sensor_msgs::PointCloud2>( "/pc2_corners", 10000 ); m_pub_pc_livox_surface = nh.advertise<sensor_msgs::PointCloud2>( "/pc2_surface", 10000 ); m_pub_pc_livox_full = nh.advertise<sensor_msgs::PointCloud2>( "/pc2_full", 10000 );
5）对角点面点进行体素滤波？？？
m_voxel_filter_for_surface.setLeafSize( m_plane_resolution / 2, m_plane_resolution / 2, m_plane_resolution / 2 ); m_voxel_filter_for_corner.setLeafSize( m_line_resolution, m_line_resolution, m_line_resolution );
2、进入回调函数laserCloudHandler
1）将ros消息转换为PCL点云 存放在laserCloudIn中
//定义一个pcl形式的输入点云 pcl::PointCloud<pcl::PointXYZI> laserCloudIn; //把ros包的点云信息转化为pcl形式 pcl::fromROSMsg( *laserCloudMsg, laserCloudIn );
2）对点云进行特征提取
a) 调用livox_feature_extractor中的extract_laser_features函数，输入的是pcl点云和ros时间戳
laserCloudScans = m_livox.extract_laser_features( laserCloudIn, laserCloudMsg->header.stamp.toSec() );
3、livox_feature_extractor.hpp
class Livox laser的定义，主要包含一下几个函数
-
void get_features
-
void set_intensity
-
cv::Mat draw_dbg_img
-
void add_mask_of_point
-
void eval_point
-
void compute_features()
-
int projection_scan_3d_2d
-
void reorder_laser_cloud_scan
-
void split_laser_scan
-
std::vector< pcl::PointCloud< pcl::PointXYZI > > extract_laser_features
1、进入extract_laser_features函数
1）按点云延申的拐点分为若干个花瓣，调用projection_scan_3d_2d函数
int clutter_size = projection_scan_3d_2d( laserCloudIn, scan_id_index );
2）计算平面点、角点，调用compute_features()
compute_features();
if ( clutter_size == 0 )
{
return laserCloudScans;
}
else
{
split_laser_scan( clutter_size, laserCloudIn, scan_id_index, laserCloudScans );
return laserCloudScans;
}
2、在1中调用了projection_scan_3d_2d函数
1）循环遍历点云，将点云转换为结构体Pt_info形式
//循环遍历点云
for ( unsigned int idx = 0; idx < pts_size; idx++ )
{
m_raw_pts_vec[ idx ] = laserCloudIn.points[ idx ];
//将点云转换为结构体的形式pt_info
Pt_infos *pt_info = &m_pts_info_vec[ idx ];
m_map_pt_idx.insert( std::make_pair( laserCloudIn.points[ idx ], pt_info ) ); //insert在指定位置前插入元素 make_pair主要的作用是将两个数据组合成一个数据，两个数据可以是同一类型或者不同类型
pt_info->raw_intensity = laserCloudIn.points[ idx ].intensity; //记录反射强度
pt_info->idx = idx;
//按下标分配微小的时间戳差距！由于时间戳是扫描到第一个点的时刻，因此所有点的时间戳会有细微不同
pt_info->time_stamp = m_current_time + ( ( float ) idx ) * m_time_internal_pts; //10us
m_last_maximum_time_stamp = pt_info->time_stamp;
m_input_points_size++;
这里值得注意的是，一帧点云并不是同时获得的，由于时间戳是扫描到第一个点的时刻，因此所有点的时间戳会有细微不同，按下标分配时间戳，一帧10ms,10000个点，每个点的时间为10μs。因此m_time_internal_pts=10e-5s.
Pt_info，点的各种信息
struct Pt_infos
{
int pt_type = e_pt_normal;
int pt_label = e_label_unlabeled;
int idx = 0.f;
float raw_intensity = 0.f;
float time_stamp = 0.0;
float polar_angle = 0.f;
int polar_direction = 0;
float polar_dis_sq2 = 0.f;
float depth_sq2 = 0.f;
float curvature = 0.0;
float view_angle = 0.0;
float sigma = 0.0;
Eigen::Matrix< float, 2, 1 > pt_2d_img; // project to X==1 plane
};
2)判断nan点和距离过近的点
//isFinite() 函数用于检查其参数是否是无穷大。如果 number 是 NaN（非数字），或者是正、负无穷大的数，则返回 false。
if ( !std::isfinite( laserCloudIn.points[ idx ].x ) ||
!std::isfinite( laserCloudIn.points[ idx ].y ) ||
!std::isfinite( laserCloudIn.points[ idx ].z ) )
{
add_mask_of_point( pt_info, e_pt_nan );
continue;
}
//距离过近的点
if ( laserCloudIn.points[ idx ].x == 0 )
{
if ( idx == 0 )
{
// TODO: handle this case.
screen_out << "First point should be normal!!!" << std::endl;
pt_info->pt_2d_img << 0.01, 0.01;
pt_info->polar_dis_sq2 = 0.0001;
add_mask_of_point( pt_info, e_pt_000 );
}
else
{
pt_info->pt_2d_img = m_pts_info_vec[ idx - 1 ].pt_2d_img;
pt_info->polar_dis_sq2 = m_pts_info_vec[ idx - 1 ].polar_dis_sq2;
add_mask_of_point( pt_info, e_pt_000 );
continue;
}
}
扫描的第一个点必须是正常点，如果不是，对其的pt_2d_img，polar_dis_sq2进行赋值//这里的两个变量存储的为什么值
3）对结构体的一些变量进行赋值
m_map_pt_idx.insert( std::make_pair( laserCloudIn.points[ idx ], pt_info ) ); //求解每个点的深度 pt_info->depth_sq2 = depth2_xyz( laserCloudIn.points[ idx ].x, laserCloudIn.points[ idx ].y, laserCloudIn.points[ idx ].z ); //距离 //投影到像素点的坐标 pt_info->pt_2d_img << laserCloudIn.points[ idx ].y / laserCloudIn.points[ idx ].x, laserCloudIn.points[ idx ].z / laserCloudIn.points[ idx ].x; //投影至像素点的坐标 x=1的平面 //投影的像素点距离投影中心点的距离 pt_info->polar_dis_sq2 = dis2_xy( pt_info->pt_2d_img( 0 ), pt_info->pt_2d_img( 1 ) ); eval_point( pt_info ); //估计强度，在投影平面上，距中心点越远的点强度小的可能性越大
4)定义点类型，通过add_mask_of_point函数
add_mask_of_point( pt_info, e_pt_nan );
add_mask_of_point( pt_info, e_pt_000 );
// 点位于视场角边界上//超出视角可信范围点 //求解投影平面的边缘点
if ( pt_info->polar_dis_sq2 > m_max_edge_polar_pos )//m_max_edge_polar_pos=0.1104跟传感器的最大视角有关
{
add_mask_of_point( pt_info, e_pt_circle_edge, 2 );
}
5）分割scans，确定花瓣的零点和拐点
a) 在投影平面上，判断本个点与上个点的延伸方向
// Split scans
if ( idx >= 1 )
{
//在投影平面上，本个点与上个点的延伸方向
float dis_incre = pt_info->polar_dis_sq2 - m_pts_info_vec[ idx - 1 ].polar_dis_sq2;
if ( dis_incre > 0 ) // far away from zero
{
pt_info->polar_direction = 1;
}
if ( dis_incre < 0 ) // move toward zero
{
pt_info->polar_direction = -1;
}
b) 确定拐点和零点，本个点向中心点延伸，上个点向外延伸则为拐点edge_idx；反之为零点zero_idx，当前点与下一个split点相隔50个点；split_idx保存的是所有的分割点
//本个点向中心点延伸，上个点向外延伸
if ( pt_info->polar_direction == -1 && m_pts_info_vec[ idx - 1 ].polar_direction == 1 )
{
if ( edge_idx.size() == 0 || ( idx - split_idx[ split_idx.size() - 1 ] ) > 50 )
{
split_idx.push_back( idx ); //这就是花瓣拐角处，每两个拐角确定一个花瓣
edge_idx.push_back( idx );
continue;
}
}
if ( pt_info->polar_direction == 1 && m_pts_info_vec[ idx - 1 ].polar_direction == -1 )
{
if ( zero_idx.size() == 0 || ( idx - split_idx[ split_idx.size() - 1 ] ) > 50 )
{
split_idx.push_back( idx );
zero_idx.push_back( idx );
continue;
}
}
}
split_idx.push_back(pts_size - 1); // 记录最后一次回调中点数的最后一个槽位
c)至少要有3个花瓣，6个分割点
if (split_idx.size() < 6) return 0;
6）再次遍历所有点云，按花瓣处理
for (int idx = 0; idx < (int)pts_size; idx++)
{
if (val_index < split_idx.size() - 2) //split_idx.push_back(pts_size - 1) split_idx里面最后存的是最有一次回调的点数，故实际split_idx的点数应为.size-1,所以最后一个点的idx为.size-2
{//val_index是split_idx的下标。 val_index++后， val_index+1指向了当前半花瓣的尽头， 保证了当前半花瓣后面的点云都在当前半花瓣内
if (idx == 0 || idx > split_idx[val_index + 1]) // if idx is the staring point or the beginning of a petal
{
if (idx > split_idx[val_index + 1]) // if idx is the beginning of a petal, then petal count++如果idx>边点索引，说明它在花瓣的起点
{
val_index++;
}
internal_size = split_idx[val_index + 1] - split_idx[val_index]; // number of points in this petal花瓣上的点数
if (m_pts_info_vec[split_idx[val_index + 1]].polar_dis_sq2 > 10000) // if the starting point of next petal's x^2+y^2+z^2 > 10000
{
pt_angle_index = split_idx[val_index + 1] - (int)(internal_size * 0.20); // TODO last 20% of the points in this petal
scan_angle = atan2(m_pts_info_vec[pt_angle_index].pt_2d_img(1), m_pts_info_vec[pt_angle_index].pt_2d_img(0)) * 57.3; // pt_2d_img(1):z/x,(0):y/x
scan_angle = scan_angle + 180.0;
}
else
{
pt_angle_index = split_idx[val_index + 1] - (int)(internal_size * 0.80); // TODO last 80% of the points in this petal
scan_angle = atan2(m_pts_info_vec[pt_angle_index].pt_2d_img(1), m_pts_info_vec[pt_angle_index].pt_2d_img(0)) * 57.3; // atan2(z/y)*57.3, angle of laser beam in degrees
scan_angle = scan_angle + 180.0;
}
}
}
m_pts_info_vec[idx].polar_angle = scan_angle; // keep in mind the above, the if conditions skip points, therefore most scan_angle will be 0
scan_id_index[idx] = scan_angle; // 只记录花瓣的起始点的扫描角度
}
7）返回分割的点数
return split_idx.size() - 1;
3、在1中调用了computer_feature函数，对点打特征标志
1) 遍历所有点云，不对开始和结束的两个点，过近点和nan点进行处理
unsigned int pts_size = m_raw_pts_vec.size();
size_t curvature_ssd_size = 2;
int critical_rm_point = e_pt_000 | e_pt_nan;
float neighbor_accumulate_xyz[ 3 ] = { 0.0, 0.0, 0.0 };
//再次遍历所有点云
for ( size_t idx = curvature_ssd_size; idx < pts_size - curvature_ssd_size; idx++ )
{
//忽略过近与nan点,yuyunsuan
if ( m_pts_info_vec[ idx ].pt_type & critical_rm_point )
{
continue;
}
2）计算曲率，与loam相似，查找曲线上前后2个点，共5个点
a) 判断是接近0点的点，nan点和无效点
for (size_t i = 1; i <= curvature_ssd_size; i++)
{
if ((m_pts_info_vec[idx + i].pt_type & e_pt_000) || (m_pts_info_vec[idx - i].pt_type & e_pt_000)) // 如果左或右邻近点为空
{
if (i == 1)
{
m_pts_info_vec[idx].pt_label |= e_label_near_zero;
}
else
{
m_pts_info_vec[idx].pt_label = e_label_invalid;
}
break;
}
else if ((m_pts_info_vec[idx + i].pt_type & e_pt_nan) || (m_pts_info_vec[idx - i].pt_type & e_pt_nan)) // 如果左或右邻近点为空 inf
{
if (i == 1)
{
m_pts_info_vec[idx].pt_label |= e_label_near_nan;
}
else
{
m_pts_info_vec[idx].pt_label = e_label_invalid;
}
break;
}
else // 太远或太近
{
neighbor_accumulate_xyz[0] += m_raw_pts_vec[idx + i].x + m_raw_pts_vec[idx - i].x;
neighbor_accumulate_xyz[1] += m_raw_pts_vec[idx + i].y + m_raw_pts_vec[idx - i].y;
neighbor_accumulate_xyz[2] += m_raw_pts_vec[idx + i].z + m_raw_pts_vec[idx - i].z;
}
}
neighbor_accumulate_xyz[0] -= curvature_ssd_size * 2 * m_raw_pts_vec[idx].x;
neighbor_accumulate_xyz[1] -= curvature_ssd_size * 2 * m_raw_pts_vec[idx].y;
neighbor_accumulate_xyz[2] -= curvature_ssd_size * 2 * m_raw_pts_vec[idx].z;
//曲率
m_pts_info_vec[idx].curvature = neighbor_accumulate_xyz[0] * neighbor_accumulate_xyz[0] + neighbor_accumulate_xyz[1] * neighbor_accumulate_xyz[1] +
neighbor_accumulate_xyz[2] * neighbor_accumulate_xyz[2];
3）计算平面角
/*********** 计算平面角 ************/ Eigen::Matrix<float, 3, 1> vec_a(m_raw_pts_vec[idx].x, m_raw_pts_vec[idx].y, m_raw_pts_vec[idx].z); Eigen::Matrix<float, 3, 1> vec_b(m_raw_pts_vec[idx + curvature_ssd_size].x - m_raw_pts_vec[idx - curvature_ssd_size].x,m_raw_pts_vec[idx + curvature_ssd_size].y - m_raw_pts_vec[idx - curvature_ssd_size].y, m_raw_pts_vec[idx + curvature_ssd_size].z - m_raw_pts_vec[idx - curvature_ssd_size].z); m_pts_info_vec[idx].view_angle = Eigen_math::vector_angle(vec_a, vec_b, 1) * 57.3; // 扫描平面和当前点之间的夹角，乘57.3转换为角度
Eigen_math::vector_angle？？？
4）去除小视角的点，标记平面点和边缘点
if (m_pts_info_vec[idx].view_angle > minimum_view_angle) // 大于10°
{
if (m_pts_info_vec[idx].curvature < thr_surface_curvature) // 曲率小于0.01, 认为时平面点
{
m_pts_info_vec[idx].pt_label |= e_label_surface; // 标记为平面点
}
float sq2_diff = 0.1;
if (m_pts_info_vec[idx].curvature > thr_corner_curvature) // 曲率大于0.05，认为时边界点
{//判断会不会被隐藏
if (m_pts_info_vec[idx].depth_sq2 <= m_pts_info_vec[idx - curvature_ssd_size].depth_sq2 &&m_pts_info_vec[idx].depth_sq2 <= m_pts_info_vec[idx + curvature_ssd_size].depth_sq2)
{
if (abs(m_pts_info_vec[idx].depth_sq2 - m_pts_info_vec[idx - curvature_ssd_size].depth_sq2) < sq2_diff * m_pts_info_vec[idx].depth_sq2 ||
bs(m_pts_info_vec[idx].depth_sq2 - m_pts_info_vec[idx + curvature_ssd_size].depth_sq2) < sq2_diff * m_pts_info_vec[idx].depth_sq2)
m_pts_info_vec[idx].pt_label |= e_label_corner;
}
}
}
}
}
enum E_feature_type // if and only if normal point can be labeled
{
e_label_invalid = -1,
e_label_unlabeled = 0,
e_label_corner = 0x0001 << 0,
e_label_surface = 0x0001 << 1,
e_label_near_nan = 0x0001 << 2,
e_label_near_zero = 0x0001 << 3,
};
4、在1中调用了函数split_laser_scan函数,将一帧激光点分割成多束scan，并去除坏点，标记每个点在当帧的索引
void split_laser_scan( const int clutter_size, const pcl::PointCloud< T > &laserCloudIn, const std::vector< float > & scan_id_index, std::vector< pcl::PointCloud< PointType > > &laserCloudScans )
首先 clutter_size = projection_scan_3d_2d( laserCloudIn, scan_id_index ); 也就是端点（拐点和零点）的个数；
std::vector< std::vector< int > > pts_mask; //二维向量 laserCloudScans.resize( clutter_size ); pts_mask.resize( clutter_size ); //指定外层容量的大小 PointType point; int scan_idx = 0;
1)遍历点云，将在projection_scan_3d_2d()得到的scan_id_index转换成scan_idx表示，将相同角度的点放在一起
注意：scan_id_index[i]记录的是花瓣起始点的扫描角度
//遍历所有点云，将在projection_scan_3d_2d()得到的scan_id_index转换成scan_idx表示
//用scan_id_index来表示，同一个极角度值可能会有多个点，不好表示
for (unsigned int i = 0; i < laserCloudIn.size(); i++)
{
point = laserCloudIn.points[i];
if (i > 0 && ((scan_id_index[i]) != (scan_id_index[i - 1])))//不是起始点且临近点的极角不同
{
scan_idx = scan_idx + 1;//把scan_id_index转化为scan_id来表示
pts_mask[scan_idx].reserve(5000); // 不同型号Livox扫描仪的点数不一样
}
laserCloudScans[scan_idx].push_back(point);//将相同角度的点放在一起
pts_mask[scan_idx].push_back(m_pts_info_vec[i].pt_type);
}
2)遍历花瓣，即laserCloudScans.size()
a) 不对0点，过近点，nan点进行处理
laserCloudScans.resize(scan_idx); int remove_point_pt_type = e_pt_000 |e_pt_too_near |e_pt_nan; int scan_avail_num = 0; std::vector<pcl::PointCloud<PointType>> res_laser_cloud_scan;
b) 遍历每个花瓣，去除无效点
for (unsigned int i = 0; i < laserCloudScans.size(); i++)//遍历花瓣
{
scan_avail_num = 0;
pcl::PointCloud<PointType> laser_clour_per_scan;
for (unsigned int idx = 0; idx < laserCloudScans[i].size(); idx++)//遍历第i个点云中的点，去除无效点，调用set_intensity()
{
if ((pts_mask[i][idx] & remove_point_pt_type) == 0)
{
if (laserCloudScans[i].points[idx].x == 0)
{
screen_printf("Error!!! Mask = %d\r\n", pts_mask[i][idx]);
assert(laserCloudScans[i].points[idx].x != 0);
continue;
}
auto temp_pt = laserCloudScans[i].points[idx];
set_intensity(temp_pt, default_return_intensity_type); // rviz中显示用到的颜色
laser_clour_per_scan.points.push_back(temp_pt);
scan_avail_num++;
}
}
if(scan_avail_num)
{
res_laser_cloud_scan.push_back(laser_clour_per_scan);
}
}
laserCloudScans= res_laser_cloud_scan;
}
这里主要是对set_intensity(temp_pt, default_return_intensity_type)做了处理，
default_return_intensity_type = e_I_motion_blur；
void set_intensity( T &pt, const E_intensity_type &i_type = e_I_motion_blur )
{
Pt_infos *pt_info = find_pt_info( pt );
switch ( i_type )
{
case ( e_I_motion_blur ):
pt.intensity = ( ( float ) pt_info->idx ) / ( float ) m_input_points_size;
assert( pt.intensity <= 1.0 && pt.intensity >= 0.0 );
return;
}
laserCloudScans中的intensity发生了变化，是当前的索引占当前帧所有点的比例；
4、继续回到laser_feature_extractor.hpp中，上诉步骤3，得到了保存了每个花瓣的点云向量laserCloudScans
5、特征提取
1、至少有5个花瓣，记录每个花瓣开始和结束的索引
if ( laserCloudScans.size() <= 5 ) // less than 5 scan
{
return;
}
m_laser_scan_number = laserCloudScans.size() * 1.0;
scanStartInd.resize( m_laser_scan_number );
scanEndInd.resize( m_laser_scan_number );
std::fill( scanStartInd.begin(), scanStartInd.end(), 0 );
std::fill( scanEndInd.begin(), scanEndInd.end(), 0 );
2、针对livox的特征提取
int piece_wise = m_piecewise_number; // 将一个scan分成3段，以减少运动模糊
if(m_if_motion_deblur)
{
piece_wise = 1;
}
vector<float> piece_wise_start( piece_wise );
vector<float> piece_wise_end( piece_wise );
1）分段处理
// 将一个scan分段 scan为一帧数据
for (int i = 0; i < piece_wise; i++)
{
int start_scans, end_scans;
start_scans = int((m_laser_scan_number * (i)) / piece_wise); // （一个scan）的0,1/3,2/3开始，将一个scan分成3段
end_scans = int((m_laser_scan_number * (i + 1)) / piece_wise) - 1; // （一个scan）的1/3,2/3,3/3处的前一个点处结束
int end_idx = laserCloudScans[end_scans].size() - 1; // 当前段结束时半片花瓣的最后一点，索引都是从0开始，所以最后一点的索引号为所有点数量-1
//开始和结束的位置
piece_wise_start[i] = ((float)m_livox.find_pt_info(laserCloudScans[start_scans].points[0])->idx) / m_livox.m_pts_info_vec.size();
piece_wise_end[i] =((float)m_livox.find_pt_info(laserCloudScans[end_scans].points[end_idx])->idx) / m_livox.m_pts_info_vec.size();
}//在这是除以size得到小数， 然后在get_features里又乘回来
2）调用get_features函数 获得livox_corners（角点）, livox_surface（平面）, livox_full（好点）特征点
for (int i = 0; i < piece_wise; i++)
{
pcl::PointCloud<PointType>::Ptr livox_corners(new pcl::PointCloud<PointType>()),
livox_surface(new pcl::PointCloud<PointType>()),
livox_full(new pcl::PointCloud<PointType>());
m_livox.get_features(*livox_corners, *livox_surface, *livox_full, piece_wise_start[i], piece_wise_end[i]);//调用get_features()函数将获得的特征点分类
m_map_pointcloud_corner_vec_vec[current_lidar_index][i] = *livox_corners;
m_map_pointcloud_surface_vec_vec[current_lidar_index][i] = *livox_surface;
m_map_pointcloud_full_vec_vec[current_lidar_index][i] = *livox_full;
}
3）发布/pc2_corners、/pc2_surface、/pc2_full
// 发布特征点
for (int i = 0; i < piece_wise; i++)
{
ros::Time current_time = ros::Time::now();
pcl::PointCloud<PointType>::Ptr livox_corners(new pcl::PointCloud<PointType>()),
livox_surface(new pcl::PointCloud<PointType>()),
livox_full(new pcl::PointCloud<PointType>());
else // 只发布当前雷达的特征点
{
*livox_full = m_map_pointcloud_full_vec_vec[current_lidar_index][i];
*livox_surface = m_map_pointcloud_surface_vec_vec[current_lidar_index][i];
*livox_corners = m_map_pointcloud_corner_vec_vec[current_lidar_index][i];
}
//转换成ROS信息并发布/pc2_corners、/pc2_surface、/pc2_full
pcl::toROSMsg(*livox_full, temp_out_msg);
temp_out_msg.header.stamp = current_time;
temp_out_msg.header.frame_id = "camera_init";
m_pub_pc_livox_full.publish(temp_out_msg);
m_voxel_filter_for_surface.setInputCloud(livox_surface);
m_voxel_filter_for_surface.filter(*livox_surface);
pcl::toROSMsg(*livox_surface, temp_out_msg);
temp_out_msg.header.stamp = current_time;
temp_out_msg.header.frame_id = "camera_init";
m_pub_pc_livox_surface.publish(temp_out_msg);
m_voxel_filter_for_corner.setInputCloud(livox_corners);
m_voxel_filter_for_corner.filter(*livox_corners);
pcl::toROSMsg(*livox_corners, temp_out_msg);
temp_out_msg.header.stamp = current_time;
temp_out_msg.header.frame_id = "camera_init";
m_pub_pc_livox_corners.publish(temp_out_msg);
if (m_odom_mode == 0) // odometry模式时，只使用一段livox激光点云
{
break;
}
}
}
return;
6、在5中调用了get_features函数
void get_features(pcl::PointCloud<PointType> &pc_corners, pcl::PointCloud<PointType> &pc_surface, pcl::PointCloud<PointType> &pc_full_res, float minimum_blur = 0.0, float maximum_blur = 0.3)
{
int corner_num = 0;
int surface_num = 0;
int full_num = 0;
pc_corners.resize(m_pts_info_vec.size());
pc_surface.resize(m_pts_info_vec.size());
pc_full_res.resize(m_pts_info_vec.size());
float maximum_idx = maximum_blur * m_pts_info_vec.size();//终止点索引
float minimum_idx = minimum_blur * m_pts_info_vec.size();//开始点索引
int pt_critical_rm_mask = e_pt_000 | e_pt_nan | e_pt_too_near;
for (size_t i = 0; i < m_pts_info_vec.size(); i++)
{
if (m_pts_info_vec[i].idx > maximum_idx ||
m_pts_info_vec[i].idx < minimum_idx) //????
continue;
if ((m_pts_info_vec[i].pt_type & pt_critical_rm_mask) == 0)//不是这三种点
{
if (m_pts_info_vec[i].pt_label & e_label_corner) // 角点
{
if (m_pts_info_vec[i].pt_type != e_pt_normal)//判断是否是一般点
continue;
if (m_pts_info_vec[i].depth_sq2 < std::pow(30, 2))
{
pc_corners.points[corner_num] = m_raw_pts_vec[i];
pc_corners.points[corner_num].intensity = m_pts_info_vec[i].time_stamp;
//在concer和surface上intensity又变为了时间戳。 所以一共有3个intensity， 第一个是筛选点时的raw_intensity,
//第二个是在划分scan时的idx/sm_input_points_size， 第三个是区分conner与surface时的时间搓
corner_num++;
}
}
if (m_pts_info_vec[i].pt_label & e_label_surface) // 平面点
{
if (m_pts_info_vec[i].depth_sq2 < std::pow(1000, 2))
{
pc_surface.points[surface_num] = m_raw_pts_vec[i];
pc_surface.points[surface_num].intensity = float(m_pts_info_vec[i].time_stamp);
surface_num++;
}
}
}
pc_full_res.points[full_num] = m_raw_pts_vec[i];
pc_full_res.points[full_num].intensity = m_pts_info_vec[i].time_stamp;
full_num++;
}
pc_corners.resize(corner_num);
pc_surface.resize(surface_num);
pc_full_res.resize(full_num);
}
这里的float minimum_blur = 0.0, float maximum_blur = 0.3 是带入的piece_wise_start[i], piece_wise_end[i]，分段处理的占比
扫一扫
2829
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
