Add a function that draws frame axes. Useful for debugging purpose and to check…

Add a function that draws frame axes. Useful for debugging purpose and to check the correctness of the output of a pose estimation method.

modules/calib3d/include/opencv2/calib3d.hpp

@@ -859,6 +859,26 @@ found, or as colored corners connected with lines if the board was found.
 CV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, Size patternSize,
                                          InputArray corners, bool patternWasFound );
 
+/** @brief Draw axes of the world/object coordinate system from pose estimation. @sa solvePnP
+
+@param image Input/output image. It must have 1 or 3 channels. The number of channels is not altered.
+@param cameraMatrix Input 3x3 floating-point matrix of camera intrinsic parameters.
+\f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$
+@param distCoeffs Input vector of distortion coefficients
+\f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
+4, 5, 8, 12 or 14 elements. If the vector is empty, the zero distortion coefficients are assumed.
+@param rvec Rotation vector (see @ref Rodrigues ) that, together with tvec , brings points from
+the model coordinate system to the camera coordinate system.
+@param tvec Translation vector.
+@param length Length of the painted axes in the same unit than tvec (usually in meters).
+@param thickness Line thickness of the painted axes.
+
+This function draws the axes of the world/object coordinate system w.r.t. to the camera frame.
+OX is drawn in red, OY in green and OZ in blue.
+ */
+CV_EXPORTS_W void drawFrameAxes(InputOutputArray image, InputArray cameraMatrix, InputArray distCoeffs,
+                                InputArray rvec, InputArray tvec, float length, int thickness=3);
+
 struct CV_EXPORTS_W_SIMPLE CirclesGridFinderParameters
 {
     CV_WRAP CirclesGridFinderParameters();

modules/calib3d/src/solvepnp.cpp

@@ -52,6 +52,33 @@
 
 namespace cv
 {
+void drawFrameAxes(InputOutputArray image, InputArray cameraMatrix, InputArray distCoeffs,
+                   InputArray rvec, InputArray tvec, float length, int thickness)
+{
+    CV_INSTRUMENT_REGION();
+
+    int type = image.type();
+    int cn = CV_MAT_CN(type);
+    CV_CheckType(type, cn == 1 || cn == 3 || cn == 4,
+                 "Number of channels must be 1, 3 or 4" );
+
+    CV_Assert(image.getMat().total() > 0);
+    CV_Assert(length > 0);
+
+    // project axes points
+    vector<Point3f> axesPoints;
+    axesPoints.push_back(Point3f(0, 0, 0));
+    axesPoints.push_back(Point3f(length, 0, 0));
+    axesPoints.push_back(Point3f(0, length, 0));
+    axesPoints.push_back(Point3f(0, 0, length));
+    vector<Point2f> imagePoints;
+    projectPoints(axesPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints);
+
+    // draw axes lines
+    line(image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255), thickness);
+    line(image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0), thickness);
+    line(image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0), thickness);
+}
 
 bool solvePnP( InputArray _opoints, InputArray _ipoints,
                InputArray _cameraMatrix, InputArray _distCoeffs,

samples/cpp/tutorial_code/features2D/Homography/decompose_homography.cpp

 #include <iostream>
-#include <opencv2/opencv_modules.hpp>
-#ifdef HAVE_OPENCV_ARUCO
 #include <opencv2/core.hpp>
-#include <opencv2/videoio.hpp>
 #include <opencv2/highgui.hpp>
 #include <opencv2/calib3d.hpp>
-#include <opencv2/aruco.hpp>
 
 using namespace std;
 using namespace cv;
@@ -187,10 +183,3 @@ int main(int argc, char *argv[])
 
     return 0;
 }
-#else
-int main()
-{
-    std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
-    return 0;
-}
-#endif

samples/cpp/tutorial_code/features2D/Homography/homography_from_camera_displacement.cpp

 #include <iostream>
-#include <opencv2/opencv_modules.hpp>
-#ifdef HAVE_OPENCV_ARUCO
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/highgui.hpp>
 #include <opencv2/calib3d.hpp>
-#include <opencv2/aruco.hpp>
 
 using namespace std;
 using namespace cv;
@@ -98,8 +95,8 @@ void homographyFromCameraDisplacement(const string &img1Path, const string &img2
 
     Mat img1_copy_pose = img1.clone(), img2_copy_pose = img2.clone();
     Mat img_draw_poses;
-    aruco::drawAxis(img1_copy_pose, cameraMatrix, distCoeffs, rvec1, tvec1, 2*squareSize);
-    aruco::drawAxis(img2_copy_pose, cameraMatrix, distCoeffs, rvec2, tvec2, 2*squareSize);
+    drawFrameAxes(img1_copy_pose, cameraMatrix, distCoeffs, rvec1, tvec1, 2*squareSize);
+    drawFrameAxes(img2_copy_pose, cameraMatrix, distCoeffs, rvec2, tvec2, 2*squareSize);
     hconcat(img1_copy_pose, img2_copy_pose, img_draw_poses);
     imshow("Chessboard poses", img_draw_poses);
 
@@ -202,10 +199,3 @@ int main(int argc, char *argv[])
 
     return 0;
 }
-#else
-int main()
-{
-    std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
-    return 0;
-}
-#endif

samples/cpp/tutorial_code/features2D/Homography/panorama_stitching_rotating_camera.cpp

@@ -2,7 +2,6 @@
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/highgui.hpp>
-#include <opencv2/stitching.hpp>
 
 using namespace std;
 using namespace cv;

samples/cpp/tutorial_code/features2D/Homography/perspective_correction.cpp

 #include <iostream>
-#include <opencv2/opencv_modules.hpp>
-#ifdef HAVE_OPENCV_ARUCO
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/calib3d.hpp>
 #include <opencv2/highgui.hpp>
-#include <opencv2/aruco.hpp>
 
 using namespace std;
 using namespace cv;
@@ -97,10 +94,3 @@ int main(int argc, char *argv[])
 
     return 0;
 }
-#else
-int main()
-{
-    std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
-    return 0;
-}
-#endif

samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp

 #include <iostream>
-#include <opencv2/opencv_modules.hpp>
-#ifdef HAVE_OPENCV_ARUCO
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/calib3d.hpp>
 #include <opencv2/highgui.hpp>
-#include <opencv2/aruco.hpp>
 
 using namespace std;
 using namespace cv;
@@ -121,7 +118,7 @@ void poseEstimationFromCoplanarPoints(const string &imgPath, const string &intri
     //! [display-pose]
     Mat rvec;
     Rodrigues(R, rvec);
-    aruco::drawAxis(img_pose, cameraMatrix, distCoeffs, rvec, tvec, 2*squareSize);
+    drawFrameAxes(img_pose, cameraMatrix, distCoeffs, rvec, tvec, 2*squareSize);
     imshow("Pose from coplanar points", img_pose);
     waitKey();
     //! [display-pose]
@@ -156,10 +153,3 @@ int main(int argc, char *argv[])
 
     return 0;
 }
-#else
-int main()
-{
-    std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
-    return 0;
-}
-#endif