提交更新

TrackingRos.cpp

@@ -67,12 +67,6 @@ void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>&
     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];
-        }
-    }
     Eigen::Matrix4f Kit2Ori = Eigen::Matrix4f::Zero();
     for (int i = 0; i < 4; i++) {
         for (int j = 0; j < 4; j++) {
@@ -82,12 +76,29 @@ void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>&
 
     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 res_corners_3d = origin4x8.block(0, 0, 3, 8).transpose();
+
+    auto head_pnt = (res_corners_3d(1) + res_corners_3d(2)) / 2;
+    auto tail_pnt = (res_corners_3d(0) + res_corners_3d(3)) / 2;
+
+    float inter_len;
+    float total_len;
+    auto lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0 * total_len) + head_pnt;
+
+    Eigen::Vector4f tranTmp;
+    tranTmp << lidar_loc(0), lidar_loc(1), lidar_loc(2), 1;
+
+    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_pos = Trans * tranTmp;
 
-    //auto world_corners_3d = Trans * svTmp;
-    gx = res_corners_3d[0];
-    gy = res_corners_3d[1];
+    gx = world_pos[0];
+    gy = world_pos[1];
 }
 
 TrackingRos::~TrackingRos()