add region detect time check

tests/vlp16_driver_test.cpp

@@ -2,7 +2,7 @@
  * @Description: vlp16驱动测试
  * @Author: yct
  * @Date: 2021-07-26 14:11:26
- * @LastEditTime: 2021-08-01 15:51:10
+ * @LastEditTime: 2021-11-12 12:54:33
  * @LastEditors: yct
  */
 
@@ -113,54 +113,35 @@ void device_test()
 #include "../velodyne_lidar/ground_region.h"
 #include "../tool/measure_filter.h"
 #include "../tool/point_tool.h"
+#include "../tool/proto_tool.h"
+
 void region_detect()
 {
-    std::ifstream in("/home/youchen/extra_space/chutian/measure/puai_wj_2021/build/comb_calib.txt");
-    if(!in.is_open())
+    std::string exce = "../tests/region_4_26x.txt", envi = "../tests/region_4_env.txt";
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_exce(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_envi(new pcl::PointCloud<pcl::PointXYZ>);
+    bool res = read_pointcloud(exce, cloud_exce);
+    res &= read_pointcloud(envi, cloud_envi);
+    if (!res)
     {
         std::cout << "cannot open file to read cloud." << std::endl;
         return;
     }
     else
     {
-        // 准备点云
-        pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-        while(!in.eof())
+        velodyne::velodyneManagerParams t_velo_params;
+        std::string prototxt_path = "../setting/velodyne_manager.prototxt";
+        if (!proto_tool::read_proto_param(prototxt_path, t_velo_params) || t_velo_params.region_size()<2)
         {
-            std::string t_linestr;
-            if (getline(in, t_linestr))
-            {
-                std::vector<std::string> str_vector;
-                std::stringstream ss(t_linestr);
-                std::string num_str;
-                while (getline(ss, num_str, ' '))
-                {
-                    str_vector.push_back(num_str);
-                }
-                if (str_vector.size() != 3)
-                {
-                    std::cout << "unsupported point cloud / cannot find point x y z value " << std::endl;
-                    return;
-                }
-                pcl::PointXYZ t_point;
-                t_point.x = stod(str_vector[0]);
-                t_point.y = stod(str_vector[1]);
-                t_point.z = stod(str_vector[2]);
-                t_cloud->push_back(t_point);
-            }
+            return;
         }
+
         // 初始化region
         velodyne::Region param;
-        param.set_minx(-3.10);
-        param.set_maxx(0);
-        param.set_miny(-2.75);
-        param.set_maxy(3.08);
-        param.set_minz(0.01);
-        param.set_maxz(0.2);
-        param.set_region_id(0);
+        param.CopyFrom(t_velo_params.region(1));
         pcl::PointCloud<pcl::PointXYZ>::Ptr left = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
         pcl::PointCloud<pcl::PointXYZ>::Ptr right = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
-        if (!read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/left_model.txt", left) || !read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/right_model.txt", right))
+        if (!read_pointcloud("../setting/left_model.txt", left) || !read_pointcloud("../setting/right_model.txt", right))
         {
             std::cout << "cannot read left/right model " << std::endl;
             return;
@@ -170,24 +151,35 @@ void region_detect()
         t_region.init(param, left, right);
         std::cout << "after init" << std::endl;
         // 测试region功能
-        detect_wheel_ceres3d::Detect_result t_result;
-        Error_manager ec = t_region.detect(t_cloud, t_result);
-        std::cout << ec.to_string() << std::endl;
-        std::cout << "---------------" << std::endl;
-        for (size_t i = 0; i < 12; i++)
+        // 手动检测，测试计算耗时
+        for (size_t i = 0; i < 600; i++)
         {
-            t_region.update_cloud(t_cloud);
-            usleep(1000*100);
+            detect_wheel_ceres3d::Detect_result t_result;
+            Error_manager ec = t_region.detect(cloud_exce, t_result);
+            std::cout << ec.to_string() << std::endl;
+            usleep(1000 * 100);
+
+            ec = t_region.detect(cloud_envi, t_result);
+            std::cout << ec.to_string() << std::endl;
+            usleep(1000 * 100);
         }
 
-        Common_data::Car_wheel_information t_wheel_info;
+        // 检测自动识别功能
+        // std::cout << "---------------" << std::endl;
+        // for (size_t i = 0; i < 12; i++)
+        // {
+        //     t_region.update_cloud(t_cloud);
+        //     usleep(1000*100);
+        // }
 
-        ec = Measure_filter::get_instance_references().get_filtered_wheel_information(0, t_wheel_info);
-        if (ec == SUCCESS)
-        {
-            std::cout << t_wheel_info.to_string() << std::endl;
-        }
-        else { std::cout << ec.to_string() << std::endl; }
+        // Common_data::Car_wheel_information t_wheel_info;
+
+        // ec = Measure_filter::get_instance_references().get_filtered_wheel_information(0, t_wheel_info);
+        // if (ec == SUCCESS)
+        // {
+        //     std::cout << t_wheel_info.to_string() << std::endl;
+        // }
+        // else { std::cout << ec.to_string() << std::endl; }
     }
 }
 
@@ -314,9 +306,9 @@ int main()
 {
     // velo_driver_test();
     // device_test();
-    // region_detect();
+    region_detect();
     // velo_manager_test();
-    message_test();
+    // message_test();
 
     return 0;
 }

velodyne_lidar/ground_region.cpp

@@ -428,7 +428,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     // 给予底盘z中心与高度初值
     double mid_z = 0.05, height = 0.08;
     z_solver_x = 0.0;
-    // Error_manager ec = t_solver.solve(cloud_z_solver, x, mid_z, width, height);
+    // Error_manager ec = t_solver.solve(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height);
     Error_manager ec = t_solver.solve_mat(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height, false);
     // 切除大于height高度以外点，并显示width直线
     // 根据z值切原始点云