提交更新

TrackingRosEx.cpp

@@ -136,9 +136,9 @@ void get_camera_tolidar_loc(float u, float v, float& x, float& y)
     Eigen::Vector3d loc_camera;
     loc_camera << outputUndistortedPoints[0].x, outputUndistortedPoints[0].y, 1;
     Eigen::Matrix<double, 3, 3> cam_trans;
-    cam_trans << -1.29665805e-02, 9.37907698e-02, -6.31390808e+02,
-        8.18329480e-02, -1.38701295e-02, 4.67566705e+01,
-        -1.22717262e-03, -1.27333545e-02, 1.00000000e+00;
+    cam_trans << -2.81227713e-04, 1.00961676e-03, -6.79337876e+00,
+        3.30666352e-03, 1.86212238e-04, -3.54019041e+00,
+        8.41395110e-05, -2.14260840e-03, 1.00000000e+00;
     auto trans_pos = cam_trans * loc_camera;
     double h = trans_pos[2];
     auto trans_pos_ = trans_pos / h;
@@ -246,9 +246,9 @@ TrackingRos::~TrackingRos()
 
 void TrackingRos::Init(ros::NodeHandle& nh)
 {
-    //float x, y;
-    //get_camera_tolidar_loc(400, 400, x, y);
-    //SDK_LOG(SDK_INFO, "get_camera_tolidar_loc x = %f, y = %f",x,y);
+    float x, y;
+    get_camera_tolidar_loc(400, 400, x, y);
+    SDK_LOG(SDK_INFO, "get_camera_tolidar_loc x = %f, y = %f",x,y);
     //get_camera_right_tolidar_loc(400, 400, x, y);
     //SDK_LOG(SDK_INFO, "get_camera_right_tolidar_loc x = %f, y = %f", x, y);
     //get_camera_left_tolidar_loc(400, 400, x, y);