提交更新

BaseTracker/Track3D.cpp

@@ -91,9 +91,9 @@ double Track3D::CalculateIou(const std::vector<float>& data)
     GetStateData(states);
 
     //SDK_LOG(SDK_INFO, "calculate size1 = %d, size2 = %d",states.size(),data.size());
-    std::vector<float> truth_poses{ states[0], states[1], states[2],0, 0, states[6]/180* 3.1415926, states[3]/2, states[4]/2, states[5]/2 };
-    std::vector<float> landmark_poses{ data[0], data[1], data[2],0, 0, data[6] / 180 * 3.1415926, data[3]/2, data[4]/2, data[5]/2 };
-    double i3d_iou = CuboidIoU(truth_poses, landmark_poses);
+    //std::vector<float> truth_poses{ states[0], states[1], states[2],0, 0, states[6]/180* 3.1415926, states[3]/2, states[4]/2, states[5]/2 };
+    //std::vector<float> landmark_poses{ data[0], data[1], data[2],0, 0, data[6] / 180 * 3.1415926, data[3]/2, data[4]/2, data[5]/2 };
+    //double i3d_iou = CuboidIoU(truth_poses, landmark_poses);
 
 
     cv::RotatedRect rect1 = cv::RotatedRect(cv::Point2f(states[0], states[1]), cv::Size2f(states[3], states[4]), states[6]);

TrackingRos.cpp

@@ -17,7 +17,8 @@ TrackingRos::~TrackingRos()
 void TrackingRos::Init(ros::NodeHandle& nh)
 {
     m_pub = nh.advertise<jfx_common_msgs::det_tracking_array>("/tracking_res", 100);
-    
+    m_pubCloud = nh.advertise<sensor_msgs::PointCloud2>("/tracking_cloud", 100);
+
     m_objsQueue.set_max_num_items(2);
 
     m_isTrackingRun = true;
@@ -28,7 +29,7 @@ void TrackingRos::TrackingCallBackFunc(const jfx_common_msgs::det_tracking_array
 {
     objTrackListPtr objsPtr = std::make_shared<jfx_common_msgs::det_tracking_array>();
     objsPtr->array = msg->array;
-    objsPtr->cloud = msg->cloud;
+    //objsPtr->cloud = msg->cloud;
 
     SDK_LOG(SDK_INFO, "recv msg size = %d",msg->array.size());
     //for (auto& item : msg->array)
@@ -54,6 +55,8 @@ void TrackingRos::TrackingCallBackFunc(const jfx_common_msgs::det_tracking_array
     //}
     //objsPtr->cloud = msg->cloud;
     m_objsQueue.push(objsPtr);
+
+    m_pubCloud.publish(msg->cloud);
 }
 
 void TrackingRos::ThreadTrackingProcess()
@@ -75,8 +78,8 @@ void TrackingRos::ThreadTrackingProcess()
                 data.push_back(obj.x);
                 data.push_back(obj.y);
                 data.push_back(obj.z);
-                data.push_back(obj.w);
                 data.push_back(obj.l);
+                data.push_back(obj.w);
                 data.push_back(obj.h);
                 data.push_back(obj.rot_y);
                 input.emplace_back(data);
@@ -105,8 +108,8 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.x = data[0];
                         obj.y = data[1];
                         obj.z = data[2];
-                        obj.w = data[3];
-                        obj.l = data[4];
+                        obj.l = data[3];
+                        obj.w = data[4];
                         obj.h = data[5];
                         obj.rot_y = data[6];
                         obj.v_x = data[7];

TrackingRos.h

@@ -35,4 +35,5 @@ public:
     bool m_isTrackingRun = false;//记录是否在运行
     std::thread m_trackingThread;//运行事件线程
     ros::Publisher m_pub;//发送所有物体信息的publisher
+    ros::Publisher m_pubCloud;//发送点云数据
 };