提交更新

TrackingRosEx.cpp

@@ -112,41 +112,6 @@ void CalculateGuassPos(jfx_common_msgs::det_tracking& obj,std::vector<std::vecto
 
     gx = world_pos[0];
     gy = world_pos[1];
-
-    //Eigen::Matrix<float, 4, 8> corners_3d;
-    //corners_3d << -0.5, 0.5, 0.5, -0.5, -0.5, 0.5, 0.5, -0.5,
-    //    0.5, 0.5, -0.5, -0.5, 0.5, 0.5, -0.5, -0.5,
-    //    0.5, 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5,
-    //    1, 1, 1, 1, 1, 1, 1, 1;
-
-    //Eigen::Vector4f svTmp;
-    //svTmp << x, y, z, 1;
-    //Eigen::Matrix4f S = svTmp.asDiagonal();//生成对角矩阵
-
-    //Eigen::Matrix4f Kit2Ori = Eigen::Matrix4f::Zero();
-    //for (int i = 0; i < 4; i++) {
-    //    for (int j = 0; j < 4; j++) {
-    //        Kit2Ori(i, j) = kitti2origin[i][j];
-    //    }
-    //}
-
-    //auto tmp4x8 = S * corners_3d;
-    //auto origin4x8 = Kit2Ori * tmp4x8;
-    //auto res_corners_3d = origin4x8.block(0, 0, 3, 8).transpose();
-
-    //Eigen::Vector3f res0(res_corners_3d(0, 0), res_corners_3d(0, 1), res_corners_3d(0, 2));
-    //Eigen::Vector3f res1(res_corners_3d(1, 0), res_corners_3d(1, 1), res_corners_3d(1, 2));
-    //Eigen::Vector3f res2(res_corners_3d(2, 0), res_corners_3d(2, 1), res_corners_3d(2, 2));
-    //Eigen::Vector3f res3(res_corners_3d(3, 0), res_corners_3d(3, 1), res_corners_3d(3, 2));
-
-    //Eigen::Vector3f head_pnt = (res3 + res2) / 2;
-    //Eigen::Vector3f tail_pnt = (res0 + res3) / 2;
-
-    //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;
-
-
 }
 
 void get_camera_tolidar_loc(float u, float v, float& x, float& y)
@@ -160,8 +125,30 @@ void get_camera_tolidar_loc(float u, float v, float& x, float& y)
     std::vector<cv::Point2f> outputUndistortedPoints;
     inputDistortedPoints.push_back(cv::Point2f(u, v));
     cv::undistortPoints(inputDistortedPoints, outputUndistortedPoints, cameraMatrix, distCoeffs, cv::Mat(), cameraMatrix);
-    if(outputUndistortedPoints.size() > 0)
-        SDK_LOG(SDK_INFO, "outputUndistortedPoints size = %d, x = %f,y = %f", outputUndistortedPoints.size(), outputUndistortedPoints[0].x, outputUndistortedPoints[0].y);
+    if (outputUndistortedPoints.size() != 1)
+        return;
+     //SDK_LOG(SDK_INFO, "outputUndistortedPoints size = %d, x = %f,y = %f", outputUndistortedPoints.size(), , outputUndistortedPoints[0].y);
+    
+    Eigen::Vector3d loc_camera;
+    loc_camera << outputUndistortedPoints[0].x, outputUndistortedPoints[0].y, 1;
+    Eigen::Matrix<double, 3, 3> cam_trans;
+    cam_trans << -6.60431137e-02, 1.93080567e+00, -8.78993882e+02,//camera to point cloud transfrom
+        3.81973306e-02, -1.21347206e+00, 5.53062060e+02,
+        7.08859171e-05, -2.17867524e-03, 1.00000000e+00;
+    auto trans_pos = cam_trans * loc_camera;
+    trans_pos /= trans_pos[2];
+    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()
@@ -178,7 +165,8 @@ TrackingRos::~TrackingRos()
 void TrackingRos::Init(ros::NodeHandle& nh)
 {
     float x, y;
-    get_camera_tolidar_loc(960, 540, x, y);
+    get_camera_tolidar_loc(400, 400, x, y);
+    SDK_LOG(SDK_INFO, "get_camera_tolidar_loc x = %f, y = %f",x,y);
 
     std::string folder,yaml;
     nh.param<std::string>("project_path", folder, "/home/nvidia/catkin_ws/src/jfxrosperceiver");