提交测试

TrackingRos.cpp

@@ -78,7 +78,10 @@ void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>&
     auto origin4x8 = Kit2Ori * tmp4x8;
     auto res_corners_3d = origin4x8.block(0, 0, 3, 8).transpose();
 
-    Eigen::Vector3f head_pnt = (res_corners_3d(1) + res_corners_3d(2)) / 2;
+    Eigen::Vector3f res0;
+    res0 << res_corners_3d(0, 0) << res_corners_3d(0, 1) << res_corners_3d(0, 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;
 
     float inter_len;