提交修改坐标值。

TrackingRos.cpp

@@ -26,19 +26,68 @@ float to360(float rot_y,float lidar_x_angle)
     return angle;
 }
 
-void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>& trans, float& gx, float& gy)
+void my_compute_box_3d(Eigen::Vector3f center, float heading, Eigen::Vector3f size, Eigen::Matrix<float, 8, 3>& res_corners_3d) 
 {
+    Eigen::Matrix<float, 4, 8> corners_3d;
+    corners_3d << -0.5, 0.5, 0.5, -0.5, -0.5, 0.5, 0.5, -0.5,
+        0.5, 0.5, -0.5, -0.5, 0.5, 0.5, -0.5, -0.5,
+        0.5, 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5,
+        1, 1, 1, 1, 1, 1, 1, 1;
+
+    Eigen::Vector4f svTmp;
+    svTmp << size[0], size[1], size[2], 1;
+    Eigen::Matrix4f S = svTmp.asDiagonal();//生成对角矩阵
+    //Eigen::Matrix3f rot;
+    //rotz(heading, rot);
+    //Eigen::Matrix4f Trans = Eigen::Matrix4f::Zero();
+    //for (int i = 0; i < 3; i++) {
+    //    for (int j = 0; j < 3; j++) {
+    //        Trans(i, j) = rot(i, j);
+    //    }
+    //}
+    //Trans(0, 3) = center[0];
+    //Trans(1, 3) = center[1];
+    //Trans(2, 3) = center[2];
+
+    auto tmp4x4 = S * corners_3d;
+    auto world_corners_3d = Trans * tmp4x4;
+    res_corners_3d = world_corners_3d.block(0, 0, 3, 8).transpose();
+}
+
+
+void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>& trans, std::vector<std::vector<float>>& kitti2origin, float& gx, float& gy)
+{
+    Eigen::Matrix<float, 4, 8> corners_3d;
+    corners_3d << -0.5, 0.5, 0.5, -0.5, -0.5, 0.5, 0.5, -0.5,
+        0.5, 0.5, -0.5, -0.5, 0.5, 0.5, -0.5, -0.5,
+        0.5, 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5,
+        1, 1, 1, 1, 1, 1, 1, 1;
+
     Eigen::Vector4f svTmp;
     svTmp << x, y, z, 1;
+    Eigen::Matrix4f S = svTmp.asDiagonal();//生成对角矩阵
+
     Eigen::Matrix4f Trans = Eigen::Matrix4f::Zero();
     for (int i = 0; i < 4; i++) {
         for (int j = 0; j < 4; j++) {
             Trans(i, j) = trans[i][j];
         }
     }
-    auto world_corners_3d = Trans * svTmp;
-    gx = world_corners_3d[0];
-    gy = world_corners_3d[1];
+    Eigen::Matrix4f Kit2Ori = Eigen::Matrix4f::Zero();
+    for (int i = 0; i < 4; i++) {
+        for (int j = 0; j < 4; j++) {
+            Kit2Ori(i, j) = kitti2origin[i][j];
+        }
+    }
+
+    auto tmp4x8 = S * corners_3d;
+    auto origin4x8 = Kit2Ori * tmp4x8;
+    auto world_corners_3d = Trans * origin4x8;
+    auto res_corners_3d = world_corners_3d.block(0, 0, 3, 8).transpose();
+
+    //auto world_corners_3d = Trans * svTmp;
+    gx = res_corners_3d[0];
+    gy = res_corners_3d[1];
 }
 
 TrackingRos::~TrackingRos()
@@ -75,6 +124,8 @@ void TrackingRos::Init(ros::NodeHandle& nh)
     auto cfg = config["TRACKING"];
     m_trans = cfg["TRANS"].as<std::vector<std::vector<float>>>();
 
+    auto cfg2 = config["POINTPILLARS"];
+    m_kitti2origin = cfg2["KITTI2ORIGIN"].as<std::vector<std::vector<float>>>();
 
     ADD_DEF_LOG(1, "tracking.csv");
     SDK_IDX_LOG(1, SDK_INFO, "date,frameNum,time,id,input_radian,input_angle,predict_radian,predict_angle,output_radian,output_angle,input_x,input_y,input_z,input_l,input_w,input_h,predict_x,predict_y,predict_z,predict_l,predict_w,preditc_h,output_x,output_y,output_z,output_l,output_w,output_h,output_rot_y,v_x,v_y,v_z,lat,lon");
@@ -229,7 +280,7 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.obj_id = iter.first;
                         float gx = 0.0f;
                         float gy = 0.0f;
-                        CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, gx, gy);
+                        CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, m_kitti2origin 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);
@@ -259,7 +310,7 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.obj_id = iter.first;
                         float gx = 0.0f;
                         float gy = 0.0f;
-                        CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, gx, gy);
+                        CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, m_kitti2origin, 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);

TrackingRos.h

@@ -44,6 +44,7 @@ public:
     ros::Publisher m_pubBoundingBoxes;//发送3D框信息
 
     std::vector<std::vector<float>> m_trans;
+    std::vector<std::vector<float>> m_kitti2origin;
 
     int m_frameNum = 0;