Skip to content

Commit

Permalink
add axes test, remove drawAxis(), update tutorial
Browse files Browse the repository at this point in the history
  • Loading branch information
@AleksandrPanov
AleksandrPanov committed Mar 10, 2022
1 parent c8936ad commit 30824a4
Show file tree
Hide file tree
Showing 27 changed files with 96 additions and 100 deletions.
25 changes: 2 additions & 23 deletions modules/aruco/include/opencv2/aruco.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -476,36 +476,15 @@ 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.
*
* @sa use cv::drawFrameAxes to get world coordinate system axis for object points
*/
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, int thickness=3);



/**
* @brief Draw a canonical marker image
*
Expand Down
3 changes: 2 additions & 1 deletion modules/aruco/include/opencv2/aruco/charuco.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -275,6 +275,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 @@ -286,7 +287,7 @@ CV_EXPORTS_W void detectCharucoDiamond(InputArray image, InputArrayOfArrays mark
OutputArrayOfArrays diamondCorners, OutputArray diamondIds,
InputArray cameraMatrix = noArray(),
InputArray distCoeffs = noArray(),
Ptr<Dictionary> dict = getPredefinedDictionary(PREDEFINED_DICTIONARY_NAME(0)));
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
28 changes: 5 additions & 23 deletions modules/aruco/src/aruco.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -811,11 +811,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 @@ -1783,24 +1783,6 @@ void drawDetectedMarkers(InputOutputArray _image, InputArrayOfArrays _corners,
}



/**
*/
void drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec,
InputArray _tvec, float length, int thickness)
{
vector<Point3f> axis;
axis.push_back(Point3f(0.f, 0.f, 0.f));
axis.push_back(Point3f(length, 0.f, 0.f));
axis.push_back(Point3f(0.f, length, 0.f));
axis.push_back(Point3f(0.f, 0.f, -length));
vector<Point2f> axis_to_img;
projectPoints(axis, _rvec, _tvec, _cameraMatrix, _distCoeffs, axis_to_img);
line(_image, Point2i(axis_to_img[0]), Point2i(axis_to_img[1]), Scalar(255,0,0), thickness);
line(_image, Point2i(axis_to_img[0]), Point2i(axis_to_img[2]), Scalar(0,255,0), thickness);
line(_image, Point2i(axis_to_img[0]), Point2i(axis_to_img[3]), Scalar(0,0,255), thickness);
}

/**
*/
void drawMarker(const Ptr<Dictionary> &dictionary, int id, int sidePixels, OutputArray _img, int borderBits) {
Expand Down
13 changes: 6 additions & 7 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 @@ -743,14 +742,14 @@ double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfAr
void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners,
InputArray _markerIds, float squareMarkerLengthRate,
OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds,
InputArray _cameraMatrix, InputArray _distCoeffs, Ptr<Dictionary> dict) {
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<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 @@ -842,10 +841,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
21 changes: 21 additions & 0 deletions modules/aruco/test/test_aruco_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,27 @@
namespace opencv_test {
namespace {

static inline vector<Point2f> getAxis(InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec,
InputArray _tvec, float length, const float offset = 0.f)
{
vector<Point3f> axis;
axis.push_back(Point3f(offset, offset, 0.f));
axis.push_back(Point3f(length+offset, offset, 0.f));
axis.push_back(Point3f(offset, length+offset, 0.f));
axis.push_back(Point3f(offset, offset, length));
vector<Point2f> axis_to_img;
projectPoints(axis, _rvec, _tvec, _cameraMatrix, _distCoeffs, axis_to_img);
return axis_to_img;
}

static inline vector<Point2f> getMarkerById(int id, const vector<vector<Point2f> >& corners, const vector<int>& ids)
{
for (size_t i = 0ull; i < ids.size(); i++)
if (ids[i] == id)
return corners[i];
return vector<Point2f>();
}

static inline double deg2rad(double deg) { return deg * CV_PI / 180.; }

/**
Expand Down
26 changes: 17 additions & 9 deletions modules/aruco/test/test_boarddetection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,9 +70,9 @@ void CV_ArucoBoardPose::run(int) {
Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0));

// for different perspectives
for(double distance = 0.2; distance <= 0.4; distance += 0.2) {
for(int yaw = -60; yaw <= 60; yaw += 30) {
for(int pitch = -60; pitch <= 60; pitch += 30) {
for(double distance = 0.2; distance <= 0.4; distance += 0.15) {
for(int yaw = -55; yaw <= 50; yaw += 25) {
for(int pitch = -55; pitch <= 50; pitch += 25) {
for(unsigned int i = 0; i < gridboard->ids.size(); i++)
gridboard->ids[i] = (iter + int(i)) % 250;
int markerBorder = iter % 2 + 1;
Expand All @@ -87,17 +87,25 @@ void CV_ArucoBoardPose::run(int) {
params->markerBorderBits = markerBorder;
aruco::detectMarkers(img, dictionary, corners, ids, params);

if(ids.size() == 0) {
ts->printf(cvtest::TS::LOG, "Marker detection failed in Board test");
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
return;
}
ASSERT_EQ(ids.size(), gridboard->ids.size());

// estimate pose
Mat rvec, tvec;
aruco::estimatePoseBoard(corners, ids, board, cameraMatrix, distCoeffs, rvec, tvec);

// check result
// check axes
vector<Point2f> axes = getAxis(cameraMatrix, distCoeffs, rvec, tvec, gridboard->rightBottomBorder.x);
vector<Point2f> topLeft = getMarkerById(gridboard->ids[0], corners, ids);
ASSERT_NEAR(topLeft[0].x, axes[0].x, 2.f);
ASSERT_NEAR(topLeft[0].y, axes[0].y, 2.f);
vector<Point2f> topRight = getMarkerById(gridboard->ids[2], corners, ids);
ASSERT_NEAR(topRight[1].x, axes[1].x, 2.f);
ASSERT_NEAR(topRight[1].y, axes[1].y, 2.f);
vector<Point2f> bottomLeft = getMarkerById(gridboard->ids[6], corners, ids);
ASSERT_NEAR(bottomLeft[3].x, axes[2].x, 2.f);
ASSERT_NEAR(bottomLeft[3].y, axes[2].y, 2.f);

// check estimate result
for(unsigned int i = 0; i < ids.size(); i++) {
int foundIdx = -1;
for(unsigned int j = 0; j < gridboard->ids.size(); j++) {
Expand Down
34 changes: 20 additions & 14 deletions modules/aruco/test/test_charucodetection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -266,11 +266,7 @@ void CV_CharucoPoseEstimation::run(int) {
params->markerBorderBits = markerBorder;
aruco::detectMarkers(img, dictionary, corners, ids, params);

if(ids.size() == 0) {
ts->printf(cvtest::TS::LOG, "Marker detection failed");
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
return;
}
ASSERT_EQ(ids.size(), board->ids.size());

// interpolate charuco corners
vector< Point2f > charucoCorners;
Expand All @@ -291,7 +287,17 @@ void CV_CharucoPoseEstimation::run(int) {
distCoeffs, rvec, tvec);


// check result
// check axes
const float offset = (board->getSquareLength() - board->getMarkerLength()) / 2.f;
vector<Point2f> axes = getAxis(cameraMatrix, distCoeffs, rvec, tvec, board->getSquareLength(), offset);
vector<Point2f> topLeft = getMarkerById(board->ids[0], corners, ids);
ASSERT_NEAR(topLeft[0].x, axes[1].x, 3.f);
ASSERT_NEAR(topLeft[0].y, axes[1].y, 3.f);
vector<Point2f> bottomLeft = getMarkerById(board->ids[2], corners, ids);
ASSERT_NEAR(bottomLeft[0].x, axes[2].x, 3.f);
ASSERT_NEAR(bottomLeft[0].y, axes[2].y, 3.f);

// check estimate result
vector< Point2f > projectedCharucoCorners;

projectPoints(board->chessboardCorners, rvec, tvec, cameraMatrix, distCoeffs,
Expand Down Expand Up @@ -416,10 +422,10 @@ void CV_CharucoDiamondDetection::run(int) {
projectedDiamondCorners);

vector< Point2f > projectedDiamondCornersReorder(4);
projectedDiamondCornersReorder[0] = projectedDiamondCorners[2];
projectedDiamondCornersReorder[1] = projectedDiamondCorners[3];
projectedDiamondCornersReorder[2] = projectedDiamondCorners[1];
projectedDiamondCornersReorder[3] = projectedDiamondCorners[0];
projectedDiamondCornersReorder[0] = projectedDiamondCorners[0];
projectedDiamondCornersReorder[1] = projectedDiamondCorners[1];
projectedDiamondCornersReorder[2] = projectedDiamondCorners[3];
projectedDiamondCornersReorder[3] = projectedDiamondCorners[2];


for(unsigned int i = 0; i < 4; i++) {
Expand All @@ -441,10 +447,10 @@ void CV_CharucoDiamondDetection::run(int) {
// check result
vector< Point2f > projectedDiamondCornersPose;
vector< Vec3f > diamondObjPoints(4);
diamondObjPoints[0] = Vec3f(-squareLength / 2.f, squareLength / 2.f, 0);
diamondObjPoints[1] = Vec3f(squareLength / 2.f, squareLength / 2.f, 0);
diamondObjPoints[2] = Vec3f(squareLength / 2.f, -squareLength / 2.f, 0);
diamondObjPoints[3] = Vec3f(-squareLength / 2.f, -squareLength / 2.f, 0);
diamondObjPoints[0] = Vec3f(0.f, 0.f, 0);
diamondObjPoints[1] = Vec3f(squareLength, 0.f, 0);
diamondObjPoints[2] = Vec3f(squareLength, squareLength, 0);
diamondObjPoints[3] = Vec3f(0.f, squareLength, 0);
projectPoints(diamondObjPoints, estimatedRvec[0], estimatedTvec[0], cameraMatrix,
distCoeffs, projectedDiamondCornersPose);

Expand Down
Loading

0 comments on commit 30824a4

Please sign in to comment.