提交opencv函数

TrackingRosEx.cpp

@@ -5,6 +5,7 @@
 #include "LogBase.h"
 #include <yaml-cpp/yaml.h>
 #include "matrix.h"
+#include <opencv2/opencv.hpp>
 #include <eigen3/Eigen/Dense>
 #ifdef _USING_NSIGHT_
 #include <nvToolsExt.h>
@@ -148,6 +149,20 @@ void CalculateGuassPos(jfx_common_msgs::det_tracking& obj,std::vector<std::vecto
 
 }
 
+void get_camera_tolidar_loc(float u, float v, float& x, float& y)
+{
+    float camera[1][5] = { -3.0546708753769027e+00, 2.8235384675530199e+01,1.5164164709152357e-02, -1.4151304833353489e-04,-5.3523523223247594e+02 };    //摄像机内参数矩阵K
+    cv::Mat cameraMatrix = cv::Mat(1, 5, CV_32FC1, camera);    //内参数K Mat类型变量
+    float dist[3][3] = { 1.1775635693883434e+04, 0.0, 9.6852512890598337e+02,0.0,4.3425255703581051e+03, 5.4191193116052807e+02,0.0, 0.0, 1.0 }
+    cv::Mat distCoeffs = cvMat(3, 3, CV_32FC1, camera);
+
+    std::vector<cv::Point2f> inputDistortedPoints;
+    std::vector<cv::Point2f> outputUndistortedPoints;
+    inputDistortedPoints.push_back(cv::Point2f(u, v));
+    cv::undistortPoints(inputDistortedPoints, outputUndistortedPoints, cameraMatrix, distCoeffs);
+
+}
+
 TrackingRos::~TrackingRos()
 {
     if (m_isTrackingRun == true)
@@ -264,6 +279,10 @@ void TrackingRos::Init(ros::NodeHandle& nh)
     tests.push_back(point2d(-1, 1));
     angle = correct_angle(tests);
     SDK_LOG(SDK_INFO, "test4 correct_angle = %f", angle);
+
+    float x, y;
+    get_camera_tolidar_loc(200, 200, x, y);
+
 }
 
 void TrackingRos::TrackingCallBackFunc(const jfx_common_msgs::det_tracking_arrayConstPtr& msg)//接受ros发过来的消息
@@ -297,6 +316,8 @@ void TrackingRos::TrackingCallBackFunc(const jfx_common_msgs::det_tracking_array
     m_pubCloud.publish(msg->cloud);
 }
 
+
+
 void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesConstPtr& msg)
 {
     static int countC = 0;