// // Created by zx on 2019/12/6. // #include "region_detect.h" /** * 有参构造函数 * */ Region_detector::Region_detector(int id, wj::Region region) : m_region_id(-1) { m_region_param.CopyFrom(region); m_region_id = id; } /** * 析构函数 * */ Region_detector::~Region_detector() { } /** * 获取区域id * */ int Region_detector::get_region_id() { return m_region_id; } /** * 预处理，xy直通滤波与离群点过滤 * 返回 PARAMETER_ERROR，WJ_REGION_EMPTY_CLOUD 或 SUCCESS * */ Error_manager Region_detector::preprocess(pcl::PointCloud::Ptr cloud_in, pcl::PointCloud::Ptr cloud_out) { // 1.参数合法性检验 if (!m_region_param.has_maxx() || !m_region_param.has_maxy() || !m_region_param.has_minx() || !m_region_param.has_miny()) return Error_manager(Error_code::PARAMETER_ERROR); if (cloud_in->size() <= 0) return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD); cloud_out->clear(); pcl::copyPointCloud(*cloud_in, *cloud_out); if (cloud_out->size() <= 0) return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD); // 2.直通滤波, 筛选xy pcl::PassThrough pass; pass.setInputCloud(cloud_out); pass.setFilterFieldName("x"); //设置想在哪个坐标轴上操作 pass.setFilterLimits(m_region_param.minx(), m_region_param.maxx()); //将x轴的0到1范围内 pass.setFilterLimitsNegative(false); //保留（true就是删除，false就是保留而删除此区间外的） pass.filter(*cloud_out); //输出到结果指针 if (cloud_out->size() <= 0) return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD); pass.setInputCloud(cloud_out); pass.setFilterFieldName("y"); //设置想在哪个坐标轴上操作 pass.setFilterLimits(m_region_param.miny(), m_region_param.maxy()); //将x轴的0到1范围内 pass.setFilterLimitsNegative(false); //保留（true就是删除，false就是保留而删除此区间外的） pass.filter(*cloud_out); //输出到结果指针 if (cloud_out->size() <= 0) return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD); // 3.离群点过滤 pcl::StatisticalOutlierRemoval sor; sor.setInputCloud(cloud_out); sor.setMeanK(15); //K近邻搜索点个数 sor.setStddevMulThresh(3.0); //标准差倍数 sor.setNegative(false); //保留未滤波点（内点） sor.filter(*cloud_out); //保存滤波结果到cloud_filter if (cloud_out->size() <= 0) return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD); else return Error_manager(Error_code::SUCCESS); } /** * 返回二维点之间距离 * */ double distance(cv::Point2f p1, cv::Point2f p2) { return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0)); } /** * 判断是否符合标准矩形， * 传入矩形四个角点，以及是否打印 * 返回 WJ_REGION_RECTANGLE_ANGLE_ERROR, WJ_REGION_RECTANGLE_SIZE_ERROR, * WJ_REGION_RECTANGLE_SYMMETRY_ERROR, WJ_REGION_CLUSTER_SIZE_ERROR, SUCCESS * */ Error_manager Region_detector::isRect(std::vector &points, bool print) { if (points.size() == 4) { double L[3] = {0.0}; L[0] = distance(points[0], points[1]); L[1] = distance(points[1], points[2]); L[2] = distance(points[0], points[2]); double max_l = L[0]; double l1 = L[1]; double l2 = L[2]; int max_index = 0; cv::Point2f ps = points[0], pt = points[1]; // 顶点 cv::Point2f pc = points[2]; for (int i = 1; i < 3; ++i) { if (L[i] > max_l) { max_index = i; max_l = L[i]; l1 = L[abs(i + 1) % 3]; l2 = L[abs(i + 2) % 3]; ps = points[i % 3]; pt = points[(i + 1) % 3]; pc = points[(i + 2) % 3]; } } // 顶角大小 double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2); // 顶角相对于90度过大或小 if (fabs(cosa) >= 0.15 /* || std::min(l1, l2) > 2.0 || std::max(l1, l2) > 3.3*/) { if (print) { LOG(WARNING) << " angle cos >0.13 =" << cosa << " i=" << max_index; // LOG(WARNING) << "L1:" << l1 << " L2:" << l2 << " L3:" << max_l; } return Error_manager(Error_code::WJ_REGION_RECTANGLE_ANGLE_ERROR); } float width = std::min(l1, l2); float length = std::max(l1, l2); // 车宽应位于[1.35, 2.0]，车长应位于[2.2, 3.0] if (width < 1.350 || width > 2.000 || length > 3.000 || length < 2.200) { if (print) { LOG(WARNING) << "\t width<1350 || width >2100 || length >3300 ||length < 2100 " << " length:" << length << " width:" << width; } return Error_manager(Error_code::WJ_REGION_RECTANGLE_SIZE_ERROR); } double d = distance(pc, points[3]); cv::Point2f center1 = (ps + pt) * 0.5; cv::Point2f center2 = (pc + points[3]) * 0.5; // 对角线形变超过0.15倍，或中心点距离偏差0.15m if (fabs(d - max_l) > max_l * 0.15 || distance(center1, center2) > 0.150) { if (print) { LOG(WARNING) << "d:" << d << " maxl:" << max_l << " center1:" << center1 << " center2:" << center2 << " center distance=" << distance(center1, center2); } return Error_manager(Error_code::WJ_REGION_RECTANGLE_SYMMETRY_ERROR); } if (print) { LOG(INFO) << " rectangle verify OK cos angle=" << cosa << " length off=" << fabs(d - max_l) << " center distance=" << distance(center1, center2); } return Error_manager(Error_code::SUCCESS); } // 以三个点进行验证 else if (points.size() == 3) { double L[3] = {0.0}; L[0] = distance(points[0], points[1]); L[1] = distance(points[1], points[2]); L[2] = distance(points[0], points[2]); double max_l = L[0]; double l1 = L[1]; double l2 = L[2]; int max_index = 0; cv::Point2f ps = points[0], pt = points[1]; cv::Point2f pc = points[2]; for (int i = 1; i < 3; ++i) { if (L[i] > max_l) { max_index = i; max_l = L[i]; l1 = L[abs(i + 1) % 3]; l2 = L[abs(i + 2) % 3]; ps = points[i % 3]; pt = points[(i + 1) % 3]; pc = points[(i + 2) % 3]; } } double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2); // 顶角应接近90度 if (fabs(cosa) >= 0.15) { if (print) { LOG(WARNING) << "3 wheels angle cos >0.12 =" << cosa << " i=" << max_index; LOG(WARNING) << "L1:" << l1 << " L2:" << l2 << " L3:" << max_l; } return Error_manager(Error_code::WJ_REGION_RECTANGLE_ANGLE_ERROR); } double length = std::max(l1, l2); double width = std::min(l1, l2); // 车宽应位于[1.4, 2.0]，车长应位于[2.2, 3.0] if (length > 1.900 && length < 3.000 && width > 1.400 && width < 1.900) { //生成第四个点 cv::Point2f vec1 = ps - pc; cv::Point2f vec2 = pt - pc; cv::Point2f point4 = (vec1 + vec2) + pc; points.push_back(point4); if (print) { LOG(WARNING) << "3 wheels rectangle verify OK cos angle=" << cosa << " L=" << length << " w=" << width; } return Error_manager(Error_code::SUCCESS); } else { if (print) { LOG(WARNING) << "3 wheels rectangle verify Failed cos angle=" << cosa << " L=" << length << " w=" << width; } return Error_manager(Error_code::WJ_REGION_RECTANGLE_SIZE_ERROR); } } else { return Error_manager(Error_code::WJ_REGION_CLUSTER_SIZE_ERROR); } } /* * 仅仅聚类 */ Error_manager Region_detector::clustering_only(pcl::PointCloud::Ptr cloud_in, std::vector> &seg_clouds, bool print) { // 1.判断参数合法性 if (cloud_in->size() <= 0) return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD); // 2.聚类 pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); kdtree->setInputCloud(cloud_in); pcl::EuclideanClusterExtraction clustering; // 设置聚类的最小值 2cm (small values may cause objects to be divided // in several clusters, whereas big values may join objects in a same cluster). clustering.setClusterTolerance(0.2); // 设置聚类的小点数和最大点云数 clustering.setMinClusterSize(10); clustering.setMaxClusterSize(500); clustering.setSearchMethod(kdtree); clustering.setInputCloud(cloud_in); std::vector clusters; clustering.extract(clusters); if(clusters.size() <= 0) return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD); for (int i = 0; i < clusters.size(); ++i) { seg_clouds.push_back(pcl::PointCloud()); } int j = 0; for (std::vector::const_iterator it = clusters.begin(); it != clusters.end(); ++it) { pcl::PointCloud::Ptr cloud_cluster(new pcl::PointCloud); //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中 for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) { cloud_cluster->points.push_back(cloud_in->points[*pit]); cloud_cluster->width = cloud_cluster->points.size(); cloud_cluster->height = 1; cloud_cluster->is_dense = true; } seg_clouds[j] = *cloud_cluster; j++; } if (print) { LOG(INFO) << " cluster classes:" << clusters.size(); } return SUCCESS; } /** * 聚类并通过矩形判断函数 * 传入原始点云，传出角点与聚类出的点云 * 返回 WJ_REGION_EMPTY_CLOUD, WJ_REGION_RECTANGLE_ANGLE_ERROR, WJ_REGION_RECTANGLE_SIZE_ERROR, * WJ_REGION_RECTANGLE_SYMMETRY_ERROR, WJ_REGION_CLUSTER_SIZE_ERROR, SUCCESS * */ Error_manager Region_detector::clustering(pcl::PointCloud::Ptr cloud_in, std::vector &corner_points, std::vector::Ptr> &seg_clouds, bool print) { // 1.判断参数合法性 if (cloud_in->size() <= 0) return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD); // 2.聚类 pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); kdtree->setInputCloud(cloud_in); pcl::EuclideanClusterExtraction clustering; // 设置聚类的最小值 2cm (small values may cause objects to be divided // in several clusters, whereas big values may join objects in a same cluster). clustering.setClusterTolerance(0.2); // 设置聚类的小点数和最大点云数 clustering.setMinClusterSize(10); clustering.setMaxClusterSize(500); clustering.setSearchMethod(kdtree); clustering.setInputCloud(cloud_in); std::vector clusters; clustering.extract(clusters); if(clusters.size() <= 0) return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD); for (int i = 0; i < clusters.size(); ++i) { seg_clouds.push_back(pcl::PointCloud::Ptr(new pcl::PointCloud)); } int j = 0; for (std::vector::const_iterator it = clusters.begin(); it != clusters.end(); ++it) { pcl::PointCloud::Ptr cloud_cluster(new pcl::PointCloud); //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中 for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) { cloud_cluster->points.push_back(cloud_in->points[*pit]); cloud_cluster->width = cloud_cluster->points.size(); cloud_cluster->height = 1; cloud_cluster->is_dense = true; } seg_clouds[j] = cloud_cluster; j++; } if (print) { LOG(INFO) << " cluster classes:" << clusters.size(); } // 3.分别计算每团点云几何中心，作为矩形角点 corner_points.clear(); for (int i = 0; i < clusters.size(); ++i) { if (seg_clouds.size() <= i || seg_clouds[i]->size() == 0) continue; float sumX = 0, sumY = 0; for (int j = 0; j < seg_clouds[i]->size(); ++j) { sumX += seg_clouds[i]->points[j].x; sumY += seg_clouds[i]->points[j].y; } float center_x = sumX / float(seg_clouds[i]->size()); float center_y = sumY / float(seg_clouds[i]->size()); corner_points.push_back(cv::Point2f(center_x, center_y)); } /*char buf[255]={0}; for (int k = 0; k < corner_points.size(); ++k) { sprintf(buf+strlen(buf), "point %d: (%.2f, %.2f)__", k, corner_points[k].x, corner_points[k].y); } LOG(WARNING) << buf;*/ cv::RotatedRect t_rect=cv::minAreaRect(corner_points); //display(t_rect,cv::Scalar(255,0,0)); return isRect(corner_points, print); } /** * 省略传出参数的四轮检测算法函数 * 传入待检测点云 * 返回值检测结果 * */ Error_manager Region_detector::detect(pcl::PointCloud::Ptr cloud_in) { pcl::PointCloud::Ptr cloud_filtered = pcl::PointCloud::Ptr(new pcl::PointCloud); // 1.预处理 Error_manager result = preprocess(cloud_in, cloud_filtered); if (result == SUCCESS) { std::vector corner_points; std::vector::Ptr> seg_clouds; // 2.聚类与矩形校验 result = clustering(cloud_filtered, corner_points, seg_clouds); } return result; } /** * ceres优化 * 优化变量:中心点、角度、轮距与宽度的四轮点云检测函数 * */ Error_manager Region_detector::detect(pcl::PointCloud::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width, double& front_wheel_theta,bool print) { Error_manager result = Error_manager(Error_code::SUCCESS); pcl::PointCloud::Ptr cloud_filtered(pcl::PointCloud::Ptr(new pcl::PointCloud)); // 1.预处理 result = preprocess(cloud_in, cloud_filtered); if (result != SUCCESS) return result; // 2.聚类 std::vector> seg_clouds; result = clustering_only(cloud_filtered, seg_clouds, print); if (result != SUCCESS) return result; if(!m_detector_ceres.detect(seg_clouds,x,y,c,wheelbase,width,front_wheel_theta)) return WJ_REGION_CERES_SOLVE_ERROR; return Error_manager(Error_code::SUCCESS); } /** * 输出中心点、角度、轮距与宽度的四轮点云检测函数 * */ Error_manager Region_detector::detect(pcl::PointCloud::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width, bool print) { Error_manager result = Error_manager(Error_code::SUCCESS); pcl::PointCloud::Ptr cloud_filtered(pcl::PointCloud::Ptr(new pcl::PointCloud)); // 1.预处理 result = preprocess(cloud_in, cloud_filtered); if (result != SUCCESS) return result; // 2.聚类计算角点 std::vector corner_points; std::vector::Ptr> seg_clouds; result = clustering(cloud_filtered, corner_points, seg_clouds, print); if (result != SUCCESS) return result; // 修改原始点云为预处理后点云，并加入角点供查验 cloud_in->clear(); cloud_in->operator+=(*cloud_filtered); for (size_t i = 0; i < corner_points.size(); i++) { cloud_in->points.push_back(pcl::PointXYZ(corner_points[i].x, corner_points[i].y, 0.0)); } // convert all points after preprocessed into 2d std::vector all_points; // car center for (int j = 0; j < cloud_filtered->size(); ++j) { all_points.push_back(cv::Point2f(cloud_filtered->points[j].x, cloud_filtered->points[j].y)); } // find bounding rectangle of all wheel points cv::RotatedRect wheel_box = cv::minAreaRect(all_points); x = wheel_box.center.x; y = wheel_box.center.y; // add center point to the input cloud for further check cloud_in->points.push_back(pcl::PointXYZ(x, y, 0.0)); // 长边向量 cv::Point2f vec; cv::Point2f vertice[4]; wheel_box.points(vertice); float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0); float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0); // 寻找长边，倾角为长边与x轴夹角 if (len1 > len2) { vec.x = vertice[0].x - vertice[1].x; vec.y = vertice[0].y - vertice[1].y; } else { vec.x = vertice[1].x - vertice[2].x; vec.y = vertice[1].y - vertice[2].y; } float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y)); c = angle_x; // get line formula, normal is (cos(theta), sin(theta)), towards the long side // the line formula is: nx * x + ny * y + (-(nx*cx+ny*cy)) = 0 Eigen::Vector2f normal, center_point; normal << vec.x / sqrt(vec.x * vec.x + vec.y * vec.y), vec.y / sqrt(vec.x * vec.x + vec.y * vec.y); center_point << x, y; float line_param_c = -1 * center_point.transpose() * normal; // get rotation matrix, get the angle towards normal vector, rather than x axis // R = [ cos -sin] // [ sin cos] float rotate_angle = M_PI_2 - acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y)); Eigen::Matrix2f rotation_matrix; rotation_matrix << cos(rotate_angle), -sin(rotate_angle), sin(rotate_angle), cos(rotate_angle); // find min x and max x, separate y values according to the line, calculate difference between mean of y values as wheelbase float min_x = 20, max_x = 0; float y_values0 = 0, y_values1 = 0; int count0 = 0, count1 = 0; for (size_t i = 0; i < seg_clouds.size(); i++) { if (seg_clouds[i]->size() <= 0) continue; for (size_t j = 0; j < seg_clouds[i]->size(); j++) { // origin point and the point rotated around the origin Eigen::Vector2f vec, vec_rot; vec << seg_clouds[i]->points[j].x, seg_clouds[i]->points[j].y; vec_rot = rotation_matrix * (vec - center_point) + center_point; // find min max x if (vec_rot[0] < min_x) min_x = vec_rot[0]; if (vec_rot[0] > max_x) max_x = vec_rot[0]; // separate point as two clusters(front and back), calc y values respectively if (normal.transpose() * vec + line_param_c > 0) { y_values0 += vec_rot[1]; count0 += 1; } else { y_values1 += vec_rot[1]; count1 += 1; } } } // check if front and back both has points if (count0 > 0 && count1 > 0) { y_values0 /= count0; y_values1 /= count1; wheelbase = fabs(y_values1 - y_values0); width = fabs(min_x - max_x); // LOG(INFO) << "--------detector find width, wheelbase: " << width << ", " << wheelbase << ", y means: [" << y_values0 << ", " << y_values1 << "] -----"; cloud_in->points.push_back(pcl::PointXYZ(min_x, y, 0.0)); cloud_in->points.push_back(pcl::PointXYZ(max_x, y, 0.0)); cloud_in->points.push_back(pcl::PointXYZ(x, y_values0, 0.0)); cloud_in->points.push_back(pcl::PointXYZ(x, y_values1, 0.0)); } else { // calculate wheelbase according to corner points // wheelbase = std::max(wheel_box.size.width, wheel_box.size.height); // LOG(INFO) << "--------detector find border: [" << wheel_box.size.width << ", " << wheel_box.size.height << "] -----"; cv::RotatedRect wheel_center_box = cv::minAreaRect(corner_points); wheelbase = std::max(wheel_center_box.size.width, wheel_center_box.size.height); width = std::min(wheel_box.size.width, wheel_box.size.height); } return Error_manager(Error_code::SUCCESS); }