提交测试

TrackingRos.cpp

@@ -78,8 +78,7 @@ 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 res0;
-    res0 << res_corners_3d(0, 0) << res_corners_3d(0, 1) << res_corners_3d(0, 2);
+    Eigen::Vector3f 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;
@@ -89,10 +88,10 @@ void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>&
     auto lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0 * total_len) + head_pnt;
 
     Eigen::Vector4f tranTmp;
-    float loc_x = lidar_loc(0, 0);
-    float loc_y = lidar_loc(0, 1);
-    float loc_z = lidar_loc(0, 2);
-    tranTmp << loc_x, loc_y, loc_z, 1;
+    //float loc_x = lidar_loc(0, 0);
+    //float loc_y = lidar_loc(0, 1);
+    //float loc_z = lidar_loc(0, 2);
+    //tranTmp << loc_x, loc_y, loc_z, 1;
 
     Eigen::Matrix4f Trans = Eigen::Matrix4f::Zero();
     for (int i = 0; i < 4; i++) {