提交更新

TrackingRosEx.cpp

@@ -160,7 +160,8 @@ 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);
-
+    if(outputUndistortedPoints.size() > 0)
+        SDK_LOG(SDK_INFO, "outputUndistortedPoints size = %d, x = %f,y = %f", outputUndistortedPoints.size(), outputUndistortedPoints[0].x, outputUndistortedPoints[0].y);
 }
 
 TrackingRos::~TrackingRos()