一、算法路径
https://github.com/Livox-SDK/livox_mapping
二、代码阅读
1、源文件：scanRegistration.cpp
2、对代码添加注解：
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
pcl::PointCloud<PointType> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
int cloudSize = laserCloudIn.points.size();
if(cloudSize > 32000) cloudSize = 32000;
int count = cloudSize;
PointType point;
pcl::PointCloud<PointType> Allpoints;
//剔除无解数据点，并做了一些没有用的计算（也暂不明白反射率为啥这样搞？）
for (int i = 0; i < cloudSize; i++) {
point.x = laserCloudIn.points[i].x;
point.y = laserCloudIn.points[i].y;
point.z = laserCloudIn.points[i].z;
//计算扫描角度
double theta = std::atan2(laserCloudIn.points[i].y,laserCloudIn.points[i].z) / M_PI * 180 + 180;
//根据扫描角度分成9份，换算出扫描线ID
scanID = std::floor(theta / 9);
float dis = point.x * point.x
+ point.y * point.y
+ point.z * point.z;
double dis2 = laserCloudIn.points[i].z * laserCloudIn.points[i].z + laserCloudIn.points[i].y * laserCloudIn.points[i].y;
//计算偏转角
double theta2 = std::asin(sqrt(dis2/dis)) / M_PI * 180;
//反射率转反射强度？？？
point.intensity = scanID+(laserCloudIn.points[i].intensity/10000);
//point.intensity = scanID+(double(i)/cloudSize);
if (!pcl_isfinite(point.x) ||
!pcl_isfinite(point.y) ||
!pcl_isfinite(point.z)) {
continue;
}
//剔除非法数据点
Allpoints.push_back(point);
}
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
*laserCloud += Allpoints;
cloudSize = laserCloud->size();
for (int i = 0; i < cloudSize; i++) {
CloudFeatureFlag[i] = 0;
}
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> laserCloudFull;
int debugnum1 = 0;
int debugnum2 = 0;
int debugnum3 = 0;
int debugnum4 = 0;
int debugnum5 = 0;
int count_num = 1;
bool left_surf_flag = false;
bool right_surf_flag = false;
Eigen::Vector3d surf_vector_current(0,0,0);
Eigen::Vector3d surf_vector_last(0,0,0);
int last_surf_position = 0;
double depth_threshold = 0.1;
//********************************************************************************************************************************************
//第一次遍历找特征点（面、角）
for (int i = 5; i < cloudSize - 5; i += count_num ) {
float depth = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
laserCloud->points[i].y * laserCloud->points[i].y +
laserCloud->points[i].z * laserCloud->points[i].z);
// if(depth < 2) depth_threshold = 0.05;
// if(depth > 30) depth_threshold = 0.1;
//left curvature
float ldiffX =
laserCloud->points[i - 4].x + laserCloud->points[i - 3].x
- 4 * laserCloud->points[i - 2].x
+ laserCloud->points[i - 1].x + laserCloud->points[i].x;
float ldiffY =
laserCloud->points[i - 4].y + laserCloud->points[i - 3].y
- 4 * laserCloud->points[i - 2].y
+ laserCloud->points[i - 1].y + laserCloud->points[i].y;
float ldiffZ =
laserCloud->points[i - 4].z + laserCloud->points[i - 3].z
- 4 * laserCloud->points[i - 2].z
+ laserCloud->points[i - 1].z + laserCloud->points[i].z;
float left_curvature = ldiffX * ldiffX + ldiffY * ldiffY + ldiffZ * ldiffZ;
//使用左边5个点的以中间那个点作为基准计算出曲率半径来衡量左面的曲率
if(left_curvature < 0.01){
std::vector<PointType> left_list;
for(int j = -4; j < 0; j++){
left_list.push_back(laserCloud->points[i+j]);
}
//曲率在0.001以下的视为平面，只设置左5中点标志位1（平面点）
if( left_curvature < 0.001) CloudFeatureFlag[i-2] = 1; //surf point flag && plane_judge(left_list,1000)
left_surf_flag = true;
}
else{
left_surf_flag = false;
}
//right curvature
float rdiffX =
laserCloud->points[i + 4].x + laserCloud->points[i + 3].x
- 4 * laserCloud->points[i + 2].x
+ laserCloud->points[i + 1].x + laserCloud->points[i].x;
float rdiffY =
laserCloud->points[i + 4].y + laserCloud->points[i + 3].y
- 4 * laserCloud->points[i + 2].y
+ laserCloud->points[i + 1].y + laserCloud->points[i].y;
float rdiffZ =
laserCloud->points[i + 4].z + laserCloud->points[i + 3].z
- 4 * laserCloud->points[i + 2].z
+ laserCloud->points[i + 1].z + laserCloud->points[i].z;
float right_curvature = rdiffX * rdiffX + rdiffY * rdiffY + rdiffZ * rdiffZ;
//使用右边5个点的以中间那个点作为基准计算出曲率半径来衡量右面的曲率
if(right_curvature < 0.01){
std::vector<PointType> right_list;
for(int j = 1; j < 5; j++){
right_list.push_back(laserCloud->points[i+j]);
}
//曲率在0.001以下的视为平面，只设置右5中点标志位1（平面点）
if(right_curvature < 0.001 ) CloudFeatureFlag[i+2] = 1; //surf point flag && plane_judge(right_list,1000)
//左右都是平面视为左右都在同一平面，则移到平面上最后一个点作为下次计算点
count_num = 4;
right_surf_flag = true;
}
else{
//左右不都是平面，则使用下一个点作为下次计算点
count_num = 1;
right_surf_flag = false;
}
//当前点左右平面都是平面时，根据两平面夹角得出角点
if(left_surf_flag && right_surf_flag){
debugnum4 ++;
Eigen::Vector3d norm_left(0,0,0);
Eigen::Vector3d norm_right(0,0,0);
//计算左四点与当前点距离加权矩阵
for(int k = 1;k<5;k++){
Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i-k].x-laserCloud->points[i].x,
laserCloud->points[i-k].y-laserCloud->points[i].y,
laserCloud->points[i-k].z-laserCloud->points[i].z);
tmp.normalize();
norm_left += (k/10.0)* tmp;//k/10.0越远点权重越大
}
//计算右四点与当前点距离加权矩阵
for(int k = 1;k<5;k++){
Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i+k].x-laserCloud->points[i].x,
laserCloud->points[i+k].y-laserCloud->points[i].y,
laserCloud->points[i+k].z-laserCloud->points[i].z);
tmp.normalize();
norm_right += (k/10.0)* tmp;
}
//左右加权矩阵组成的平面夹角
double cc = fabs( norm_left.dot(norm_right) / (norm_left.norm()*norm_right.norm()) );
//计算当前点与左右最远点的矩阵
Eigen::Vector3d last_tmp = Eigen::Vector3d(laserCloud->points[i-4].x-laserCloud->points[i].x,
laserCloud->points[i-4].y-laserCloud->points[i].y,
laserCloud->points[i-4].z-laserCloud->points[i].z);
Eigen::Vector3d current_tmp = Eigen::Vector3d(laserCloud->points[i+4].x-laserCloud->points[i].x,
laserCloud->points[i+4].y-laserCloud->points[i].y,
laserCloud->points[i+4].z-laserCloud->points[i].z);
//取模，即距离值
double last_dis = last_tmp.norm();
double current_dis = current_tmp.norm();
//如果夹角在60°~120°之间，并且左右最远距离点都大于0.05，则当前点视为角点，设置标志位150
if(cc < 0.5 && last_dis > 0.05 && current_dis > 0.05 ){ //
debugnum5 ++;
CloudFeatureFlag[i] = 150;
}
}
}
//第二次遍历
for(int i = 5; i < cloudSize - 5; i ++){
float diff_left[2];
float diff_right[2];
//当前点深度
float depth = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
laserCloud->points[i].y * laserCloud->points[i].y +
laserCloud->points[i].z * laserCloud->points[i].z);
//分别与左/右近一、二两点的向量差值的深度（后面只用到了近一点的结果）
for(int count = 1; count < 3; count++ ){
float diffX1 = laserCloud->points[i + count].x - laserCloud->points[i].x;
float diffY1 = laserCloud->points[i + count].y - laserCloud->points[i].y;
float diffZ1 = laserCloud->points[i + count].z - laserCloud->points[i].z;
diff_right[count - 1] = sqrt(diffX1 * diffX1 + diffY1 * diffY1 + diffZ1 * diffZ1);
float diffX2 = laserCloud->points[i - count].x - laserCloud->points[i].x;
float diffY2 = laserCloud->points[i - count].y - laserCloud->points[i].y;
float diffZ2 = laserCloud->points[i - count].z - laserCloud->points[i].z;
diff_left[count - 1] = sqrt(diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2);
}
//右近一点的深度
float depth_right = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x +
laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
//左近一点的深度
float depth_left = sqrt(laserCloud->points[i - 1].x * laserCloud->points[i - 1].x +
laserCloud->points[i - 1].y * laserCloud->points[i - 1].y +
laserCloud->points[i - 1].z * laserCloud->points[i - 1].z);
//当与左右近一点向量差值的深度大于当前点深度的0.1倍时，则视为异常点（遗弃），设置标志250，直接进入下一点计算
if( (diff_right[0] > 0.1*depth && diff_left[0] > 0.1*depth) ){
debugnum1 ++;
CloudFeatureFlag[i] = 250;
continue;
}
//break points
if(fabs(diff_right[0] - diff_left[0])>0.1){//左右两点的深度差大于0.1时
if(diff_right[0] > diff_left[0]){//如果是右边更深
Eigen::Vector3d surf_vector = Eigen::Vector3d(laserCloud->points[i-4].x-laserCloud->points[i].x,
laserCloud->points[i-4].y-laserCloud->points[i].y,
laserCloud->points[i-4].z-laserCloud->points[i].z);
Eigen::Vector3d lidar_vector = Eigen::Vector3d(laserCloud->points[i].x,
laserCloud->points[i].y,
laserCloud->points[i].z);
double left_surf_dis = surf_vector.norm();
//计算当前点和最左点与当前点组成的面的夹角（入射角）
double cc = fabs( surf_vector.dot(lidar_vector) / (surf_vector.norm()*lidar_vector.norm()) );
std::vector<PointType> left_list;
double min_dis = 10000;
double max_dis = 0;
//计算左边两点平面的模的最大最小值
for(int j = 0; j < 4; j++){ //TODO: change the plane window size and add thin rod support
left_list.push_back(laserCloud->points[i-j]);
Eigen::Vector3d temp_vector = Eigen::Vector3d(laserCloud->points[i-j].x-laserCloud->points[i-j-1].x,
laserCloud->points[i-j].y-laserCloud->points[i-j-1].y,
laserCloud->points[i-j].z-laserCloud->points[i-j-1].z);
if(j == 3) break;
double temp_dis = temp_vector.norm();
if(temp_dis < min_dis) min_dis = temp_dis;
if(temp_dis > max_dis) max_dis = temp_dis;
}
//左临近两点的协方差矩阵最大特征值>100倍第二特征值时则认为是平面
bool left_is_plane = plane_judge(left_list,100);
//左边点是平面，两点深度差不多，最左点深度小于当前点深度0.05倍，入射角在36.9°~143.1°
if(left_is_plane && (max_dis < 2*min_dis) && left_surf_dis < 0.05 * depth && cc < 0.8){
//设置点的标志为断点
if(depth_right > depth_left){//右边点比左边点纵深
CloudFeatureFlag[i] = 100;
}
else{//右边点比左边点纵浅为0时
if(depth_right == 0) CloudFeatureFlag[i] = 100;
}
}
}
else{//如果是左边更深，跟上面一样的判断逻辑
Eigen::Vector3d surf_vector = Eigen::Vector3d(laserCloud->points[i+4].x-laserCloud->points[i].x,
laserCloud->points[i+4].y-laserCloud->points[i].y,
laserCloud->points[i+4].z-laserCloud->points[i].z);
Eigen::Vector3d lidar_vector = Eigen::Vector3d(laserCloud->points[i].x,
laserCloud->points[i].y,
laserCloud->points[i].z);
double right_surf_dis = surf_vector.norm();
//calculate the angle between the laser direction and the surface
double cc = fabs( surf_vector.dot(lidar_vector) / (surf_vector.norm()*lidar_vector.norm()) );
std::vector<PointType> right_list;
double min_dis = 10000;
double max_dis = 0;
for(int j = 0; j < 4; j++){ //TODO: change the plane window size and add thin rod support
right_list.push_back(laserCloud->points[i-j]);
Eigen::Vector3d temp_vector = Eigen::Vector3d(laserCloud->points[i+j].x-laserCloud->points[i+j+1].x,
laserCloud->points[i+j].y-laserCloud->points[i+j+1].y,
laserCloud->points[i+j].z-laserCloud->points[i+j+1].z);
if(j == 3) break;
double temp_dis = temp_vector.norm();
if(temp_dis < min_dis) min_dis = temp_dis;
if(temp_dis > max_dis) max_dis = temp_dis;
}
bool right_is_plane = plane_judge(right_list,100);
if(right_is_plane && (max_dis < 2*min_dis) && right_surf_dis < 0.05 * depth && cc < 0.8){ //
if(depth_right < depth_left){
CloudFeatureFlag[i] = 100;
}
else{
if(depth_left == 0) CloudFeatureFlag[i] = 100;
}
}
}
}
//对断点进一步复核左右加权向量的夹角大小
if(CloudFeatureFlag[i] == 100){
debugnum2++;
std::vector<Eigen::Vector3d> front_norms;
Eigen::Vector3d norm_front(0,0,0);
Eigen::Vector3d norm_back(0,0,0);
for(int k = 1;k<4;k++){
Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i-k].x-laserCloud->points[i].x,
laserCloud->points[i-k].y-laserCloud->points[i].y,
laserCloud->points[i-k].z-laserCloud->points[i].z);
tmp.normalize();
front_norms.push_back(tmp);
norm_front += (k/6.0)* tmp;
}
std::vector<Eigen::Vector3d> back_norms;
for(int k = 1;k<4;k++){
Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i+k].x-laserCloud->points[i].x,
laserCloud->points[i+k].y-laserCloud->points[i].y,
laserCloud->points[i+k].z-laserCloud->points[i].z);
tmp.normalize();
back_norms.push_back(tmp);
norm_back += (k/6.0)* tmp;
}
//左右三点加权向量夹角范围必须在在36.9°~143.1°，如果不符合则清除断点标志
double cc = fabs( norm_front.dot(norm_back) / (norm_front.norm()*norm_back.norm()) );
if(cc < 0.8){
debugnum3++;
}else{
CloudFeatureFlag[i] = 0;
}
continue;
}
}
//特征点分类和发布
for(int i = 0; i < cloudSize; i++){
//此两处计算结果貌似没卵用
//laserCloud->points[i].intensity = double(CloudFeatureFlag[i]) / 10000;
float dis = laserCloud->points[i].x * laserCloud->points[i].x
+ laserCloud->points[i].y * laserCloud->points[i].y
+ laserCloud->points[i].z * laserCloud->points[i].z;
float dis2 = laserCloud->points[i].y * laserCloud->points[i].y + laserCloud->points[i].z * laserCloud->points[i].z;
float theta2 = std::asin(sqrt(dis2/dis)) / M_PI * 180;
//std::cout<<"DEBUG theta "<<theta2<<std::endl;
// if(theta2 > 34.2 || theta2 < 1){
// continue;
// }
//if(dis > 30*30) continue;
if(CloudFeatureFlag[i] == 1){
surfPointsFlat.push_back(laserCloud->points[i]);
continue;
}
if(CloudFeatureFlag[i] == 100 || CloudFeatureFlag[i] == 150){
cornerPointsSharp.push_back(laserCloud->points[i]);
}
}
std::cout<<"ALL point: "<<cloudSize<<" outliers: "<< debugnum1 << std::endl
<<" break points: "<< debugnum2<<" break feature: "<< debugnum3 << std::endl
<<" normal points: "<< debugnum4<<" surf-surf feature: " << debugnum5 << std::endl;
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/livox";
pubLaserCloud.publish(laserCloudOutMsg);
sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/livox";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);
sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/livox";
pubSurfPointsFlat.publish(surfPointsFlat2);
}
434
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
