提交配置修改

TrackingRosEx.cpp

@@ -283,7 +283,12 @@ void TrackingRos::Init(ros::NodeHandle& nh)
     SDK_LOG(SDK_INFO, "detct_camera_car_size = %d", m_detectCameraCarSize);
     nh.param<float>("lidar_camera_msg_iou", m_lidar_camera_msg_iou, 0.4);
     SDK_LOG(SDK_INFO, "matrix_angle_r_value = %f", m_lidar_camera_msg_iou);
-    
+    nh.param<int32_t>("camera_one_center_type", m_camera_one_center_type, 0);
+    SDK_LOG(SDK_INFO, "camera_one_center_type = %d", m_camera_one_center_type);
+    nh.param<int32_t>("camera_cloud_merge_type", m_camera_cloud_merge_type, 0);
+    SDK_LOG(SDK_INFO, "camera_cloud_merge_type = %d", m_camera_cloud_merge_type);
+    nh.param<int32_t>("camera_debug_type", m_camera_debug_type, 0);
+    SDK_LOG(SDK_INFO, "camera_debug_type = %d", m_camera_debug_type);
 
     YAML::Node config = YAML::LoadFile(file.c_str());
     auto cfg = config["TRACKING"];
@@ -469,6 +474,10 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
                     float y = 0.0f;
                     if (i == 0)
                     {
+                        if(m_camera_one_center_type == 1 || m_camera_one_center_type == 2)
+                            bottom_center_x = x1 + (bottom_center_x / 1920) * (x2 - x1);
+                        if(m_camera_one_center_type == 2)
+                            bottom_center_y = (float(y1) + float(y2)) / 2;
                         get_camera_tolidar_loc(bottom_center_x, bottom_center_y, x, y);
                     }
                     else if (i == 1)
@@ -490,7 +499,7 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
                     if (i == 1 || i == 2)
                     {
                         //SDK_LOG(SDK_INFO, "camera obj i = %d,num_class = %d,bottom_center_x = %f,bottom_center_y = %f, x = %f,y = %f",i, num_class, bottom_center_x, bottom_center_y, x, y);
-                        if (x < -10 || abs(y) > 5)
+                        if (x < -15 || abs(y) > 5)
                             continue;
                     }
                     jfx_common_msgs::det_tracking objMsg = {};
@@ -547,33 +556,40 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
                     }
                     else
                         continue;
-
-                    objMsg.x = objMsg.x + (objMsg.l / 2);
+                    if(i == 0 && m_camera_one_center_type != 2)
+                        objMsg.x = objMsg.x + (objMsg.l / 2);
                     int duplicate = 0;
                     if (_GET_VALID_VALUE((unsigned char)0, m_recvMsgBit))
                     {
                         for (auto& obj : cloudPtr->array)
                         {
-                            if (abs(obj.x - objMsg.x) < std::max(objMsg.l * 1.2, (double)2.0) && abs(obj.y - objMsg.y) < std::max(objMsg.w * 1.2, (double)2.0))
+                            if (m_camera_cloud_merge_type == 0)
+                            {
+                                if (abs(obj.x - objMsg.x) < std::max(objMsg.l * 1.2, (double)2.0) && abs(obj.y - objMsg.y) < std::max(objMsg.w * 1.2, (double)2.0))
+                                {
+                                    duplicate = 1;
+                                    break;
+                                }
+                            }
+                            else if (m_camera_cloud_merge_type == 1)
                             {
-                                duplicate = 1;
-                                break;
+                                cv::RotatedRect rect1 = cv::RotatedRect(cv::Point2f(objMsg.x, objMsg.y), cv::Size2f(objMsg.l, objMsg.w), objMsg.rot_y);
+                                cv::RotatedRect rect2 = cv::RotatedRect(cv::Point2f(obj.x, obj.y), cv::Size2f(obj.l, obj.w), obj.rot_y * 180 / 3.1415926);
+                                float rate = calcIntersectionRate(rect1, rect2);
+                                if (rate > m_lidar_camera_msg_iou)
+                                {
+                                    duplicate = 1;
+                                    break;
+                                }
                             }
-                            //cv::RotatedRect rect1 = cv::RotatedRect(cv::Point2f(objMsg.x, objMsg.y), cv::Size2f(objMsg.l, objMsg.w), objMsg.rot_y);
-                            //cv::RotatedRect rect2 = cv::RotatedRect(cv::Point2f(obj.x, obj.y), cv::Size2f(obj.l, obj.w), obj.rot_y * 180 / 3.1415926);
-                            //float rate = calcIntersectionRate(rect1, rect2);
-                            //if (rate > m_lidar_camera_msg_iou)
-                            //{
-                            //    duplicate = 1;
-                            //    break;
-                            //}
                         }
                     }
                     if (duplicate == 0)
                     {
-                        if (i == 1 || i == 2)
+                        if (i == 0 || i == 1 || i == 2)
                         {
-                            //objMsg.type = 7;
+                            if(m_camera_debug_type == 1)
+                                objMsg.type = 7;
                             //SDK_LOG(SDK_INFO, "camera 1,2 x = %f,y = %f", objMsg.x, objMsg.y);
                         }
                         if (_GET_VALID_VALUE((unsigned char)(i + 1), m_recvMsgBit))

TrackingRosEx.h

@@ -61,5 +61,8 @@ public:
 
     int m_recvMsgBit = 0b1111;//处理消息的类型
     int m_detectCameraCarSize = 150;//处理相机汽车边长的最小限制
-    float m_lidar_camera_msg_iou;//相机和雷达消息过滤用iou值
+    float m_lidar_camera_msg_iou = 0.01;//相机和雷达消息过滤用iou值
+    int m_camera_one_center_type = 0;//计算中心点坐标的配置，0是原来的配置，1，使用比例获取x值，2是比例取值并且用中心点坐标
+    int m_camera_cloud_merge_type = 0;//0是使用距离判断，1是使用iou判断
+    int m_camera_debug_type = 0;//调试相机类型是否修改为7，0是正常，1是调试为7
 };