加入打印

BaseTracker/BaseTracker.h

@@ -147,7 +147,7 @@ void BaseTracker<T>::AssociateDetectionsToTrackers(const std::vector<std::vector
             if (0 == association[i][j])
             {
                 // Filter out matched with low IOU
-                SDK_LOG(SDK_INFO, "match info i = %d,j = %d, iou_matrix = %f, m_iou_threshold = %f",i,j, iou_matrix[i][j], trk.second->m_iou_threshold);
+                //SDK_LOG(SDK_INFO, "match info i = %d,j = %d, iou_matrix = %f, m_iou_threshold = %f",i,j, iou_matrix[i][j], trk.second->m_iou_threshold);
                 if (iou_matrix[i][j] >= trk.second->m_iou_threshold)
                 {
                     matched[trk.first] = i;

TrackingRos.cpp

@@ -142,15 +142,19 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.v_y = data[8];
                         obj.v_z = data[9];
                         obj.obj_id = iter.first;
+                        SDK_LOG(SDK_INFO, "id = %llu, input(%f,%f,%f,%f,%f,%f,%f) output(%f,%f,%f,%f,%f,%f,%f) prob = %f",
+                            iter.first, objsPtr->array[updateId[iter.first]].x, objsPtr->array[updateId[iter.first]].y, objsPtr->array[updateId[iter.first]].z,
+                            objsPtr->array[updateId[iter.first]].l, objsPtr->array[updateId[iter.first]].w, objsPtr->array[updateId[iter.first]].h, objsPtr->array[updateId[iter.first]].rot_y,
+                            obj.x,obj.y,obj.z,obj.l,obj.w,obj.h,obj.rot_y, iter.second->GetProb());
                         float gx = 0.0f;
                         float gy = 0.0f;
                         CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, gx, gy);
-                        SDK_LOG(SDK_INFO, "CalculateGuassPos x = %f,y = %f, gx = %f, gy = %f,prob = %f",obj.x,obj.y,gx,gy,iter.second->GetProb());
+                        //SDK_LOG(SDK_INFO, "CalculateGuassPos x = %f,y = %f, gx = %f, gy = %f,prob = %f",obj.x,obj.y,gx,gy,iter.second->GetProb());
                         jfx::Array gpos = {gx,gy};
                         jfx::Array pos = jfx::Inverse(gpos);
                         obj.Lat = pos[0];
                         obj.Long = pos[1];
-                        SDK_LOG(SDK_INFO, "lat = %.10f, lon = %.10f",obj.Lat,obj.Long);
+                        //SDK_LOG(SDK_INFO, "lat = %.10f, lon = %.10f",obj.Lat,obj.Long);
                     }
                     iter.second->m_obj = std::make_shared<jfx_common_msgs::det_tracking>(obj);
                 }
@@ -158,6 +162,30 @@ void TrackingRos::ThreadTrackingProcess()
                 {
                     if(iter.second->m_obj)
                         obj = *(iter.second->m_obj);
+                    std::vector<float> data;
+                    if (iter.second->GetStateData(data) == 0)
+                    {
+                        obj.x = data[0];
+                        obj.y = data[1];
+                        obj.z = data[2];
+                        obj.l = data[3];
+                        obj.w = data[4];
+                        obj.h = data[5];
+                        obj.rot_y = data[6];
+                        obj.v_x = data[7];
+                        obj.v_y = data[8];
+                        obj.v_z = data[9];
+                        obj.obj_id = iter.first;
+                        float gx = 0.0f;
+                        float gy = 0.0f;
+                        CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, gx, gy);
+                        //SDK_LOG(SDK_INFO, "CalculateGuassPos x = %f,y = %f, gx = %f, gy = %f,prob = %f", obj.x, obj.y, gx, gy, iter.second->GetProb());
+                        jfx::Array gpos = { gx,gy };
+                        jfx::Array pos = jfx::Inverse(gpos);
+                        obj.Lat = pos[0];
+                        obj.Long = pos[1];
+                        //SDK_LOG(SDK_INFO, "lat = %.10f, lon = %.10f", obj.Lat, obj.Long);
+                    }
                 }
                 sendObjs.array.emplace_back(obj);
             }