2d激光点云识别退化场景（长走廊）

本文主要是介绍2d激光点云识别退化场景（长走廊），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

注：算法只适用于静态场景，在有动态场景（行人）的环境下不适用
退化场景描述
场景一：长走廊
在这里插入图片描述
激光探测距离有限，在长走廊环境下，激光在某些位置无法探测到走廊尽头，会出现如上图情况，激光轮廓为红色的两条平行线。对于这种情况，我们只需寻找到只有两个平行线，即位退化场景
场景二：单一墙面
在这里插入图片描述
通常情况下，退化场景为如上两种情况，当然多条平行线也是符合的。

算法思路为，如果激光雷达点云构成的特征都是平行线或者只有一条线，我们反过来想，如果找到两条不平行的线，则不是退化场景。

步骤如下
1、特征分割

2、拟合直线

3、计算直线与其他直线斜率是否近似

特征分割

这里用的是分离合并算法检测断点，代码为自己手撸。

void MarkIndexDetection(const sensor::PointCloud& pointcloud, std::vector<size_t>* mark_index) {//取首尾点为断点std::vector<size_t> mark_index_t;mark_index_t.push_back(0);mark_index_t.push_back(pointcloud.size()-1);while (true) {std::vector<size_t> cur_mark_index;for(size_t i = 0; i < mark_index_t.size()-1 ; i++) {//依次遍历两两断点//断点索引const size_t &front_index_id = mark_index_t[i];const size_t &back_index_id = mark_index_t[i+1];const double &p1_x = pointcloud[front_index_id].position.x();const double &p1_y = pointcloud[front_index_id].position.y();const double &p2_x = pointcloud[back_index_id].position.x();const double &p2_y = pointcloud[back_index_id].position.y();double max_dist = 0;size_t max_dist_index = 0;for(size_t j = front_index_id + 1; j < back_index_id; j++) {const double &point_x = pointcloud[j].position.x();const double &point_y = pointcloud[j].position.y();/*计算 point 到 p1和p2构成的直线的投影i但坐标 line_pt*/const double A = point_x - p1_x;const double B = point_y - p1_y;const double C = p2_x - p1_x;const double D = p2_y - p1_y;const double param = (A * C + B * D) / (C * C + D * D);const double line_pt_x = p1_x + param * C;const double line_pt_y = p1_y + param * D;//计算point与line_pt的距离const double dist = std::hypot(line_pt_x - point_x, line_pt_y - point_y);if(dist > max_dist) {max_dist = dist;max_dist_index = j;}}//距离大于预值，则为标记点const double SPLIT_THRED = 0.12;if(max_dist > SPLIT_THRED) {cur_mark_index.emplace_back(max_dist_index);}}//如果没有新的断点增加,则退出if(cur_mark_index.empty()) break;//把新增加的断点加入到所有断点中mark_index_t.insert(mark_index_t.end(), cur_mark_index.begin(), cur_mark_index.end());//从小到大排序std::sort(mark_index_t.begin(), mark_index_t.end());}*mark_index = mark_index_t;}

拟合直线与斜率检测

// pointcloud 为激光点云数据（已完成过滤，无效点过滤） 函数返回 true表示在退化场景 false表示不在退化场景
bool DegradedSceneDetection(const sensor::PointCloud& pointcloud) {//如果点数过少，则是退化场景if(pointcloud.size() < 3) return true;std::vector<size_t> mark_index;MarkIndexDetection(pointcloud, &mark_index);  std::vector<LineSeg> line_seg;for(size_t i = 0; i < mark_index.size()-1; i++) {const size_t &front_index_id = mark_index[i];const size_t &back_index_id  = mark_index[i+1];if(front_index_id +1 == back_index_id) continue;// 拟合直线sensor::PointCloud fitting_line_point_cloud;//取出两个断点之间的点  for(size_t j = front_index_id; j < back_index_id; j++) {fitting_line_point_cloud.push_back(pointcloud[j]);// LOG(INFO) << " point x: " << fit_point_cloud_t[j].x() << ", y: " << fit_point_cloud_t[j].y();}// LOG(INFO) << "fitting line point cloud size = " << fitting_line_point_cloud.size();if(fitting_line_point_cloud.size() < 5) { continue; }LineSeg ls;//拟合直线FittingLine2D(fitting_line_point_cloud, ls.k, ls.b, ls.k_is_exist);// LOG(INFO) << "k: " << ls.k << ", b: " << ls.b <<", k_is_exist" << ls.k_is_exist;//计算线段的两个端点PointToLineSubpoint(fitting_line_point_cloud.points().front().position.x(), fitting_line_point_cloud.points().front().position.y(),ls.k_is_exist, ls.k, ls.b, ls.start_x, ls.start_y);PointToLineSubpoint(fitting_line_point_cloud.points().back().position.x(), fitting_line_point_cloud.points().back().position.y(),ls.k_is_exist, ls.k, ls.b, ls.start_x, ls.start_y);                    //检测l与之前的线段的角度差异,如果大于阈值,则没必要在计算,则是不是退化场景const double NOT_IN_LINE_THRED = 20;for(const auto &a_ls : line_seg) {double ls_k_angle = 90;double a_ls_k_angle = 90;if(ls.k_is_exist) { ls_k_angle = RadToDeg(std::atan(ls.k)); }if(a_ls.k_is_exist) { a_ls_k_angle = RadToDeg(std::atan(a_ls.k)); }//如果存在两个直线角度相差20，则认为不在退化场景if(std::fabs(ls_k_angle - a_ls_k_angle) > NOT_IN_LINE_THRED) {// LOG(INFO) << "ls_k_angle: " << ls_k_angle << ", a_ls_k_angle: " << a_ls_k_angle;return false;}}// if(ls.k_is_exist){//   LOG(INFO) << "k: " << common::RadToDeg(std::atan(ls.k));// } else {//   LOG(INFO) << "k: 90"; // }line_seg.push_back(ls);}return true;
}

其他一些函数

bool FittingLine2D(const sensor::PointCloud& data, double &k, double &b, bool &k_exist) {// LOG(WARNING) << "start FittingLine2D";ceres::Problem problem;double sum_x = 0, sum_y = 0;int rows = data.size();//取得样本数据的个数if(rows == 0) { k = 0; b = 0;// LOG(WARNING) << "rows of input data is zero, can't work";return false;}for(int i = 0 ; i < rows; ++i) {problem.AddResidualBlock(new ceres::AutoDiffCostFunction<LineResidual, 1, 1, 1>(new LineResidual(data[i].position.x(), data[i].position.y())), NULL, &k, &b);//检测x变化，检测斜率是否为无穷大sum_x += data[i].position.x();sum_y += data[i].position.y();}double average_x = sum_x/rows;double average_y = sum_y/rows;//计算方差double variance_x = 0;double variance_y = 0;for(int i = 0; i < rows; ++i) {variance_x += (data[i].position.x() - average_x) * (data[i].position.x() - average_x);variance_y += (data[i].position.y() - average_y) * (data[i].position.y() - average_y);}variance_x = variance_x / rows;variance_y = variance_y / rows;if(variance_x < 0.001 && variance_y < 0.001 ) { return false; }//方差小于一定阈值if(variance_x < 0.001) {k = 0xffff;// 直线与x轴垂直,k为无穷大b = average_x;k_exist = false;return true;}ceres::Solver::Options options;options.max_num_iterations = 10;options.linear_solver_type = ceres::DENSE_QR;options.minimizer_progress_to_stdout = false;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);k_exist = true;// LOG(WARNING) << summary.BriefReport() << "\n";// LOG(WARNING) << "Final k: " << k << " b: " << b << "\n";return true;}void PointToLineSubpoint(const double point_x, const double point_y, const double p1_x, const double p1_y, const double p2_x, const double p2_y, double &line_pt_x, double &line_pt_y) {const double &A = point_x - p1_x;const double &B = point_y - p1_y;const double &C = p2_x - p1_x;const double &D = p2_y - p1_y;const double &param = (A * C + B * D) / (C * C + D * D);line_pt_x = p1_x + param * C;line_pt_y = p1_y + param * D;}void PointToLineSubpoint(const double point_x, const double point_y,const bool k_exist, const double k, const double b,                 double &line_pt_x, double &line_pt_y) {double p1_x = 0, p1_y = 0;double p2_x = 0, p2_y = 0;if(k_exist) {p1_x = -10; p1_y = k * p1_x + b;p2_x = 10; p2_y = k * p2_x + b;} else {p1_x = b; p1_y = 10;p2_x = b; p2_y = -10;}PointToLineSubpoint(point_x, point_y, p1_x, p1_y, p2_x, p2_y, line_pt_x, line_pt_y);}// Converts from degrees to radians.constexpr double DegToRad(double deg) { return M_PI * deg / 180.; }// Converts form radians to degrees.constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; }//拟合生成的线段,如果出现  两个不平行的直线,则推出,不是退化场景struct LineSeg{double start_x = 0;double start_y = 0;double end_x = 0;double end_y = 0;double k = 0,b = 0;bool k_is_exist = false;};