car width accuracy improvement

velodyne_lidar/ground_region.cpp

@@ -578,6 +578,46 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
         return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, valid_info);
     }
 
+    // 车宽精度优化
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_width_extract_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+        t_width_extract_cloud->operator+=(*mp_cloud_filtered);
+
+        //离群点过滤
+        sor.setInputCloud(t_width_extract_cloud);
+        sor.setMeanK(5);             //K近邻搜索点个数
+        sor.setStddevMulThresh(1.0); //标准差倍数
+        sor.setNegative(false);      //保留未滤波点（内点）
+        sor.filter(*t_width_extract_cloud); //保存滤波结果到cloud_filter
+
+        Eigen::Affine3d t_affine = Eigen::Affine3d::Identity();
+        t_affine.prerotate(Eigen::AngleAxisd((90 - last_result.theta) * M_PI / 180.0, Eigen::Vector3d::UnitZ()));
+        pcl::transformPointCloud(*t_width_extract_cloud, *t_width_extract_cloud, t_affine.matrix());
+
+        // 车宽重计算，并赋值到当前车宽
+        pcl::PointXYZ t_min_p, t_max_p;
+        pcl::getMinMax3D(*t_width_extract_cloud, t_min_p, t_max_p);
+
+        double accurate_width = t_max_p.x - t_min_p.x;
+        last_result.width = accurate_width;
+
+        // !!!暂时不限制宽度数据
+        // char valid_info[255];
+        // sprintf(valid_info, "validation for car width, origin/accurate: (%.3f, %.3f)", last_result.width, accurate_width);
+        // // 允许一边5cm误差，且保证车宽应比车轮识别计算出的大，否则为错误宽度数据
+        // if (accurate_width - last_result.width > 0.1 || accurate_width < last_result.width)
+        // {
+        //     return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, valid_info);
+        // }
+        // if (m_region.region_id() == 0 || m_region.region_id() == 4)
+        // {
+        //     LOG(WARNING) << valid_info;
+        //     // 保存车宽数据，统计用
+        //     if (m_file_out.is_open())
+        //         m_file_out << std::setprecision(5) << std::to_string(1.883) << "," << last_result.width << "," << accurate_width << std::endl;
+        // }
+    }
+
     // LOG_IF(INFO, m_region.region_id() == 4) << last_result.to_string();
 
     return SUCCESS;