2021 use filter for wheelbase.

init value alterations.

main.cpp

@@ -64,7 +64,7 @@ int main(int argc,char* argv[])
 	gp_visualizer->addCoordinateSystem();
 	gp_visualizer->setBackgroundColor(0, 0, 0);
 	gp_visualizer->initCameraParameters();
-	gp_visualizer->addPointCloud(t_cloud, "region4");
+	gp_visualizer->addPointCloud(t_cloud, "region");
 #endif
 
 	const char* logPath = "./log/";
@@ -130,13 +130,13 @@ int main(int argc,char* argv[])
 		std::map<int, Ground_region *> t_ground_region_map = Velodyne_manager::get_instance_references().get_ground_region_map();
 		for (auto iter = t_ground_region_map.begin(); iter != t_ground_region_map.end(); ++iter)
 		{
-			if (iter->first == 4)
+			if (iter->first == DISP_TERM_ID)
 			{
 				t_region_cloud->clear();
 				// 获取区域点云
 				iter->second->get_region_cloud(t_region_cloud, Ground_region::Region_cloud_type::filtered);
 				// gp_visualizer->showCloud(t_region_cloud);
-				gp_visualizer->updatePointCloud(t_region_cloud, "region4");
+				gp_visualizer->updatePointCloud(t_region_cloud, "region");
 				// LOG(INFO) << "pcl cloud updated." <<t_region_cloud->size();
 			}
 		}

setting/velodyne_manager.prototxt

@@ -340,7 +340,7 @@ region
 # 0 region
 region
 {
-    minx:-1.5
+    minx:-1.1
 	maxx:1.5
 	miny:-2.6
 	maxy:2.3

setting/velodyne_manager_chutian.prototxt

@@ -4,6 +4,8 @@ left_model_path:"/home/zx/yct/chutian_measure_2021/setting/left_model.txt"
 right_model_path:"/home/zx/yct/chutian_measure_2021/setting/right_model.txt"
 distribution_mode:false
 
+#-----------------------------------lidars, id 0-6 from A1-C2
+
 # 6 lidar 192.168.1.201
 velodyne_lidars
 {
@@ -56,7 +58,7 @@ velodyne_lidars
     }
 }
 
-# 3 lidar 192.168.1.203
+# 4 lidar 192.168.1.203
 velodyne_lidars
 {
     ip:""
@@ -78,7 +80,77 @@ velodyne_lidars
     }
 }
 
-# 1 region
+# 3 lidar 192.168.1.204
+
+# 2 lidar 192.168.1.205
+velodyne_lidars
+{
+    ip:""
+    port:2372
+    model:"VLP16"
+    calibrationFile:"../setting/VLP16db.yaml"
+    lidar_id:2
+    max_range:8.0
+    min_range:0.1
+    min_angle:0
+    max_angle:360
+    rpm:600
+    calib
+    {
+        r:-0.1517
+        p:1.37069
+        y:86.3796
+        cz:0.0461
+    }
+}
+
+# 1 lidar 192.168.1.206
+velodyne_lidars
+{
+    ip:""
+    port:2373
+    model:"VLP16"
+    calibrationFile:"../setting/VLP16db.yaml"
+    lidar_id:1
+    max_range:8.0
+    min_range:0.1
+    min_angle:0
+    max_angle:360
+    rpm:600
+    calib
+    {
+        r:-0.01438
+        p:0.108841
+        y:91.3879
+        cz:0.0686
+    }
+}
+
+# 0 lidar 192.168.1.207
+velodyne_lidars
+{
+    ip:""
+    port:2374
+    model:"VLP16"
+    calibrationFile:"../setting/VLP16db.yaml"
+    lidar_id:0
+    max_range:8.0
+    min_range:0.1
+    min_angle:0
+    max_angle:360
+    rpm:600
+    calib
+    {
+        r:0.03026
+        p:0.68354
+        y:90.6538
+        cz:0.07697
+    }
+}
+
+#-----------------------------------regions, 0-5 from A1 to C2
+
+# 5 region
 region
 {
     minx:-1.6
@@ -94,7 +166,14 @@ region
     border_maxx:1.3
     plc_offsetx:1.913
     plc_offsety:-6.078
-    plc_border_miny:-8.0
+    plc_border_miny:-7.51
+    car_min_width:1.55
+    car_max_width:1.95
+    car_min_wheelbase:2.3
+    car_max_wheelbase:3.15
+    turnplate_angle_limit_anti_clockwise:5.3
+    turnplate_angle_limit_clockwise:5.3
+
     lidar_exts
     {
         lidar_id:6
@@ -117,7 +196,7 @@ region
     }
 }
 
-# 2 region
+# 4 region
 # 利用共用雷达标定plc偏移量
 # plc_4 = plc_5 + calib5in5 - calib5in4
 # plc_4x = 1.913 + (-1.9018) - (1.9095) = -1.8983
@@ -137,7 +216,14 @@ region
     border_maxx:1.3
     plc_offsetx:-1.8783
     plc_offsety:-6.11489
-    plc_border_miny:-8.0
+    plc_border_miny:-7.51
+    car_min_width:1.55
+    car_max_width:1.95
+    car_min_wheelbase:2.3
+    car_max_wheelbase:3.15
+    turnplate_angle_limit_anti_clockwise:5.3
+    turnplate_angle_limit_clockwise:5.3
+
     # 4-->5: -3.81895 0.038776
     lidar_exts
     {
@@ -158,3 +244,140 @@ region
         }
     }
 }
+
+# 3 region
+
+# # 2 region
+# region
+# {
+#     minx:-1.5
+# 	maxx:1.5
+# 	miny:-2.6
+# 	maxy:2.3
+# 	minz:0.03
+# 	maxz:0.5
+#     region_id:2
+#     turnplate_cx:0.0
+#     turnplate_cy:0.0
+#     border_minx:-1.3
+#     border_maxx:1.3
+#     plc_offsetx:0.0
+#     plc_offsety:0.0
+#     plc_border_miny:-7.51
+#     car_min_width:1.55
+#     car_max_width:1.95
+#     car_min_wheelbase:2.3
+#     car_max_wheelbase:3.15
+#     turnplate_angle_limit_anti_clockwise:5.3
+#     turnplate_angle_limit_clockwise:5.3
+
+#     # 2-->3: 
+#     lidar_exts
+#     {
+#         lidar_id:3
+#         calib
+#         {
+#             cx:0.0
+#             cy:0.0
+#         }
+#     }
+#     lidar_exts
+#     {
+#         lidar_id:2
+#         calib
+#         {
+#             cx:0.0
+#             cy:0.0
+#         }
+#     }
+# }
+
+# 1 region
+region
+{
+    minx:-1.5
+	maxx:1.5
+	miny:-2.6
+	maxy:2.3
+	minz:0.03
+	maxz:0.5
+    region_id:1
+    turnplate_cx:0.0
+    turnplate_cy:0.0
+    border_minx:-1.3
+    border_maxx:1.3
+    plc_offsetx:0.0
+    plc_offsety:0.0
+    plc_border_miny:-7.51
+    car_min_width:1.55
+    car_max_width:1.95
+    car_min_wheelbase:2.3
+    car_max_wheelbase:3.15
+    turnplate_angle_limit_anti_clockwise:5.3
+    turnplate_angle_limit_clockwise:5.3
+
+    # 2-->1: 3.82222438  -0.106852
+    lidar_exts
+    {
+        lidar_id:2
+        calib
+        {
+            cx:1.91111219
+            cy:-0.053426
+        }
+    }
+    lidar_exts
+    {
+        lidar_id:1
+        calib
+        {
+            cx:-1.91111219
+            cy:0.053426
+        }
+    }
+}
+
+# 0 region
+region
+{
+    minx:-1.5
+	maxx:1.5
+	miny:-2.6
+	maxy:2.3
+	minz:0.03
+	maxz:0.5
+    region_id:0
+    turnplate_cx:0.0
+    turnplate_cy:0.0
+    border_minx:-1.3
+    border_maxx:1.3
+    plc_offsetx:0.0
+    plc_offsety:0.0
+    plc_border_miny:-7.51
+    car_min_width:1.55
+    car_max_width:1.95
+    car_min_wheelbase:2.3
+    car_max_wheelbase:3.15
+    turnplate_angle_limit_anti_clockwise:5.3
+    turnplate_angle_limit_clockwise:5.3
+
+    # 1-->0: 3.8173811 -0.0273465
+    lidar_exts
+    {
+        lidar_id:1
+        calib
+        {
+            cx:1.9087
+            cy:-0.01367
+        }
+    }
+    lidar_exts
+    {
+        lidar_id:0
+        calib
+        {
+            cx:-1.9087
+            cy:0.01367
+        }
+    }
+}

system/system_executor.cpp

@@ -368,8 +368,8 @@ Error_manager System_executor::encapsulate_send_status()
 
 		std::string t_msg = t_multi_status_msg.SerializeAsString();
 		System_communication::get_instance_references().encapsulate_msg(t_msg);
-		// if(t_multi_status_msg.id_struct().terminal_id()==4)
-        // 	std::cout<<t_multi_status_msg.DebugString()<<std::endl<<std::endl;
+		if(t_multi_status_msg.id_struct().terminal_id()==DISP_TERM_ID)
+        	std::cout<<t_multi_status_msg.DebugString()<<std::endl<<std::endl;
 	}
 
 	// 普爱统一一个万集节点, 各终端消息分别发送

system/system_executor.h

@@ -10,6 +10,7 @@
 #include "../error_code/error_code.h"
 #include "../communication/communication_message.h"
 
+#define DISP_TERM_ID 0
 
 class System_executor:public Singleton<System_executor>
 {

tool/measure_filter.h

@@ -18,9 +18,9 @@
 #include <algorithm>
 #include "glog/logging.h"
 
-#define FILTER_SIZE 6
-#define MAX_QUEUE_SIZE 12
-#define MAX_TIME_INTERVAL_MILLI 9000
+#define FILTER_SIZE 10
+#define MAX_QUEUE_SIZE 20
+#define MAX_TIME_INTERVAL_MILLI 5000
 
 class Measure_filter : public Singleton<Measure_filter>
 {
@@ -115,7 +115,7 @@ public:
         t_final_result /= t_result_count;
         result = t_final_result.wheel_data;
 
-//        LOG(INFO) << "\navg: \n\t"<<t_avg_result.wheel_data.to_string()<<"final: \t"<<t_final_result.wheel_data.to_string();
+    //    LOG(INFO) << "\navg: \n\t"<<t_avg_result.wheel_data.to_string()<<"final: \t"<<t_final_result.wheel_data.to_string();
         return SUCCESS;
     }
 

velodyne_lidar/ground_region.cpp

@@ -247,17 +247,20 @@ bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
         return false;
     }
     std::vector<cv::Point2f> centers;
+    cv::Point2f temp_centers(0,0);
     for (int i = 0; i < seg_clouds.size(); ++i)
     {
         Eigen::Vector4f centroid;
         pcl::compute3DCentroid(*seg_clouds[i], centroid);
         centers.push_back(cv::Point2f(centroid[0], centroid[1]));
+        temp_centers += cv::Point2f(centroid[0], centroid[1]);
     }
+    temp_centers /= 4.0f;
     bool ret = isRect(centers);
     if (ret)
     {
-
         std::string error_str;
+        // LOG(WARNING) << "region id: "<< m_region.region_id();
         if (m_detector->detect(seg_clouds, result, error_str))
         {
             return true;
@@ -412,7 +415,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_IF(INFO, m_region.region_id() == 0) << "car pose :::: cx: " << car_pose_x+m_region.plc_offsetx() << ", cy: " << car_pose_y+m_region.plc_offsety() << ", theta: " << car_pose_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);
@@ -492,7 +495,9 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
 
     while (chassis_z > (mid_z - height / 2.0))
     {
-        // LOG_IF(WARNING, m_region.region_id() == 4) << "chassis z: " << chassis_z << ", midz: " << mid_z - height / 2.0;
+        // LOG_IF(WARNING, m_region.region_id() == 0) << "chassis z: " << chassis_z << ", midz: " << mid_z - height / 2.0;
+        // 初值中x使用第一步优化的值
+        result.cx = car_pose_x;
         ret = classify_ceres_detect(cloud_filtered, chassis_z, result);
         // changed by yct, 根据底盘z识别，失败则向下移动直至成功或超过底盘内空中点
         if(ret)
@@ -733,6 +738,8 @@ void Ground_region::thread_measure_func()
             Error_manager ec = detect(mp_cloud_collection, t_result);
       
             std::lock_guard<std::mutex> lck(m_car_result_mutex);
+            // 增加滤波轴距
+            Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
             if (ec == SUCCESS)
             {
                 m_car_wheel_information.correctness = true;
@@ -755,17 +762,24 @@ void Ground_region::thread_measure_func()
                 m_car_wheel_information.uniform_car_x += m_region.plc_offsetx();
                 m_car_wheel_information.uniform_car_y += m_region.plc_offsety();
 
-                Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
                 t_wheel_info_stamped.wheel_data = m_car_wheel_information;
-				t_wheel_info_stamped.measure_time = m_detect_update_time;
-				Measure_filter::get_instance_references().update_data(m_region.region_id(), t_wheel_info_stamped);
+                t_wheel_info_stamped.measure_time = m_detect_update_time;
+                Measure_filter::get_instance_references().update_data(m_region.region_id(), t_wheel_info_stamped);
 
+                ec = Measure_filter::get_instance_references().get_filtered_wheel_information(m_region.region_id(), t_wheel_info_stamped.wheel_data);
+                if (ec == SUCCESS)
+                {
+                    m_car_wheel_information.car_wheel_base = t_wheel_info_stamped.wheel_data.car_wheel_base;
+                }
+                // else{
+                //     std::cout<<ec.to_string()<<std::endl;
+                // }
                  //LOG_IF(INFO, m_region.region_id() == 4) << m_car_wheel_information.to_string();
             }
             else
             {
                 m_car_wheel_information.correctness = false;
-                //LOG_IF(ERROR, m_region.region_id() == 4) << ec.to_string();
+                // LOG_IF(ERROR, m_region.region_id() == 0) << ec.to_string();
             }
             m_detect_update_time = std::chrono::system_clock::now();
         }

velodyne_lidar/match3d/detect_wheel_ceres3d.cpp

@@ -317,6 +317,7 @@ 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");
 
@@ -347,6 +348,8 @@ bool detect_wheel_ceres3d::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Pt
     }
     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);
+    // 20211203 added by yct, 初始xy不准确，逻辑调整
+    pcl::PointCloud<pcl::PointXYZ> t_cloud_left, t_cloud_right, t_cloud_front, t_cloud_rear;
     //第二步, 将每份点云旋转回去,计算点云重心所在象限
     for (int i = 0; i < cloud_vec.size(); ++i)
     {
@@ -374,29 +377,46 @@ bool detect_wheel_ceres3d::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Pt
         {
             //第一象限
             m_left_front_cloud = cloud;
+            t_cloud_left+=(m_left_front_cloud);
+            t_cloud_front+=(m_left_front_cloud);
         }
         if (x > 0 && y < 0)
         {
             //第四象限
             m_right_front_cloud = cloud;
+            t_cloud_right+=(m_right_front_cloud);
+            t_cloud_front+=(m_right_front_cloud);
         }
         if (x < 0 && y > 0)
         {
             //第二象限
             m_left_rear_cloud = cloud;
+            t_cloud_left+=(m_left_rear_cloud);
+            t_cloud_rear+=(m_left_rear_cloud);
         }
         if (x < 0 && y < 0)
         {
             //第三象限
             m_right_rear_cloud = cloud;
-
+            t_cloud_right+=(m_right_rear_cloud);
+            t_cloud_rear+=(m_right_rear_cloud);
         }
-
     }
-
+    // 20211203 added by yct, 重新计算xy
+    Eigen::Vector4f centroid_left,centroid_right,centroid_front,centroid_rear;
+    // 计算x
+    pcl::compute3DCentroid(t_cloud_left, centroid_left);
+    pcl::compute3DCentroid(t_cloud_right, centroid_right);
+    // 计算y
+    pcl::compute3DCentroid(t_cloud_front, centroid_front);
+    pcl::compute3DCentroid(t_cloud_rear, centroid_rear);
+    // LOG(WARNING)<<"x,y: "<<(centroid_left.x()+centroid_right.x())/2.0f<<", "<<(centroid_front.y()+centroid_rear.y())/2.0f;
+    
+    // 使用外部第一步优化产生的x，以及当前计算后的y作为初值
     // 多次优化，获取最佳优化结果
-    detect_result.cx = center_x;
-    detect_result.cy = center_y;
+    // detect_result.cx = center_x;
+    detect_result.cx = detect_result.cx;
+    detect_result.cy = (centroid_front.y()+centroid_rear.y())/2.0f;
     detect_result.wheel_base=std::max(len1,len2);
     // 已计算次数，初始传入值正常，之后solve函数会修改初始值，需要恢复角度单位为弧度
     int calc_count = 0;