提交测试

TrackingRos.cpp

@@ -79,13 +79,16 @@ void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>&
     auto res_corners_3d = origin4x8.block(0, 0, 3, 8).transpose();
 
     Eigen::Vector3f res0(res_corners_3d(0, 0), res_corners_3d(0, 1), res_corners_3d(0, 2));
+    Eigen::Vector3f res1(res_corners_3d(1, 0), res_corners_3d(1, 1), res_corners_3d(1, 2));
+    Eigen::Vector3f res2(res_corners_3d(2, 0), res_corners_3d(2, 1), res_corners_3d(2, 2));
+    Eigen::Vector3f res3(res_corners_3d(3, 0), res_corners_3d(3, 1), res_corners_3d(3, 2));
 
-    auto head_pnt = (res_corners_3d(1) + res_corners_3d(2)) / 2;
-    auto tail_pnt = (res_corners_3d(0) + res_corners_3d(3)) / 2;
+    Eigen::Vector3f head_pnt = (res3 + res2) / 2;
+    Eigen::Vector3f tail_pnt = (res0 + res3) / 2;
 
     float inter_len;
     float total_len;
-    auto lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0 * total_len) + head_pnt;
+    Eigen::Vector3f lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0 * total_len) + head_pnt;
 
     Eigen::Vector4f tranTmp;
     //float loc_x = lidar_loc(0, 0);