提交更新

TrackingRosEx.cpp

@@ -138,18 +138,18 @@ void get_camera_tolidar_loc(float u, float v, float& x, float& y)
     auto trans_pos = cam_trans * loc_camera;
     double h = trans_pos[2];
     auto trans_pos_ = trans_pos / h;
-    //Eigen::Matrix<double, 3, 3> R;
-    //R << -0.602479577065, -0.797206640244, -0.038464091718,
-    //    0.796449661255, -0.603639006615, 0.035887073725,
-    //    -0.051827855408, -0.009013481438, 0.998615324497;
-    //Eigen::Vector3d T;
-    //T << -886.266723632813, 556.056396484375, -0.398632347584;
-    //auto R_inv = R.inverse();
-
-    //auto P_ = R_inv * (trans_pos - T);
-
-    //x = P_[0] - 3.5;
-    //y = P_[1] + 0.4;
+    Eigen::Matrix<double, 3, 3> R;
+    R << -0.602479577065, -0.797206640244, -0.038464091718,
+        0.796449661255, -0.603639006615, 0.035887073725,
+        -0.051827855408, -0.009013481438, 0.998615324497;
+    Eigen::Vector3d T;
+    T << -886.266723632813, 556.056396484375, -0.398632347584;
+    auto R_inv = R.inverse();
+
+    auto P_ = R_inv * (trans_pos_ - T);
+
+    x = P_[0] - 3.5;
+    y = P_[1] + 0.4;
 }
 
 TrackingRos::~TrackingRos()