代码来自于 livox_mapping，先简单说结论：
异常点提取：与左右点的距离大于深度值的十分之一
平面点提取： 主要通过左边或者右边四个点的曲率小于 0.001 计算得到；
角点提取：主要有几个来源
平面角点，左右都是平面且平面夹角 60-120之间; （可以理解为方形柱子的边缘特征点）
断点角点：（可以理解为物体的边缘到远处的扫描点 ）根据当前点与左右附近点的距离判断，断点阈值0.1； 并且有一边的点是平面； 点与点之间没有太远； 最远点距离不超过深度值的二十分之一； 当前点和最远点合成的平面 和当前点向量的夹角，夹角 37度到 143度；
其他的livox 项目特征提取也是大同小异的，看懂了这个基本其他的也都类似；
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
pcl::PointCloud<PointType> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
int cloudSize = laserCloudIn.points.size();
std::cout<<"DEBUG first cloudSize "<<cloudSize<<std::endl;
if(cloudSize > 32000) cloudSize = 32000;
int count = cloudSize;
PointType point;
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
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;
point.intensity = laserCloudIn.points[i].intensity;
point.curvature = laserCloudIn.points[i].curvature;
int scanID = 0;
if (N_SCANS == 6) {
scanID = (int)point.intensity;
}
laserCloudScans[scanID].push_back(point);
} //todo 这里先把他分成了6份，又在下面把点云合成了一份 意思是先把点云按照每段分开，然后再合成；
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++) {
*laserCloud += laserCloudScans[i];
}
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;
// todo 第一次遍历 找特征点
//********************************************************************************************************************************************
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// todo 以 i-2 为中心的曲率计算
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;
// todo 左边曲率
float left_curvature = ldiffX * ldiffX + ldiffY * ldiffY + ldiffZ * ldiffZ;
if(left_curvature < 0.01){ // todo 平面点
std::vector<PointType> left_list; // todo 这个list没卵用啊。。。。
for(int j = -4; j < 0; j++){
left_list.push_back(laserCloud->points[i+j]);
}
// todo 为啥还弄了2个平面点的标准 这个更严格 但是它是把 i-2 设置为平面点了， 不是i 平面点主要在这里找到的 还是靠曲率 不过标准好高啊
if( left_curvature < 0.001) CloudFeatureFlag[i-2] = 1; //surf point flag && plane_judge(left_list,1000)
// todo 平面点判断在这里
left_surf_flag = true; // todo 起作用的是它
}
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;
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]);
}
if(right_curvature < 0.001 ) CloudFeatureFlag[i+2] = 1; //surf point flag && plane_judge(right_list,1000)
// todo 平面点判断在这里
count_num = 4;
right_surf_flag = true;
}
else{
count_num = 1;
right_surf_flag = false;
}
//surf-surf corner feature
// todo 如果左右都是平面，那么就继续处理； 这里找到的是左右都是平面的角点
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; // todo 左右点的向量相加，越远的权重越大
}
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;
}
// todo 左右点群的向量夹角
//calculate the angle between this group and the previous group
double cc = fabs( norm_left.dot(norm_right) / (norm_left.norm()*norm_right.norm()) );
//calculate the maximum distance, the distance cannot be too small
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();
// todo 与左，与右最远点的距离大于阈值 并且 左右点群的向量夹角 小于 0.5; 0.5 是60度，0.95是18度； 0.8是37度； 这里就是 60-120度
todo 为什么 小于 60度的不算角点呢？？？ 可能因为这里还只是初步计算了平面类型的角点，不是所有类型的
if(cc < 0.5 && last_dis > 0.05 && current_dis > 0.05 ){ //
debugnum5 ++;
CloudFeatureFlag[i] = 150; // todo 角点
}
}
}
// todo 第二次遍历计算特征 断点角点
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);
// todo 就是与左右点的距离计算 得到了一个列表 这里是计算了左右各三个点的距离
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);
}
// todo 这里是左右单个点的深度值
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);
// todo outliers 与左右点的距离大于深度值的十分之一，就是异常点，不要他
if( (diff_right[0] > 0.1*depth && diff_left[0] > 0.1*depth) ){
debugnum1 ++;
CloudFeatureFlag[i] = 250;
continue;
}
//break points // todo 与左边点的距离差 - 与右边点的距离差 大于 0.1 ，就是断点
if(fabs(diff_right[0] - diff_left[0])>0.1){
if(diff_right[0] > diff_left[0]){ // todo 当与右边点距离更大的时候，使用左边点
// todo 最远点合成的向量
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();
// todo 最远点平面 和当前点向量的夹角
//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> 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;
}
// todo 第2个特征根大于100倍的最小特征根，就是平面
bool left_is_plane = plane_judge(left_list,100);
// todo 左边点是平面； 点与点之间没有太远； 左边最远点距离不超过深度值的二十分之一； 夹角 37度到 143度；
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; // todo 角点
}
else{
if(depth_right == 0) CloudFeatureFlag[i] = 100; // todo 只在右边点深度值为0的时候才是角点
}
}
}
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;
}
}
}
}
// break point select // todo 如果当前点是断点 继续操作
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;
}
// todo 左边三个点群的加权向量 和右边三个点群的加强向量的夹角 37~143之间 ； 超过了就把它标签设置为0， 对之前判断的100标签的再次判断
double cc = fabs( norm_front.dot(norm_back) / (norm_front.norm()*norm_back.norm()) );
if(cc < 0.8){
debugnum3++;
}else{
CloudFeatureFlag[i] = 0;
}
continue;
}
}
// todo 第三次遍历 放入特征点
//push_back feature
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);
}
2555
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
