From 644846c702091d63115617a23cfa10dcac4755bc Mon Sep 17 00:00:00 2001 From: catree Date: Thu, 25 Oct 2018 17:57:18 +0200 Subject: [PATCH] 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 | 20 ++++++++++++++ modules/calib3d/src/solvepnp.cpp | 27 +++++++++++++++++++ .../Homography/decompose_homography.cpp | 11 -------- .../homography_from_camera_displacement.cpp | 14 ++-------- .../panorama_stitching_rotating_camera.cpp | 1 - .../Homography/perspective_correction.cpp | 10 ------- .../Homography/pose_from_homography.cpp | 12 +-------- 7 files changed, 50 insertions(+), 45 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index efec733332..c58350b1da 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/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(); diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index c26bca10da..b544595bb5 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/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 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 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, diff --git a/samples/cpp/tutorial_code/features2D/Homography/decompose_homography.cpp b/samples/cpp/tutorial_code/features2D/Homography/decompose_homography.cpp index 3673c3f632..5f671bbc13 100644 --- a/samples/cpp/tutorial_code/features2D/Homography/decompose_homography.cpp +++ b/samples/cpp/tutorial_code/features2D/Homography/decompose_homography.cpp @@ -1,11 +1,7 @@ #include -#include -#ifdef HAVE_OPENCV_ARUCO #include -#include #include #include -#include 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 diff --git a/samples/cpp/tutorial_code/features2D/Homography/homography_from_camera_displacement.cpp b/samples/cpp/tutorial_code/features2D/Homography/homography_from_camera_displacement.cpp index ce2f76fa38..b0bb4da1cc 100644 --- a/samples/cpp/tutorial_code/features2D/Homography/homography_from_camera_displacement.cpp +++ b/samples/cpp/tutorial_code/features2D/Homography/homography_from_camera_displacement.cpp @@ -1,11 +1,8 @@ #include -#include -#ifdef HAVE_OPENCV_ARUCO #include #include #include #include -#include 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 diff --git a/samples/cpp/tutorial_code/features2D/Homography/panorama_stitching_rotating_camera.cpp b/samples/cpp/tutorial_code/features2D/Homography/panorama_stitching_rotating_camera.cpp index 88dcf47477..1dadb4c354 100755 --- a/samples/cpp/tutorial_code/features2D/Homography/panorama_stitching_rotating_camera.cpp +++ b/samples/cpp/tutorial_code/features2D/Homography/panorama_stitching_rotating_camera.cpp @@ -2,7 +2,6 @@ #include #include #include -#include using namespace std; using namespace cv; diff --git a/samples/cpp/tutorial_code/features2D/Homography/perspective_correction.cpp b/samples/cpp/tutorial_code/features2D/Homography/perspective_correction.cpp index 16476c6830..91abddffea 100644 --- a/samples/cpp/tutorial_code/features2D/Homography/perspective_correction.cpp +++ b/samples/cpp/tutorial_code/features2D/Homography/perspective_correction.cpp @@ -1,11 +1,8 @@ #include -#include -#ifdef HAVE_OPENCV_ARUCO #include #include #include #include -#include 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 diff --git a/samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp b/samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp index 50a16c223e..9be865eaab 100644 --- a/samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp +++ b/samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp @@ -1,11 +1,8 @@ #include -#include -#ifdef HAVE_OPENCV_ARUCO #include #include #include #include -#include 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