Skip to content

Commit

Permalink
Merge pull request opencv#3174 from AleksandrPanov:fix_gridboard_objP…
Browse files Browse the repository at this point in the history 
…oints

Fix objPoints order in GridBoard and CharucoBoard

* fix gridBoard

* fix charucoBoard

* add rightBottomBorder

* add test_aruco_utils and code refactoring/fix tests

* fix axes and add charuco dict

* add axes test, remove drawAxis(), update tutorial

(cherry picked from commit 56d492c)
  • Loading branch information
@AleksandrPanov
Alexander Panov authored and AleksandrPanov committed Mar 25, 2022
1 parent 1d0115a commit b4701d2
Show file tree
Hide file tree
Showing 27 changed files with 286 additions and 349 deletions.
42 changes: 16 additions & 26 deletions modules/aruco/include/opencv2/aruco.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -272,8 +272,9 @@ CV_EXPORTS_W void detectMarkers(InputArray image, const Ptr<Dictionary> &diction
* 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:
* (-markerLength/2, markerLength/2, 0), (markerLength/2, markerLength/2, 0),
* (markerLength/2, -markerLength/2, 0), (-markerLength/2, -markerLength/2, 0)
* (0, 0, 0), (markerLength, 0, 0),
* (markerLength, markerLength, 0), (0, markerLength, 0)
* @sa use cv::drawFrameAxes to get world coordinate system axis for object points
*/
CV_EXPORTS_W void estimatePoseSingleMarkers(InputArrayOfArrays corners, float markerLength,
InputArray cameraMatrix, InputArray distCoeffs,
Expand Down Expand Up @@ -316,7 +317,14 @@ class CV_EXPORTS_W Board {
CV_WRAP void setIds(InputArray ids);

/// array of object points of all the marker corners in the board
/// each marker include its 4 corners in CCW order. For M markers, the size is Mx4.
/// each marker include its 4 corners in this order:
///- objPoints[i][0] - left-top point of i-th marker
///- objPoints[i][1] - right-top point of i-th marker
///- objPoints[i][2] - right-bottom point of i-th marker
///- objPoints[i][3] - left-bottom point of i-th marker
///
/// Markers are placed in a certain order - row by row, left to right in every row.
/// For M markers, the size is Mx4.
CV_PROP std::vector< std::vector< Point3f > > objPoints;

/// the dictionary of markers employed for this board
Expand All @@ -325,6 +333,9 @@ class CV_EXPORTS_W Board {
/// vector of the identifiers of the markers in the board (same size than objPoints)
/// The identifiers refers to the board dictionary
CV_PROP_RW std::vector< int > ids;

/// coordinate of the bottom right corner of the board, is set when calling the function create()
CV_PROP Point3f rightBottomBorder;
};


Expand Down Expand Up @@ -424,6 +435,7 @@ class CV_EXPORTS_W GridBoard : public Board {
* Input markers that are not included in the board layout are ignored.
* The function returns the number of markers from the input employed for the board pose estimation.
* Note that returning a 0 means the pose has not been estimated.
* @sa use cv::drawFrameAxes to get world coordinate system axis for object points
*/
CV_EXPORTS_W int estimatePoseBoard(InputArrayOfArrays corners, InputArray ids, const Ptr<Board> &board,
InputArray cameraMatrix, InputArray distCoeffs, InputOutputArray rvec,
Expand Down Expand Up @@ -488,36 +500,14 @@ CV_EXPORTS_W void refineDetectedMarkers(
* Given an array of detected marker corners and its corresponding ids, this functions draws
* the markers in the image. The marker borders are painted and the markers identifiers if provided.
* Useful for debugging purposes.
*
*/
CV_EXPORTS_W void drawDetectedMarkers(InputOutputArray image, InputArrayOfArrays corners,
InputArray ids = noArray(),
Scalar borderColor = Scalar(0, 255, 0));



/**
* @brief Draw coordinate system axis from pose estimation
*
* @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 camera matrix
* \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$
* @param distCoeffs 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]])\f$ of 4, 5, 8 or 12 elements
* @param rvec rotation vector of the coordinate system that will be drawn. (@sa Rodrigues).
* @param tvec translation vector of the coordinate system that will be drawn.
* @param length length of the painted axis in the same unit than tvec (usually in meters)
*
* Given the pose estimation of a marker or board, this function draws the axis of the world
* coordinate system, i.e. the system centered on the marker/board. Useful for debugging purposes.
*
* @deprecated use cv::drawFrameAxes
*/
CV_EXPORTS_W void drawAxis(InputOutputArray image, InputArray cameraMatrix, InputArray distCoeffs,
InputArray rvec, InputArray tvec, float length);



/**
* @brief Draw a canonical marker image
*
Expand Down
5 changes: 4 additions & 1 deletion modules/aruco/include/opencv2/aruco/charuco.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,6 +181,7 @@ CV_EXPORTS_W int interpolateCornersCharuco(InputArrayOfArrays markerCorners, Inp
* This function estimates a Charuco board pose from some detected corners.
* The function checks if the input corners are enough and valid to perform pose estimation.
* If pose estimation is valid, returns true, else returns false.
* @sa use cv::drawFrameAxes to get world coordinate system axis for object points
*/
CV_EXPORTS_W bool estimatePoseCharucoBoard(InputArray charucoCorners, InputArray charucoIds,
const Ptr<CharucoBoard> &board, InputArray cameraMatrix,
Expand Down Expand Up @@ -275,6 +276,7 @@ CV_EXPORTS_W double calibrateCameraCharuco(
* diamond.
* @param cameraMatrix Optional camera calibration matrix.
* @param distCoeffs Optional camera distortion coefficients.
* @param dictionary dictionary of markers indicating the type of markers.
*
* This function detects Diamond markers from the previous detected ArUco markers. The diamonds
* are returned in the diamondCorners and diamondIds parameters. If camera calibration parameters
Expand All @@ -285,7 +287,8 @@ CV_EXPORTS_W void detectCharucoDiamond(InputArray image, InputArrayOfArrays mark
InputArray markerIds, float squareMarkerLengthRate,
OutputArrayOfArrays diamondCorners, OutputArray diamondIds,
InputArray cameraMatrix = noArray(),
InputArray distCoeffs = noArray());
InputArray distCoeffs = noArray(),
Ptr<Dictionary> dictionary = getPredefinedDictionary(PREDEFINED_DICTIONARY_NAME(0)));



Expand Down
2 changes: 1 addition & 1 deletion modules/aruco/samples/calibrate_camera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ int main(int argc, char *argv[]) {
if(parser.get<bool>("zt")) calibrationFlags |= CALIB_ZERO_TANGENT_DIST;
if(parser.get<bool>("pc")) calibrationFlags |= CALIB_FIX_PRINCIPAL_POINT;

Ptr<aruco::DetectorParameters> detectorParams;
Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
if(parser.has("dp")) {
FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);
Expand Down
2 changes: 1 addition & 1 deletion modules/aruco/samples/calibrate_camera_charuco.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ int main(int argc, char *argv[]) {
if(parser.get<bool>("zt")) calibrationFlags |= CALIB_ZERO_TANGENT_DIST;
if(parser.get<bool>("pc")) calibrationFlags |= CALIB_FIX_PRINCIPAL_POINT;

Ptr<aruco::DetectorParameters> detectorParams;
Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
if(parser.has("dp")) {
FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);
Expand Down
4 changes: 2 additions & 2 deletions modules/aruco/samples/detect_board.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ int main(int argc, char *argv[]) {
}
}

Ptr<aruco::DetectorParameters> detectorParams;
Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
if(parser.has("dp")) {
FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);
Expand Down Expand Up @@ -199,7 +199,7 @@ int main(int argc, char *argv[]) {
aruco::drawDetectedMarkers(imageCopy, rejected, noArray(), Scalar(100, 0, 255));

if(markersOfBoardDetected > 0)
aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength);
cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength);

imshow("out", imageCopy);
char key = (char)waitKey(waitTime);
Expand Down
4 changes: 2 additions & 2 deletions modules/aruco/samples/detect_board_charuco.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ int main(int argc, char *argv[]) {
}
}

Ptr<aruco::DetectorParameters> detectorParams;
Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
if(parser.has("dp")) {
FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);
Expand Down Expand Up @@ -213,7 +213,7 @@ int main(int argc, char *argv[]) {
}

if(validPose)
aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength);
cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength);

imshow("out", imageCopy);
char key = (char)waitKey(waitTime);
Expand Down
6 changes: 3 additions & 3 deletions modules/aruco/samples/detect_diamonds.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ int main(int argc, char *argv[]) {
bool autoScale = parser.has("as");
float autoScaleFactor = autoScale ? parser.get<float>("as") : 1.f;

Ptr<aruco::DetectorParameters> detectorParams;
Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
if(parser.has("dp")) {
FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);
Expand Down Expand Up @@ -219,8 +219,8 @@ int main(int argc, char *argv[]) {

if(estimatePose) {
for(unsigned int i = 0; i < diamondIds.size(); i++)
aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i],
squareLength * 0.5f);
cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i],
squareLength * 1.1f);
}
}

Expand Down
5 changes: 2 additions & 3 deletions modules/aruco/samples/detect_markers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ int main(int argc, char *argv[]) {
bool estimatePose = parser.has("c");
float markerLength = parser.get<float>("l");

Ptr<aruco::DetectorParameters> detectorParams;
Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
if(parser.has("dp")) {
FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);
Expand Down Expand Up @@ -179,8 +179,7 @@ int main(int argc, char *argv[]) {

if(estimatePose) {
for(unsigned int i = 0; i < ids.size(); i++)
aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i],
markerLength * 0.5f);
cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i], markerLength * 1.5f, 2);
}
}

Expand Down
2 changes: 1 addition & 1 deletion modules/aruco/samples/tutorial_charuco_create_detect.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ static inline void detectCharucoBoardWithCalibrationPose()
//! [pose]
// if charuco pose is valid
if (valid)
cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvec, tvec, 0.1f);
cv::drawFrameAxes(imageCopy, cameraMatrix, distCoeffs, rvec, tvec, 0.1f);
}
}
cv::imshow("out", imageCopy);
Expand Down
42 changes: 19 additions & 23 deletions modules/aruco/src/aruco.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -785,11 +785,11 @@ static void _getSingleMarkerObjectPoints(float markerLength, OutputArray _objPoi

_objPoints.create(4, 1, CV_32FC3);
Mat objPoints = _objPoints.getMat();
// set coordinate system in the middle of the marker, with Z pointing out
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);
// 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);
}

/**
Expand Down Expand Up @@ -1594,6 +1594,7 @@ Ptr<Board> Board::create(InputArrayOfArrays objPoints, const Ptr<Dictionary> &di
CV_Assert(objPoints.type() == CV_32FC3 || objPoints.type() == CV_32FC1);

std::vector< std::vector< Point3f > > obj_points_vector;
Point3f rightBottomBorder = Point3f(0.f, 0.f, 0.f);
for (unsigned int i = 0; i < objPoints.total(); i++) {
std::vector<Point3f> corners;
Mat corners_mat = objPoints.getMat(i);
Expand All @@ -1603,7 +1604,11 @@ Ptr<Board> Board::create(InputArrayOfArrays objPoints, const Ptr<Dictionary> &di
CV_Assert(corners_mat.total() == 4);

for (int j = 0; j < 4; j++) {
corners.push_back(corners_mat.at<Point3f>(j));
const Point3f& corner = corners_mat.at<Point3f>(j);
corners.push_back(corner);
rightBottomBorder.x = std::max(rightBottomBorder.x, corner.x);
rightBottomBorder.y = std::max(rightBottomBorder.y, corner.y);
rightBottomBorder.z = std::max(rightBottomBorder.z, corner.z);
}
obj_points_vector.push_back(corners);
}
Expand All @@ -1612,6 +1617,7 @@ Ptr<Board> Board::create(InputArrayOfArrays objPoints, const Ptr<Dictionary> &di
ids.copyTo(res->ids);
res->objPoints = obj_points_vector;
res->dictionary = cv::makePtr<Dictionary>(dictionary);
res->rightBottomBorder = rightBottomBorder;
return res;
}

Expand Down Expand Up @@ -1647,20 +1653,19 @@ Ptr<GridBoard> GridBoard::create(int markersX, int markersY, float markerLength,
}

// calculate Board objPoints
float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparation;
for(int y = 0; y < markersY; y++) {
for(int x = 0; x < markersX; x++) {
vector< Point3f > corners;
corners.resize(4);
vector<Point3f> corners(4);
corners[0] = Point3f(x * (markerLength + markerSeparation),
maxY - y * (markerLength + markerSeparation), 0);
y * (markerLength + markerSeparation), 0);
corners[1] = corners[0] + Point3f(markerLength, 0, 0);
corners[2] = corners[0] + Point3f(markerLength, -markerLength, 0);
corners[3] = corners[0] + Point3f(0, -markerLength, 0);
corners[2] = corners[0] + Point3f(markerLength, markerLength, 0);
corners[3] = corners[0] + Point3f(0, markerLength, 0);
res->objPoints.push_back(corners);
}
}

res->rightBottomBorder = Point3f(markersX * markerLength + markerSeparation * (markersX - 1),
markersY * markerLength + markerSeparation * (markersY - 1), 0.f);
return res;
}

Expand Down Expand Up @@ -1712,15 +1717,6 @@ void drawDetectedMarkers(InputOutputArray _image, InputArrayOfArrays _corners,
}



/**
*/
void drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec,
InputArray _tvec, float length)
{
drawFrameAxes(_image, _cameraMatrix, _distCoeffs, _rvec, _tvec, length, 3);
}

/**
*/
void drawMarker(const Ptr<Dictionary> &dictionary, int id, int sidePixels, OutputArray _img, int borderBits) {
Expand Down Expand Up @@ -1785,7 +1781,7 @@ void _drawPlanarBoardImpl(Board *_board, Size outSize, OutputArray _img, int mar
// move top left to 0, 0
pf -= Point2f(minX, minY);
pf.x = pf.x / sizeX * float(out.cols);
pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
pf.y = pf.y / sizeY * float(out.rows);
outCorners[j] = pf;
}

Expand Down
30 changes: 14 additions & 16 deletions modules/aruco/src/charuco.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,6 @@ the use of this software, even if advised of the possibility of such damage.
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>


namespace cv {
namespace aruco {

Expand Down Expand Up @@ -106,7 +105,7 @@ void CharucoBoard::draw(Size outSize, OutputArray _img, int marginSize, int bord

double startX, startY;
startX = squareSizePixels * double(x);
startY = double(chessboardZoneImg.rows) - squareSizePixels * double(y + 1);
startY = squareSizePixels * double(y);

Mat squareZone = chessboardZoneImg.rowRange(int(startY), int(startY + squareSizePixels))
.colRange(int(startX), int(startX + squareSizePixels));
Expand Down Expand Up @@ -135,18 +134,17 @@ Ptr<CharucoBoard> CharucoBoard::create(int squaresX, int squaresY, float squareL
float diffSquareMarkerLength = (squareLength - markerLength) / 2;

// calculate Board objPoints
for(int y = squaresY - 1; y >= 0; y--) {
for(int y = 0; y < squaresY; y++) {
for(int x = 0; x < squaresX; x++) {

if(y % 2 == x % 2) continue; // black corner, no marker here

vector< Point3f > corners;
corners.resize(4);
vector<Point3f> corners(4);
corners[0] = Point3f(x * squareLength + diffSquareMarkerLength,
y * squareLength + diffSquareMarkerLength + markerLength, 0);
y * squareLength + diffSquareMarkerLength, 0);
corners[1] = corners[0] + Point3f(markerLength, 0, 0);
corners[2] = corners[0] + Point3f(markerLength, -markerLength, 0);
corners[3] = corners[0] + Point3f(0, -markerLength, 0);
corners[2] = corners[0] + Point3f(markerLength, markerLength, 0);
corners[3] = corners[0] + Point3f(0, markerLength, 0);
res->objPoints.push_back(corners);
// first ids in dictionary
int nextId = (int)res->ids.size();
Expand All @@ -164,7 +162,8 @@ Ptr<CharucoBoard> CharucoBoard::create(int squaresX, int squaresY, float squareL
res->chessboardCorners.push_back(corner);
}
}

res->rightBottomBorder = Point3f(squaresX * squareLength,
squaresY * squareLength, 0.f);
res->_getNearestMarkerCorners();

return res;
Expand Down Expand Up @@ -701,15 +700,14 @@ double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfAr
void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners,
InputArray _markerIds, float squareMarkerLengthRate,
OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds,
InputArray _cameraMatrix, InputArray _distCoeffs) {
InputArray _cameraMatrix, InputArray _distCoeffs, Ptr<Dictionary> dictionary) {

CV_Assert(_markerIds.total() > 0 && _markerIds.total() == _markerCorners.total());

const float minRepDistanceRate = 1.302455f;

// create Charuco board layout for diamond (3x3 layout)
Ptr<Dictionary> dict = getPredefinedDictionary(PREDEFINED_DICTIONARY_NAME(0));
Ptr<CharucoBoard> _charucoDiamondLayout = CharucoBoard::create(3, 3, squareMarkerLengthRate, 1., dict);
Ptr<CharucoBoard> _charucoDiamondLayout = CharucoBoard::create(3, 3, squareMarkerLengthRate, 1., dictionary);


vector< vector< Point2f > > diamondCorners;
Expand Down Expand Up @@ -801,10 +799,10 @@ void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners,
// reorder corners
vector< Point2f > currentMarkerCornersReorder;
currentMarkerCornersReorder.resize(4);
currentMarkerCornersReorder[0] = currentMarkerCorners[2];
currentMarkerCornersReorder[1] = currentMarkerCorners[3];
currentMarkerCornersReorder[2] = currentMarkerCorners[1];
currentMarkerCornersReorder[3] = currentMarkerCorners[0];
currentMarkerCornersReorder[0] = currentMarkerCorners[0];
currentMarkerCornersReorder[1] = currentMarkerCorners[1];
currentMarkerCornersReorder[2] = currentMarkerCorners[3];
currentMarkerCornersReorder[3] = currentMarkerCorners[2];

diamondCorners.push_back(currentMarkerCornersReorder);
diamondIds.push_back(markerId);
Expand Down
Loading

0 comments on commit b4701d2

Please sign in to comment.