|
|
@@ -31,42 +31,42 @@ Error_manager Region_detector::init(Point2D_tool::Point2D_box point2D_box)
|
|
|
* */
|
|
|
Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
|
|
|
{
|
|
|
- // 1.参数合法性检验
|
|
|
- if (p_cloud->size() <= 0)
|
|
|
- return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
-std::cout << " huli test :::: " << " 222 = " << 222 << std::endl;
|
|
|
- // 2.直通滤波, 筛选xy
|
|
|
- pcl::PassThrough<pcl::PointXYZ> pass;
|
|
|
- pass.setInputCloud(p_cloud);
|
|
|
- pass.setFilterFieldName("x"); //设置想在哪个坐标轴上操作
|
|
|
- pass.setFilterLimits(m_region_box.x_min, m_region_box.x_max); //将x轴的0到1范围内
|
|
|
- pass.setFilterLimitsNegative(false); //保留（true就是删除，false就是保留而删除此区间外的）
|
|
|
- pass.filter(*p_cloud); //输出到结果指针
|
|
|
-std::cout << " huli test :::: " << " 333 = " << 333 << std::endl;
|
|
|
- if (p_cloud->size() <= 0)
|
|
|
- return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
-
|
|
|
- pass.setInputCloud(p_cloud);
|
|
|
- pass.setFilterFieldName("y"); //设置想在哪个坐标轴上操作
|
|
|
- pass.setFilterLimits(m_region_box.y_min, m_region_box.y_max); //将x轴的0到1范围内
|
|
|
- pass.setFilterLimitsNegative(false); //保留（true就是删除，false就是保留而删除此区间外的）
|
|
|
- pass.filter(*p_cloud); //输出到结果指针
|
|
|
-std::cout << " huli test :::: " << " 444 = " << 444 << std::endl;
|
|
|
- if (p_cloud->size() <= 0)
|
|
|
- return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
-std::cout << " huli test :::: " << " 445 = " << 445 << std::endl;
|
|
|
- // 3.离群点过滤
|
|
|
- pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
|
|
|
- sor.setInputCloud(p_cloud);
|
|
|
- sor.setMeanK(15); //K近邻搜索点个数
|
|
|
- sor.setStddevMulThresh(3.0); //标准差倍数
|
|
|
- sor.setNegative(false); //保留未滤波点（内点）
|
|
|
- sor.filter(*p_cloud); //保存滤波结果到cloud_filter
|
|
|
-std::cout << " huli test :::: " << " 555 = " << 555 << std::endl;
|
|
|
- if (p_cloud->size() <= 0)
|
|
|
- return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
- else
|
|
|
- return Error_manager(Error_code::SUCCESS);
|
|
|
+ // 1.参数合法性检验
|
|
|
+ if (p_cloud->size() <= 0)
|
|
|
+ return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
+ std::cout << " huli test :::: " << " 222 = " << 222 << std::endl;
|
|
|
+ // 2.直通滤波, 筛选xy
|
|
|
+ pcl::PassThrough<pcl::PointXYZ> pass;
|
|
|
+ pass.setInputCloud(p_cloud);
|
|
|
+ pass.setFilterFieldName("x"); //设置想在哪个坐标轴上操作
|
|
|
+ pass.setFilterLimits(m_region_box.x_min, m_region_box.x_max); //将x轴的0到1范围内
|
|
|
+ pass.setFilterLimitsNegative(false); //保留（true就是删除，false就是保留而删除此区间外的）
|
|
|
+ pass.filter(*p_cloud); //输出到结果指针
|
|
|
+ std::cout << " huli test :::: " << " 333 = " << 333 << std::endl;
|
|
|
+ if (p_cloud->size() <= 0)
|
|
|
+ return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
+
|
|
|
+ pass.setInputCloud(p_cloud);
|
|
|
+ pass.setFilterFieldName("y"); //设置想在哪个坐标轴上操作
|
|
|
+ pass.setFilterLimits(m_region_box.y_min, m_region_box.y_max); //将x轴的0到1范围内
|
|
|
+ pass.setFilterLimitsNegative(false); //保留（true就是删除，false就是保留而删除此区间外的）
|
|
|
+ pass.filter(*p_cloud); //输出到结果指针
|
|
|
+ std::cout << " huli test :::: " << " 444 = " << 444 << std::endl;
|
|
|
+ if (p_cloud->size() <= 0)
|
|
|
+ return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
+ std::cout << " huli test :::: " << " 445 = " << 445 << std::endl;
|
|
|
+ // 3.离群点过滤
|
|
|
+ pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
|
|
|
+ sor.setInputCloud(p_cloud);
|
|
|
+ sor.setMeanK(15); //K近邻搜索点个数
|
|
|
+ sor.setStddevMulThresh(3.0); //标准差倍数
|
|
|
+ sor.setNegative(false); //保留未滤波点（内点）
|
|
|
+ sor.filter(*p_cloud); //保存滤波结果到cloud_filter
|
|
|
+ std::cout << " huli test :::: " << " 555 = " << 555 << std::endl;
|
|
|
+ if (p_cloud->size() <= 0)
|
|
|
+ return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
+ else
|
|
|
+ return Error_manager(Error_code::SUCCESS);
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -74,7 +74,7 @@ std::cout << " huli test :::: " << " 555 = " << 555 << std::endl;
|
|
|
* */
|
|
|
double distance(cv::Point2f p1, cv::Point2f p2)
|
|
|
{
|
|
|
- return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
|
|
|
+ return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -85,204 +85,204 @@ double distance(cv::Point2f p1, cv::Point2f p2)
|
|
|
* */
|
|
|
Error_manager Region_detector::isRect(std::vector<cv::Point2f> &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 > 2.100 && length < 3.000 && width > 1.400 && width < 2.100)
|
|
|
- {
|
|
|
- //生成第四个点
|
|
|
- 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);
|
|
|
- }
|
|
|
+ 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 > 2.100 && length < 3.000 && width > 1.400 && width < 2.100)
|
|
|
+ {
|
|
|
+ //生成第四个点
|
|
|
+ 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<pcl::PointXYZ>::Ptr cloud_in,
|
|
|
- std::vector<pcl::PointCloud<pcl::PointXYZ>> &seg_clouds, bool print)
|
|
|
+ std::vector<pcl::PointCloud<pcl::PointXYZ>> &seg_clouds, bool print)
|
|
|
{
|
|
|
- // 1.判断参数合法性
|
|
|
- if (cloud_in->size() <= 0)
|
|
|
- return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
-
|
|
|
- // 2.聚类
|
|
|
- pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
|
|
|
- kdtree->setInputCloud(cloud_in);
|
|
|
-
|
|
|
- pcl::EuclideanClusterExtraction<pcl::PointXYZ> 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.5);
|
|
|
- // 设置聚类的小点数和最大点云数
|
|
|
- clustering.setMinClusterSize(10);
|
|
|
- clustering.setMaxClusterSize(500);
|
|
|
- clustering.setSearchMethod(kdtree);
|
|
|
- clustering.setInputCloud(cloud_in);
|
|
|
- std::vector<pcl::PointIndices> 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<pcl::PointXYZ>());
|
|
|
- }
|
|
|
-
|
|
|
- int j = 0;
|
|
|
- for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
|
|
|
- {
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
- //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
|
|
|
- for (std::vector<int>::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;
|
|
|
+ // 1.判断参数合法性
|
|
|
+ if (cloud_in->size() <= 0)
|
|
|
+ return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
+
|
|
|
+ // 2.聚类
|
|
|
+ pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
|
|
|
+ kdtree->setInputCloud(cloud_in);
|
|
|
+
|
|
|
+ pcl::EuclideanClusterExtraction<pcl::PointXYZ> 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.5);
|
|
|
+ // 设置聚类的小点数和最大点云数
|
|
|
+ clustering.setMinClusterSize(10);
|
|
|
+ clustering.setMaxClusterSize(500);
|
|
|
+ clustering.setSearchMethod(kdtree);
|
|
|
+ clustering.setInputCloud(cloud_in);
|
|
|
+ std::vector<pcl::PointIndices> 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<pcl::PointXYZ>());
|
|
|
+ }
|
|
|
+
|
|
|
+ int j = 0;
|
|
|
+ for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
|
|
|
+ {
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
|
|
|
+ for (std::vector<int>::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;
|
|
|
}
|
|
|
|
|
|
|
|
|
@@ -294,80 +294,80 @@ Error_manager Region_detector::clustering_only(pcl::PointCloud<pcl::PointXYZ>::P
|
|
|
* */
|
|
|
Error_manager Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f> &corner_points, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, bool print)
|
|
|
{
|
|
|
- // 1.判断参数合法性
|
|
|
- if (cloud_in->size() <= 0)
|
|
|
- return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
-
|
|
|
- // 2.聚类
|
|
|
- pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
|
|
|
- kdtree->setInputCloud(cloud_in);
|
|
|
-
|
|
|
- pcl::EuclideanClusterExtraction<pcl::PointXYZ> 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<pcl::PointIndices> 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<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
|
|
|
- }
|
|
|
-
|
|
|
- int j = 0;
|
|
|
- for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
|
|
|
- {
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
- //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
|
|
|
- for (std::vector<int>::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);
|
|
|
+ // 1.判断参数合法性
|
|
|
+ if (cloud_in->size() <= 0)
|
|
|
+ return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
|
|
|
+
|
|
|
+ // 2.聚类
|
|
|
+ pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
|
|
|
+ kdtree->setInputCloud(cloud_in);
|
|
|
+
|
|
|
+ pcl::EuclideanClusterExtraction<pcl::PointXYZ> 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<pcl::PointIndices> 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<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
|
|
|
+ }
|
|
|
+
|
|
|
+ int j = 0;
|
|
|
+ for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
|
|
|
+ {
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
|
|
|
+ for (std::vector<int>::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);
|
|
|
}
|
|
|
|
|
|
|
|
|
@@ -375,24 +375,24 @@ Error_manager Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
|
|
|
|
|
|
// 检测传入点云，识别为四类返回中心点、角度、轮距与宽度
|
|
|
Error_manager Region_detector::detect(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
|
|
|
-Common_data::Car_wheel_information& car_wheel_information, bool print)
|
|
|
+ Common_data::Car_wheel_information& car_wheel_information, bool print)
|
|
|
{
|
|
|
//整个函数都加锁, 这里面非法修改了输入点云, 是为了将筛选点云的结构传出, 然后点云存txt文件
|
|
|
std::unique_lock<std::mutex> t_lock(*p_cloud_mutex);
|
|
|
Error_manager result = Error_manager(Error_code::SUCCESS);
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
*p_cloud_filtered = *p_cloud_in;
|
|
|
|
|
|
- // 1.预处理
|
|
|
- result = preprocess(p_cloud_filtered);
|
|
|
- if (result != SUCCESS)
|
|
|
- return result;
|
|
|
+ // 1.预处理
|
|
|
+ result = preprocess(p_cloud_filtered);
|
|
|
+ if (result != SUCCESS)
|
|
|
+ return result;
|
|
|
|
|
|
- // 2.聚类计算角点
|
|
|
- std::vector<cv::Point2f> corner_points;
|
|
|
- std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
|
|
|
- result = clustering(p_cloud_filtered, corner_points, seg_clouds, print);
|
|
|
- if (result != SUCCESS)
|
|
|
+ // 2.聚类计算角点
|
|
|
+ std::vector<cv::Point2f> corner_points;
|
|
|
+ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
|
|
|
+ result = clustering(p_cloud_filtered, corner_points, seg_clouds, print);
|
|
|
+ if (result != SUCCESS)
|
|
|
return result;
|
|
|
// 修改原始点云为预处理后点云，并加入角点供查验
|
|
|
|
|
|
@@ -407,89 +407,89 @@ Common_data::Car_wheel_information& car_wheel_information, bool print)
|
|
|
std::vector<cv::Point2f> all_points;
|
|
|
// car center
|
|
|
for (int j = 0; j < p_cloud_filtered->size(); ++j)
|
|
|
- {
|
|
|
- all_points.push_back(cv::Point2f(p_cloud_filtered->points[j].x, p_cloud_filtered->points[j].y));
|
|
|
- }
|
|
|
- // find bounding rectangle of all wheel points
|
|
|
- cv::RotatedRect wheel_box = cv::minAreaRect(all_points);
|
|
|
+ {
|
|
|
+ all_points.push_back(cv::Point2f(p_cloud_filtered->points[j].x, p_cloud_filtered->points[j].y));
|
|
|
+ }
|
|
|
+ // find bounding rectangle of all wheel points
|
|
|
+ cv::RotatedRect wheel_box = cv::minAreaRect(all_points);
|
|
|
car_wheel_information.center_x = wheel_box.center.x;
|
|
|
car_wheel_information.center_y = wheel_box.center.y;
|
|
|
- // add center point to the input cloud for further check
|
|
|
+ // add center point to the input cloud for further check
|
|
|
p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.center_x, car_wheel_information.center_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));
|
|
|
+ // 长边向量
|
|
|
+ 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));
|
|
|
car_wheel_information.car_angle = 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 << car_wheel_information.center_x, car_wheel_information.center_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;
|
|
|
+ // 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 << car_wheel_information.center_x, car_wheel_information.center_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;
|
|
|
car_wheel_information.wheel_base = fabs(y_values1 - y_values0);
|
|
|
car_wheel_information.wheel_width = fabs(min_x - max_x);
|
|
|
// LOG(INFO) << "--------detector find width, wheelbase: " << width << ", " << wheelbase << ", y means: [" << y_values0 << ", " << y_values1 << "] -----";
|
|
|
@@ -497,18 +497,18 @@ Common_data::Car_wheel_information& car_wheel_information, bool print)
|
|
|
p_cloud_in->points.push_back(pcl::PointXYZ(max_x, car_wheel_information.center_y, 0.0));
|
|
|
p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.center_x, y_values0, 0.0));
|
|
|
p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.center_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);
|
|
|
+ }
|
|
|
+ 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);
|
|
|
car_wheel_information.wheel_base = std::max(wheel_center_box.size.width, wheel_center_box.size.height);
|
|
|
car_wheel_information.wheel_width = std::min(wheel_box.size.width, wheel_box.size.height);
|
|
|
- }
|
|
|
+ }
|
|
|
|
|
|
- return Error_manager(Error_code::SUCCESS);
|
|
|
+ return Error_manager(Error_code::SUCCESS);
|
|
|
}
|
|
|
|
|
|
|
|
|
@@ -536,8 +536,12 @@ Error_manager Region_detector::detect_ex(std::mutex* p_cloud_mutex, pcl::PointCl
|
|
|
std::cout << " huli test :::: " << " 112 = " << 112 << std::endl;
|
|
|
detect_wheel_ceres::Detect_result detect_result;
|
|
|
if(!m_detector_ceres.detect(seg_clouds, detect_result))
|
|
|
+ {
|
|
|
return Error_manager(Error_code::WJ_REGION_CLUSTER_SIZE_ERROR);
|
|
|
- else {
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::unique_lock<std::mutex> t_lock(*p_cloud_mutex);
|
|
|
car_wheel_information.center_x = detect_result.cx;
|
|
|
car_wheel_information.center_y = detect_result.cy;
|
|
|
car_wheel_information.car_angle = detect_result.theta;
|
