提交更新

TrackingRosEx.cpp

@@ -7,6 +7,7 @@
 #include "matrix.h"
 #include <opencv2/opencv.hpp>
 #include <eigen3/Eigen/Dense>
+#include <math.h>
 #ifdef _USING_NSIGHT_
 #include <nvToolsExt.h>
 #endif
@@ -151,6 +152,72 @@ void get_camera_tolidar_loc(float u, float v, float& x, float& y)
     x = P_[0] - 3.5;
     y = P_[1] + 0.4;
 }
+void get_camera_right_tolidar_loc(float u, float v, float& x, float& y)
+{
+    float camera[3][3] = { 6.3504560297918590e+02, 0.0, 6.2178154273047369e+02,0.0, 6.3430547692629773e+02, 3.6059071556316638e+02,0.0, 0.0, 1.0 };
+    cv::Mat cameraMatrix = cv::Mat(3, 3, CV_32F, camera);
+    float dist[5] = { -2.8832763961089897e-01, 7.8904579864635727e-02,-3.3990455415458144e-05, 8.6290635804868075e-04,-9.5910232578267241e-03 };    //摄像机内参数矩阵K
+    cv::Mat distCoeffs = cv::Mat(1, 5, CV_32F, dist);    //内参数K Mat类型变量
+
+    std::vector<cv::Point2f> inputDistortedPoints;
+    std::vector<cv::Point2f> outputUndistortedPoints;
+    inputDistortedPoints.push_back(cv::Point2f(u, v));
+    cv::undistortPoints(inputDistortedPoints, outputUndistortedPoints, cameraMatrix, distCoeffs, cv::Mat(), cameraMatrix);
+    if (outputUndistortedPoints.size() != 1)
+        return;
+    Eigen::Vector3d loc_camera;
+    loc_camera << outputUndistortedPoints[0].x, outputUndistortedPoints[0].y, 1;
+    Eigen::Matrix<double, 3, 3> cam_trans;
+    cam_trans << -0.003723879345, -0.010894616134, 4.359775066376,//camera to point cloud transfrom
+        -0.000860022672, -0.007504287176, 0.190786495805,
+        -0.000394680159, -0.003708023345, 1.000000000000;
+    auto trans_pos = cam_trans * loc_camera;
+    double h = trans_pos[2];
+    auto trans_pos_ = trans_pos / h;
+
+    double relatx = trans_pos_[1];
+    double relaty = -trans_pos_[0];
+
+    auto srx = (-relatx) * cos(2 * M_PI / 3) + (-relaty) * sin(2 * M_PI / 3);
+    auto sry = (-relaty) * cos(2 * M_PI / 3) - (-relatx) * sin(2 * M_PI / 3);
+    srx = srx - 5;
+    sry = sry - 1;
+    x = srx;
+    y = sry;
+}
+
+void get_camera_left_tolidar_loc(float u, float v, float& x, float& y)
+{
+    float camera[3][3] = { 6.3504560297918590e+02, 0.0, 6.2178154273047369e+02,0.0, 6.3430547692629773e+02, 3.6059071556316638e+02,0.0, 0.0, 1.0 };
+    cv::Mat cameraMatrix = cv::Mat(3, 3, CV_32F, camera);
+    float dist[5] = { -2.8832763961089897e-01, 7.8904579864635727e-02,-3.3990455415458144e-05, 8.6290635804868075e-04,-9.5910232578267241e-03 };    //摄像机内参数矩阵K
+    cv::Mat distCoeffs = cv::Mat(1, 5, CV_32F, dist);    //内参数K Mat类型变量
+
+    std::vector<cv::Point2f> inputDistortedPoints;
+    std::vector<cv::Point2f> outputUndistortedPoints;
+    inputDistortedPoints.push_back(cv::Point2f(u, v));
+    cv::undistortPoints(inputDistortedPoints, outputUndistortedPoints, cameraMatrix, distCoeffs, cv::Mat(), cameraMatrix);
+    if (outputUndistortedPoints.size() != 1)
+        return;
+    Eigen::Vector3d loc_camera;
+    loc_camera << outputUndistortedPoints[0].x, outputUndistortedPoints[0].y, 1;
+    Eigen::Matrix<double, 3, 3> cam_trans;
+    cam_trans << 0.012736757286, -0.007320342120, -15.075451850891,//camera to point cloud transfrom
+        0.017054133117, -0.042287658900, 24.450130462646,
+        0.002793390071, 0.003237717086, 1.000000000000;
+    auto trans_pos = cam_trans * loc_camera;
+    double h = trans_pos[2];
+    auto trans_pos_ = trans_pos / h;
+    double relatx = trans_pos_[1];
+    double relaty = -trans_pos_[0];
+
+    auto srx = (-relatx) * cos(M_PI / 4) + (-relaty) * sin(M_PI / 4);
+    auto sry = (-relaty) * cos(M_PI / 4) - (-relatx) * sin(M_PI / 4);
+    srx = srx + 8;
+    sry = sry;
+    x = srx;
+    y = sry;
+}
 
 TrackingRos::~TrackingRos()
 {
@@ -168,6 +235,10 @@ 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);
+    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);
+    SDK_LOG(SDK_INFO, "get_camera_left_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");