1、算法测量模块cut line优化；

Modules/MeasureNodeAlgTest/alg.cpp

@@ -166,6 +166,7 @@ Error_manager GroundRegion::init(velodyne::Region region, pcl::PointCloud<pcl::P
     mp_cloud_detect_z = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
     m_surface_hull = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
     m_pose_record.cy = -1e8;
+    m_cut_line = m_region.minz() + 0.07;
     // 创建多边形区域指针，用于裁剪点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr(new pcl::PointCloud<pcl::PointXYZ>);
     printf("%d\n", m_region.cut_box().cut_points_size());
@@ -404,6 +405,7 @@ GroundRegion::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_cer
         m_filtered_cloud_mutex.lock();
         mp_cloud_filtered->clear();
         m_filtered_cloud_mutex.unlock();
+        m_cut_line = m_region.minz() + 0.07;
         return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "no input point");
     }
 
@@ -415,7 +417,7 @@ GroundRegion::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_cer
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
     ret = ClippingAndFilterCloud(cloud, cloud_filtered);
     if (ret != SUCCESS) {
-//        LOG(WARNING) << ret.to_string();
+        m_cut_line = m_region.minz() + 0.07;
         return ret;
     }
 
@@ -439,14 +441,17 @@ GroundRegion::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_cer
 //    LOG(INFO) << "m_pose_record.cy " << m_pose_record.cy << ", roughly_pose.cy " << roughly_pose.cy;
 //    LOG(INFO) << fabs(m_pose_record.cy - roughly_pose.cy) << " " << m_cut_line;
     if (m_pose_record.success) {
-        m_pose_record.loss.total_avg_loss > 0.013 ? m_cut_line -= 0.01 : m_cut_line += 0.01;
+        // cut line 调整
+        m_cut_line < m_region.minz() + 0.04 ? m_cut_line += 0.05 : false;
+        m_pose_record.loss.total_avg_loss > 0.03 ? m_cut_line -= 0.01 : false;
+        m_pose_record.loss.total_avg_loss > 0.02 ? m_cut_line -= 0.01 : false;
         m_pose_record.loss.total_avg_loss < 0.005 ? m_cut_line += 0.01 : false;
-        if (m_cut_line > m_region.minz() + 0.2 || m_cut_line < m_region.minz()) {
-            m_cut_line = m_region.minz() + 0.2;
-        }
+        m_pose_record.loss.total_avg_loss < 0.003 ? m_cut_line += 0.01 : false;
     } else {
-        m_cut_line += m_region.minz() + 0.2;
+        m_cut_line += 0.07;
     }
+    m_cut_line = MIN(m_cut_line, m_region.minz() + 0.2);
+    m_cut_line = MAX(m_cut_line, m_region.minz());
 
 //    coast_t = std::chrono::steady_clock::now() - t;
 //    LOG(INFO) << "get car_clean_cloud coast time: " << coast_t.count() * 1000 << "ms";
@@ -478,9 +483,10 @@ GroundRegion::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_cer
 //        LOG(INFO) << "after classify_ceres_detect coast time: " << coast_t.count() * 1000 << "ms";
 
         if (ret) {
-            if (!results.empty() && CompareCeresResult(results.back(), result)) {
+            if (!results.empty()/* && CompareCeresResult(results.back(), result)*/) {
                 last_result = results.back().loss.total_avg_loss < result.loss.total_avg_loss ? results.back() : result;
                 results.push_back(result);
+                m_cut_line -= 0.01; // 为了抵消只成功一次导致cut line的削减，这里也削减一次
                 break;
             }
             results.push_back(result);
@@ -488,6 +494,7 @@ GroundRegion::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_cer
         index_z_detect++;
         m_cut_line -= 0.01;
     }
+    m_cut_line += 0.01; // 抵消cut line 未检测过的削减
 
     if (results.size() == 1) {
         last_result = results.back();