|
@@ -34,7 +34,6 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
|
|
|
// 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);
|
|
@@ -42,7 +41,6 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
|
|
|
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);
|
|
|
|
|
@@ -51,10 +49,10 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
|
|
|
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;
|
|
|
+// std::cout << p_cloud->size()<<","<<m_region_box.x_min<<","<<m_region_box.x_max<<","<<m_region_box.y_min<<","<<m_region_box.y_max<<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);
|
|
@@ -62,7 +60,7 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
|
|
|
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
|
|
@@ -527,13 +525,13 @@ Error_manager Region_detector::detect_ex(std::mutex* p_cloud_mutex, pcl::PointCl
|
|
|
result = preprocess(p_cloud_filtered);
|
|
|
if (result != SUCCESS)
|
|
|
return result;
|
|
|
- std::cout << " huli test :::: " << " 111 = " << 111 << std::endl;
|
|
|
+// std::cout << " huli test :::: " << " 111 = " << 111 << std::endl;
|
|
|
// 2.聚类
|
|
|
std::vector<pcl::PointCloud<pcl::PointXYZ>> seg_clouds;
|
|
|
result = clustering_only(p_cloud_filtered, seg_clouds, print);
|
|
|
if (result != SUCCESS)
|
|
|
return result;
|
|
|
- std::cout << " huli test :::: " << " 112 = " << 112 << std::endl;
|
|
|
+// std::cout << " huli test :::: " << " 112 = " << 112 << std::endl;
|
|
|
detect_wheel_ceres::Detect_result detect_result;
|
|
|
if(!m_detector_ceres.detect(seg_clouds, detect_result))
|
|
|
{
|