提交测试

TrackingRos.cpp

@@ -86,15 +86,12 @@ void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>&
     Eigen::Vector3f head_pnt = (res3 + res2) / 2;
     Eigen::Vector3f tail_pnt = (res0 + res3) / 2;
 
-    float inter_len;
-    float total_len;
+    float inter_len = 0.413f;
+    float total_len = 4.6f;
     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);
-    //float loc_y = lidar_loc(0, 1);
-    //float loc_z = lidar_loc(0, 2);
-    //tranTmp << loc_x, loc_y, loc_z, 1;
+    tranTmp << lidar_loc[0], lidar_loc[1], lidar_loc[2], 1;
 
     Eigen::Matrix4f Trans = Eigen::Matrix4f::Zero();
     for (int i = 0; i < 4; i++) {