!! pcl-1.7 mulfunction in compute3DCentroid !!

close some print, can be opened any time.

tool/common_data.h

@@ -120,7 +120,7 @@ public:
 		std::string to_string()
         {
             char buf[512]={0};
-            sprintf(buf, "%.4f %.4f %.4f %.4f %.4f %.4f\n", car_center_x, car_center_y, car_angle, car_wheel_base, car_wheel_width, car_front_theta);
+            sprintf(buf, "cx:%.4f cy:%.4f ang:%.4f wb:%.4f w:%.4f fr:%.4f", car_center_x, car_center_y, car_angle, car_wheel_base, car_wheel_width, car_front_theta);
             return std::string(buf);
         }
 

velodyne_lidar/ground_region.cpp

@@ -264,7 +264,7 @@ bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
         }
         else
         {
-            LOG(WARNING) << error_str;
+            // LOG(WARNING) << error_str;
             return false;
         }
     }
@@ -357,7 +357,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
     sor.setInputCloud(cloud_filtered);
     sor.setMeanK(15);            //K近邻搜索点个数
-    sor.setStddevMulThresh(1.0); //标准差倍数
+    sor.setStddevMulThresh(3.0); //标准差倍数
     sor.setNegative(false);      //保留未滤波点（内点）
     sor.filter(*cloud_filtered); //保存滤波结果到cloud_filter
 
@@ -367,7 +367,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     //下采样
     pcl::ApproximateVoxelGrid<pcl::PointXYZ> vox;   //创建滤波对象
     vox.setInputCloud(cloud_filtered);   //设置需要过滤的点云给滤波对象
-    vox.setLeafSize(0.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体
+    vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体
     vox.filter(*cloud_filtered);         //执行滤波处理，存储输出
 
     if (cloud_filtered->size() == 0)
@@ -386,7 +386,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     {
         return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "find general car pose value failed.");
     }
-    LOG(INFO) << "car pose :::: cx: " << x+m_region.plc_offsetx() << ", cy: " << y+m_region.plc_offsety() << ", theta: " << theta*180.0/M_PI << std::endl;
+    // LOG(INFO) << "car pose :::: cx: " << x+m_region.plc_offsetx() << ", cy: " << y+m_region.plc_offsety() << ", theta: " << theta*180.0/M_PI << std::endl;
 
     std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
     std::chrono::duration<double> time_used_bowl = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
@@ -406,7 +406,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     //离群点过滤
     sor.setInputCloud(cloud_z_solver);
     sor.setMeanK(15);            //K近邻搜索点个数
-    sor.setStddevMulThresh(1.0); //标准差倍数
+    sor.setStddevMulThresh(3.0); //标准差倍数
     sor.setNegative(false);      //保留未滤波点（内点）
     sor.filter(*cloud_z_solver); //保存滤波结果到cloud_filter
 
@@ -456,11 +456,11 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     {
         return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, (std::string("optimized chassis z value out of range: ")+std::to_string(chassis_z)).c_str());
     }
-    bool ret = false;
+    bool ret = false; 
     
     while (chassis_z > (mid_z))
     {
-        LOG_IF(WARNING, m_region.region_id() == 4) << "chassis z: " << chassis_z;
+        // LOG_IF(WARNING, m_region.region_id() == 4) << "chassis z: " << chassis_z;
         ret = classify_ceres_detect(cloud_filtered, chassis_z, result);
         // changed by yct, 根据底盘z识别，失败则向下移动直至成功或超过底盘内空中点
         if(ret)
@@ -479,11 +479,11 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
 
     if (results.size() == 0)
     {
-        // 20201010, may lead to problem in chutian, uncomment in debug only
-        // changed by yct, save 3d wheel detect result. 
-        static int save_debug = 0;
-        if (save_debug++ == 5)
-            m_detector->save_debug_data("/home/youchen/extra_space/chutian/measure/chutian_velo_ws/log/debug");
+        // // 20201010, may lead to problem in chutian, uncomment in debug only
+        // // changed by yct, save 3d wheel detect result. 
+        // static int save_debug = 0;
+        // if (save_debug++ == 5)
+        //     m_detector->save_debug_data("/home/youchen/extra_space/chutian/measure/chutian_velo_ws/log/debug");
 
         // std::cout << "\n-------- no result: " << std::endl;
         LOG_IF(INFO, m_region.region_id() == 4) << "failed with midz, currz: " << mid_z << ", " << chassis_z;

velodyne_lidar/match3d/detect_wheel_ceres3d.cpp

@@ -317,6 +317,9 @@ bool detect_wheel_ceres3d::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Pt
     pcl::compute3DCentroid(cloud_all, centroid);
     double center_x = centroid[0];
     double center_y = centroid[1];
+    // printf("3dwheel x,y:%.3f, %.3f\n", center_x, center_y);
+    // save_cloud_txt(cloud_all.makeShared(), "total_wheel.txt");
+
     //计算外接旋转矩形
     std::vector<cv::Point2f> points_cv;
     for (int i = 0; i < cloud_all.size(); ++i)
@@ -343,7 +346,7 @@ bool detect_wheel_ceres3d::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Pt
         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));
-    printf("rect theta: %.3f\n",angle_x);
+    // printf("rect theta: %.3f\n",angle_x);
     //第二步, 将每份点云旋转回去,计算点云重心所在象限
     for (int i = 0; i < cloud_vec.size(); ++i)
     {
@@ -564,8 +567,8 @@ bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss
     double init_theta_front=0*M_PI/180.0;
 
     double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front};
-    printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
-            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
+    // printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+    //         variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
 
     // 第二部分：构建寻优问题
     ceres::Problem problem;
@@ -582,7 +585,7 @@ bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss
 
     //第三部分： 配置并运行求解器
     ceres::Solver::Options options;
-    options.use_nonmonotonic_steps=false;
+    // options.use_nonmonotonic_steps=false;
     options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
     //options.logging_type = ceres::LoggingType::SILENT;
     options.max_num_iterations=500;
@@ -591,8 +594,8 @@ bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss
     ceres::Solver::Summary summary;//优化信息
     ceres::Solve(options, &problem, &summary);//求解!!!
 
-    printf("after solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
-            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
+    // printf("after solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+    //         variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
 
     double loss=summary.final_cost/(m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());