Skip to content

Commit

Permalink
merge PatternPos/EstimateParameters after rebase
Browse files Browse the repository at this point in the history
  • Loading branch information
@AleksandrPanov
AleksandrPanov committed Jun 7, 2022
1 parent b4cd02a commit 2b78dfe
Show file tree
Hide file tree
Showing 3 changed files with 90 additions and 17 deletions.
71 changes: 65 additions & 6 deletions modules/aruco/include/opencv2/aruco/aruco_calib_pose.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,13 +4,65 @@
#ifndef __OPENCV_ARUCO_CALIB_POSE_HPP__
#define __OPENCV_ARUCO_CALIB_POSE_HPP__
#include <opencv2/aruco/board.hpp>
#include <opencv2/calib3d.hpp>

namespace cv {
namespace aruco {

//! @addtogroup aruco
//! @{

/** @brief
* rvec/tvec define the right handed coordinate system of the marker.
* PatternPos defines center this system and axes direction.
* Axis X (red color) - first coordinate, axis Y (green color) - second coordinate,
* axis Z (blue color) - third coordinate.
* @sa estimatePoseSingleMarkers(), @ref tutorial_aruco_detection
*/
enum PatternPos {
/** @brief The marker coordinate system is centered on the middle of the marker.
* The coordinates of the four corners (CCW order) of the marker in its own coordinate system are:
* (-markerLength/2, markerLength/2, 0), (markerLength/2, markerLength/2, 0),
* (markerLength/2, -markerLength/2, 0), (-markerLength/2, -markerLength/2, 0).
*
* These pattern points define this coordinate system:
* ![Image with axes drawn](images/singlemarkersaxes.jpg)
*/
CCW_center,
/** @brief The marker coordinate system is centered on the top-left corner of the marker.
* The coordinates of the four corners (CW order) of the marker in its own coordinate system are:
* (0, 0, 0), (markerLength, 0, 0),
* (markerLength, markerLength, 0), (0, markerLength, 0).
*
* These pattern points define this coordinate system:
* ![Image with axes drawn](images/singlemarkersaxes2.jpg)
*/
CW_top_left_corner
};

/** @brief
* Pose estimation parameters
* @param pattern Defines center this system and axes direction (default PatternPos::CCW_center).
* @param useExtrinsicGuess Parameter used for SOLVEPNP_ITERATIVE. If true (1), the function uses the provided
* rvec and tvec values as initial approximations of the rotation and translation vectors, respectively, and further
* optimizes them (default false).
* @param solvePnPMethod Method for solving a PnP problem: see @ref calib3d_solvePnP_flags (default SOLVEPNP_ITERATIVE).
* @sa PatternPos, solvePnP(), @ref tutorial_aruco_detection
*/
struct CV_EXPORTS_W EstimateParameters {
CV_PROP_RW PatternPos pattern;
CV_PROP_RW bool useExtrinsicGuess;
CV_PROP_RW SolvePnPMethod solvePnPMethod;

EstimateParameters(): pattern(CCW_center), useExtrinsicGuess(false),
solvePnPMethod(SOLVEPNP_ITERATIVE) {}

CV_WRAP static Ptr<EstimateParameters> create() {
return makePtr<EstimateParameters>();
}
};


/**
* @brief Pose estimation for single markers
*
Expand All @@ -29,21 +81,28 @@ namespace aruco {
* @param tvecs array of output translation vectors (e.g. std::vector<cv::Vec3d>).
* Each element in tvecs corresponds to the specific marker in imgPoints.
* @param _objPoints array of object points of all the marker corners
* @param estimateParameters set the origin of coordinate system and the coordinates of the four corners of the marker
* (default estimateParameters.pattern = PatternPos::CCW_center, estimateParameters.useExtrinsicGuess = false,
* estimateParameters.solvePnPMethod = SOLVEPNP_ITERATIVE).
*
* This function receives the detected markers and returns their pose estimation respect to
* the camera individually. So for each marker, one rotation and translation vector is returned.
* The returned transformation is the one that transforms points from each marker coordinate system
* to the camera coordinate system.
* The marker corrdinate system is centered on the middle of the marker, with the Z axis
* perpendicular to the marker plane.
* The coordinates of the four corners of the marker in its own coordinate system are:
* (0, 0, 0), (markerLength, 0, 0),
* (markerLength, markerLength, 0), (0, markerLength, 0)
* The marker coordinate system is centered on the middle (by default) or on the top-left corner of the marker,
* with the Z axis perpendicular to the marker plane.
* estimateParameters defines the coordinates of the four corners of the marker in its own coordinate system (by default) are:
* (-markerLength/2, markerLength/2, 0), (markerLength/2, markerLength/2, 0),
* (markerLength/2, -markerLength/2, 0), (-markerLength/2, -markerLength/2, 0)
* @sa use cv::drawFrameAxes to get world coordinate system axis for object points
* @sa @ref tutorial_aruco_detection
* @sa EstimateParameters
* @sa PatternPos
*/
CV_EXPORTS_W void estimatePoseSingleMarkers(InputArrayOfArrays corners, float markerLength,
InputArray cameraMatrix, InputArray distCoeffs,
OutputArray rvecs, OutputArray tvecs, OutputArray _objPoints = noArray());
OutputArray rvecs, OutputArray tvecs, OutputArray _objPoints = noArray(),
Ptr<EstimateParameters> estimateParameters = EstimateParameters::create());

/**
* @brief Pose estimation for a board of markers
Expand Down
34 changes: 23 additions & 11 deletions modules/aruco/src/aruco_calib_pose.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
// of this distribution and at http://opencv.org/license.html

#include <opencv2/aruco/aruco_calib_pose.hpp>
#include <opencv2/calib3d.hpp>

namespace cv {
namespace aruco {
Expand Down Expand Up @@ -41,24 +40,37 @@ void getBoardObjectAndImagePoints(const Ptr<Board> &board, InputArrayOfArrays de
}

/**
* @brief Return object points for the system centered in a single marker, given the marker length
* @brief Return object points for the system centered in a middle (by default) or in a top left corner of single
* marker, given the marker length
*/
static Mat _getSingleMarkerObjectPoints(float markerLength) {
static Mat _getSingleMarkerObjectPoints(float markerLength, const EstimateParameters& estimateParameters) {
CV_Assert(markerLength > 0);
Mat objPoints(4, 1, CV_32FC3);
// set coordinate system in the top-left corner of the marker, with Z pointing out
objPoints.ptr< Vec3f >(0)[0] = Vec3f(0.f, 0.f, 0);
objPoints.ptr< Vec3f >(0)[1] = Vec3f(markerLength, 0.f, 0);
objPoints.ptr< Vec3f >(0)[2] = Vec3f(markerLength, markerLength, 0);
objPoints.ptr< Vec3f >(0)[3] = Vec3f(0.f, markerLength, 0);
if (estimateParameters.pattern == CW_top_left_corner) {
objPoints.ptr<Vec3f>(0)[0] = Vec3f(0.f, 0.f, 0);
objPoints.ptr<Vec3f>(0)[1] = Vec3f(markerLength, 0.f, 0);
objPoints.ptr<Vec3f>(0)[2] = Vec3f(markerLength, markerLength, 0);
objPoints.ptr<Vec3f>(0)[3] = Vec3f(0.f, markerLength, 0);
}
else if (estimateParameters.pattern == CCW_center) {
objPoints.ptr<Vec3f>(0)[0] = Vec3f(-markerLength/2.f, markerLength/2.f, 0);
objPoints.ptr<Vec3f>(0)[1] = Vec3f(markerLength/2.f, markerLength/2.f, 0);
objPoints.ptr<Vec3f>(0)[2] = Vec3f(markerLength/2.f, -markerLength/2.f, 0);
objPoints.ptr<Vec3f>(0)[3] = Vec3f(-markerLength/2.f, -markerLength/2.f, 0);
}
else
CV_Error(Error::StsBadArg, "Unknown estimateParameters pattern");
return objPoints;
}

void estimatePoseSingleMarkers(InputArrayOfArrays _corners, float markerLength, InputArray _cameraMatrix,
InputArray _distCoeffs, OutputArray _rvecs, OutputArray _tvecs, OutputArray _objPoints) {
void estimatePoseSingleMarkers(InputArrayOfArrays _corners, float markerLength,
InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvecs, OutputArray _tvecs, OutputArray _objPoints,
Ptr<EstimateParameters> estimateParameters) {
CV_Assert(markerLength > 0);

Mat markerObjPoints = _getSingleMarkerObjectPoints(markerLength);
Mat markerObjPoints = _getSingleMarkerObjectPoints(markerLength, *estimateParameters);
int nMarkers = (int)_corners.total();
_rvecs.create(nMarkers, 1, CV_64FC3);
_tvecs.create(nMarkers, 1, CV_64FC3);
Expand All @@ -72,7 +84,7 @@ void estimatePoseSingleMarkers(InputArrayOfArrays _corners, float markerLength,

for (int i = begin; i < end; i++) {
solvePnP(markerObjPoints, _corners.getMat(i), _cameraMatrix, _distCoeffs, rvecs.at<Vec3d>(i),
tvecs.at<Vec3d>(i));
tvecs.at<Vec3d>(i), estimateParameters->useExtrinsicGuess, estimateParameters->solvePnPMethod);
}
});

Expand Down
2 changes: 2 additions & 0 deletions modules/aruco/src/aruco_detector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -755,7 +755,9 @@ static void _refineCandidateLines(std::vector<Point>& nContours, std::vector<Poi
cntPts[group].push_back(contour2f[i]);
}
for (int i = 0; i < 4; i++)
{
CV_Assert(cornerIndex[i] != -1);
}
// saves extra group into corresponding
if( !cntPts[4].empty() ){
for( unsigned int i=0; i < cntPts[4].size() ; i++ )
Expand Down

0 comments on commit 2b78dfe

Please sign in to comment.