41 #include <visp3/core/vpConfig.h> 43 #if defined(VISP_HAVE_CATCH2) && defined(VISP_HAVE_APRILTAG) 44 #define CATCH_CONFIG_RUNNER 48 #include <visp3/core/vpDisplay.h> 49 #include <visp3/core/vpIoTools.h> 50 #include <visp3/core/vpPoint.h> 51 #include <visp3/core/vpPixelMeterConversion.h> 52 #include <visp3/detection/vpDetectorAprilTag.h> 53 #include <visp3/io/vpImageIo.h> 54 #include <visp3/vision/vpPose.h> 57 struct TagGroundTruth {
58 std::string m_message;
59 std::vector<vpImagePoint> m_corners;
61 TagGroundTruth(
const std::string &msg,
const std::vector<vpImagePoint> &c) : m_message(msg), m_corners(c) {}
63 bool operator==(
const TagGroundTruth &b)
const 65 if (m_message != b.m_message || m_corners.size() != b.m_corners.size()) {
69 for (
size_t i = 0; i < m_corners.size(); i++) {
71 if (!
vpMath::equal(m_corners[i].get_u(), b.m_corners[i].get_u(), 0.5) ||
72 !
vpMath::equal(m_corners[i].get_v(), b.m_corners[i].get_v(), 0.5)) {
80 bool operator!=(
const TagGroundTruth &b)
const {
return !(*
this == b); }
82 double rmse(
const std::vector<vpImagePoint> &c)
86 if (m_corners.size() == c.size()) {
87 for (
size_t i = 0; i < m_corners.size(); i++) {
97 return sqrt(error / (2*m_corners.size()));
101 #if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3) 102 std::ostream &operator<<(std::ostream &os, TagGroundTruth &t)
104 os << t.m_message << std::endl;
105 for (
size_t i = 0; i < t.m_corners.size(); i++) {
106 os << t.m_corners[i] << std::endl;
113 struct FailedTestCase {
121 : m_family(family), m_method(method), m_tagId(tagId) {}
123 bool operator==(
const FailedTestCase &b)
const 125 return m_family == b.m_family && m_method == b.m_method && m_tagId == b.m_tagId;
128 bool operator!=(
const FailedTestCase &b)
const {
return !(*
this == b); }
132 TEST_CASE(
"Apriltag pose estimation test",
"[apriltag_pose_estimation_test]") {
133 std::map<vpDetectorAprilTag::vpAprilTagFamily, std::string> apriltagMap = {
138 #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY) 146 std::map<vpDetectorAprilTag::vpAprilTagFamily, double> tagSizeScales = {
151 #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY) 161 #if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3) 168 std::map<vpDetectorAprilTag::vpPoseEstimationMethod, std::string> methodNames = {
172 #if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3) 179 const size_t nbTags = 5;
180 const double tagSize_ = 0.25;
181 std::map<int, double> tagsSize = {
189 std::map<int, double> errorTranslationThresh = {
196 std::map<int, double> errorRotationThresh = {
203 std::vector<FailedTestCase> ignoreTests = {
212 std::map<int, vpHomogeneousMatrix> groundTruthPoses;
213 for (
size_t i = 0; i < nbTags; i++) {
215 std::string(
"AprilTag/benchmark/640x480/cMo_") +
216 std::to_string(i) + std::string(
".txt"));
217 std::ifstream file(filename);
218 groundTruthPoses[
static_cast<int>(i)].load(file);
221 for (
const auto& kv : apriltagMap) {
222 auto family = kv.first;
223 std::cout <<
"\nApriltag family: " << family << std::endl;
225 std::string(
"AprilTag/benchmark/640x480/") +
226 kv.second + std::string(
"_640x480.png"));
227 const double tagSize = tagSize_ * tagSizeScales[family];
232 REQUIRE(I.
getSize() == 640*480);
236 for (
auto method : poseMethods) {
237 std::cout <<
"\tPose estimation method: " << method << std::endl;
238 apriltag_detector.setAprilTagPoseEstimationMethod(method);
242 std::vector<vpHomogeneousMatrix> cMo_vec;
243 apriltag_detector.detect(I, tagSize, cam, cMo_vec);
244 CHECK(cMo_vec.size() == nbTags);
246 std::vector<std::vector<vpImagePoint>> tagsCorners = apriltag_detector.getPolygon();
247 CHECK(tagsCorners.size() == nbTags);
249 std::vector<std::string> messages = apriltag_detector.getMessage();
250 CHECK(messages.size() == nbTags);
252 std::vector<int> tagsId = apriltag_detector.getTagsId();
253 CHECK(tagsId.size() == nbTags);
254 std::map<int, int> idsCount;
255 for (
size_t i = 0; i < tagsId.size(); i++) {
256 idsCount[tagsId[i]]++;
257 CHECK((tagsId[i] >= 0 && tagsId[i] < 5));
259 CHECK(idsCount.size() == nbTags);
261 for (
size_t i = 0; i < cMo_vec.size(); i++) {
268 std::cout <<
"\t\tSame size, Tag: " << i << std::endl;
272 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
274 double error_trans = sqrt(error_translation.
sumSquare()/3);
275 double error_orientation = sqrt(error_thetau.
sumSquare() / 3);
276 std::cout <<
"\t\t\tTranslation error: " << error_trans <<
" / Rotation error: " << error_orientation << std::endl;
277 CHECK((error_trans < errorTranslationThresh[
id] &&
278 error_orientation < errorRotationThresh[
id]));
284 apriltag_detector.detect(I);
286 std::vector<std::vector<vpImagePoint>> tagsCorners = apriltag_detector.getPolygon();
287 CHECK(tagsCorners.size() == nbTags);
289 std::vector<std::string> messages = apriltag_detector.getMessage();
290 CHECK(messages.size() == nbTags);
292 std::vector<int> tagsId = apriltag_detector.getTagsId();
293 CHECK(tagsId.size() == nbTags);
294 std::map<int, int> idsCount;
295 for (
size_t i = 0; i < tagsId.size(); i++) {
296 idsCount[tagsId[i]]++;
297 CHECK((tagsId[i] >= 0 && tagsId[i] < 5));
299 CHECK(idsCount.size() == nbTags);
301 for (
size_t idx = 0; idx < tagsId.size(); idx++) {
302 std::cout <<
"\t\tCustom size, Tag: " << idx << std::endl;
303 const int id = tagsId[idx];
305 apriltag_detector.getPose(idx, tagsSize[
id] * tagSizeScales[family], cam, cMo);
311 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
313 double error_trans = sqrt(error_translation.
sumSquare()/3);
314 double error_orientation = sqrt(error_thetau.
sumSquare() / 3);
315 std::cout <<
"\t\t\tTranslation error: " << error_trans <<
" / Rotation error: " << error_orientation << std::endl;
316 if (std::find(ignoreTests.begin(), ignoreTests.end(), FailedTestCase(family, method, static_cast<int>(idx))) == ignoreTests.end()) {
317 CHECK((error_trans < errorTranslationThresh[
id] &&
318 error_orientation < errorRotationThresh[
id]));
325 apriltag_detector.detect(I);
327 std::vector<std::vector<vpImagePoint>> tagsCorners = apriltag_detector.getPolygon();
328 CHECK(tagsCorners.size() == nbTags);
330 std::vector<std::string> messages = apriltag_detector.getMessage();
331 CHECK(messages.size() == nbTags);
333 std::vector<int> tagsId = apriltag_detector.getTagsId();
334 CHECK(tagsId.size() == nbTags);
335 std::map<int, int> idsCount;
336 for (
size_t i = 0; i < tagsId.size(); i++) {
337 idsCount[tagsId[i]]++;
338 CHECK((tagsId[i] >= 0 && tagsId[i] < 5));
340 CHECK(idsCount.size() == nbTags);
342 for (
size_t idx = 0; idx < tagsId.size(); idx++) {
343 std::cout <<
"\t\tCustom size + aligned Z-axis, Tag: " << idx << std::endl;
344 const int id = tagsId[idx];
346 apriltag_detector.setZAlignedWithCameraAxis(
true);
347 apriltag_detector.getPose(idx, tagsSize[
id] * tagSizeScales[family], cam, cMo);
348 apriltag_detector.setZAlignedWithCameraAxis(
false);
355 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
357 double error_trans = sqrt(error_translation.
sumSquare()/3);
358 double error_orientation = sqrt(error_thetau.
sumSquare() / 3);
359 std::cout <<
"\t\t\tTranslation error: " << error_trans <<
" / Rotation error: " << error_orientation << std::endl;
360 if (std::find(ignoreTests.begin(), ignoreTests.end(), FailedTestCase(family, method, static_cast<int>(idx))) == ignoreTests.end()) {
361 CHECK((error_trans < errorTranslationThresh[
id] &&
362 error_orientation < errorRotationThresh[
id]));
370 TEST_CASE(
"Apriltag corners accuracy test",
"[apriltag_corners_accuracy_test]") {
371 std::map<vpDetectorAprilTag::vpAprilTagFamily, std::string> apriltagMap = {
376 #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY) 384 const size_t nbTags = 5;
385 std::map<vpDetectorAprilTag::vpAprilTagFamily, std::map<int, std::vector<vpImagePoint>>> groundTruthCorners;
386 for (
const auto& kv : apriltagMap) {
388 std::string(
"AprilTag/benchmark/640x480/corners_") +
389 kv.second + std::string(
".txt"));
390 std::ifstream file(filename);
391 REQUIRE(file.is_open());
394 double p0x = 0, p0y = 0;
395 double p1x = 0, p1y = 0;
396 double p2x = 0, p2y = 0;
397 double p3x = 0, p3y = 0;
398 while (file >>
id >> p0x >> p0y >> p1x >> p1y >>
399 p2x >> p2y >> p3x >> p3y) {
400 groundTruthCorners[kv.first][id].push_back(
vpImagePoint(p0y, p0x));
401 groundTruthCorners[kv.first][id].push_back(
vpImagePoint(p1y, p1x));
402 groundTruthCorners[kv.first][id].push_back(
vpImagePoint(p2y, p2x));
403 groundTruthCorners[kv.first][id].push_back(
vpImagePoint(p3y, p3x));
404 REQUIRE(groundTruthCorners[kv.first][
id].size() == 4);
408 for (
const auto& kv : apriltagMap) {
409 auto family = kv.first;
410 std::cout <<
"\nApriltag family: " << family << std::endl;
412 std::string(
"AprilTag/benchmark/640x480/") +
413 kv.second + std::string(
"_640x480.png"));
418 REQUIRE(I.
getSize() == 640*480);
421 apriltag_detector.detect(I);
422 std::vector<int> tagsId = apriltag_detector.getTagsId();
423 std::vector<std::vector<vpImagePoint>> tagsCorners = apriltag_detector.getTagsCorners();
425 REQUIRE(tagsCorners.size() == nbTags);
426 REQUIRE(tagsId.size() == nbTags);
427 for (
size_t i = 0; i < tagsCorners.size(); i++) {
428 const int tagId = tagsId[i];
429 REQUIRE(tagsCorners[i].size() == 4);
431 TagGroundTruth corners_ref(
"", groundTruthCorners[family][tagId]);
432 TagGroundTruth corners_cur(
"", tagsCorners[i]);
433 CHECK((corners_ref == corners_cur));
435 std::cout <<
"\tid: " << tagId <<
" - RMSE: " << corners_ref.rmse(corners_cur.m_corners) << std::endl;
440 #if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3) 441 TEST_CASE(
"Apriltag regression test",
"[apriltag_regression_test]") {
447 REQUIRE(I.
getSize() == 640*480);
451 const double tagSize = 0.053;
452 const float quad_decimate = 1.0;
455 dynamic_cast<vpDetectorAprilTag *
>(detector)->setAprilTagPoseEstimationMethod(poseEstimationMethod);
460 std::vector<vpHomogeneousMatrix> cMo_vec;
464 std::map<std::string, TagGroundTruth> mapOfTagsGroundTruth;
467 "AprilTag/ground_truth_detection.txt");
468 std::ifstream file_ground_truth(filename_ground_truth.c_str());
469 REQUIRE(file_ground_truth.is_open());
470 std::string message =
"";
471 double v1 = 0.0, v2 = 0.0, v3 = 0.0, v4 = 0.0;
472 double u1 = 0.0, u2 = 0.0, u3 = 0.0, u4 = 0.0;
473 while (file_ground_truth >> message >> v1 >> u1 >> v2 >> u2 >> v3 >> u3 >> v4 >> u4) {
474 std::vector<vpImagePoint> tagCorners(4);
475 tagCorners[0].set_ij(v1, u1);
476 tagCorners[1].set_ij(v2, u2);
477 tagCorners[2].set_ij(v3, u3);
478 tagCorners[3].set_ij(v4, u4);
479 mapOfTagsGroundTruth.insert(std::make_pair(message, TagGroundTruth(message, tagCorners)));
483 std::map<std::string, vpPoseVector> mapOfPosesGroundTruth;
486 "AprilTag/ground_truth_pose.txt");
487 std::ifstream file_ground_truth(filename_ground_truth.c_str());
488 REQUIRE(file_ground_truth.is_open());
489 std::string message =
"";
490 double tx = 0.0, ty = 0.0, tz = 0.0;
491 double tux = 0.0, tuy = 0.0, tuz = 0.0;
492 while (file_ground_truth >> message >> tx >> ty >> tz >> tux >> tuy >> tuz) {
493 mapOfPosesGroundTruth.insert(std::make_pair(message,
vpPoseVector(tx, ty, tz, tux, tuy, tuz)));
498 std::vector<vpImagePoint> p = detector->
getPolygon(i);
500 std::string message = detector->
getMessage(i);
501 std::replace(message.begin(), message.end(),
' ',
'_');
502 std::map<std::string, TagGroundTruth>::iterator it = mapOfTagsGroundTruth.find(message);
503 TagGroundTruth current(message, p);
504 if (it == mapOfTagsGroundTruth.end()) {
505 std::cerr <<
"Problem with tag decoding (tag_family or id): " << message << std::endl;
506 }
else if (it->second != current) {
507 std::cerr <<
"Problem, current detection:\n" << current <<
"\nReference:\n" << it->second << std::endl;
509 REQUIRE(it != mapOfTagsGroundTruth.end());
510 CHECK(it->second == current);
513 for (
size_t i = 0; i < cMo_vec.size(); i++) {
516 std::string message = detector->
getMessage(i);
517 std::replace(message.begin(), message.end(),
' ',
'_');
518 std::map<std::string, vpPoseVector>::iterator it = mapOfPosesGroundTruth.find(message);
519 if (it == mapOfPosesGroundTruth.end()) {
520 std::cerr <<
"Problem with tag decoding (tag_family or id): " << message << std::endl;
522 REQUIRE(it != mapOfPosesGroundTruth.end());
523 std::cout <<
"Tag: " << message << std::endl;
524 std::cout <<
"\tEstimated pose: " << pose_vec.t() << std::endl;
525 std::cout <<
"\tReference pose: " << it->second.t() << std::endl;
526 for (
unsigned int cpt = 0; cpt < 3; cpt++) {
529 std::cerr <<
"Problem, current pose: " << pose_vec.t() <<
"\nReference pose: " << it->second.t()
532 CHECK((
vpMath::equal(it->second[cpt], pose_vec[cpt], 0.005) &&
540 TEST_CASE(
"Apriltag copy constructor test",
"[apriltag_copy_constructor_test]") {
542 "AprilTag/benchmark/640x480/tag21_07_640x480.png");
547 REQUIRE(I.
getSize() == 640*480);
551 const double tagSize = 0.25 * 5/9;
552 const float quad_decimate = 1.0;
559 std::vector<vpHomogeneousMatrix> cMo_vec;
560 detector->
detect(I, tagSize, cam, cMo_vec);
561 std::vector<std::vector<vpImagePoint> > tagsCorners = detector->
getTagsCorners();
562 std::vector<int> tagsId = detector->
getTagsId();
569 std::vector<std::vector<vpImagePoint> > tagsCorners_copy = detector_copy.
getTagsCorners();
570 std::vector<int> tagsId_copy = detector_copy.getTagsId();
571 REQUIRE(tagsCorners_copy.size() == tagsCorners.size());
572 REQUIRE(tagsId_copy.size() == tagsId.size());
573 REQUIRE(tagsCorners_copy.size() == tagsId_copy.size());
575 for (
size_t i = 0; i < tagsCorners.size(); i++) {
576 const std::vector<vpImagePoint>& corners_ref = tagsCorners[i];
577 const std::vector<vpImagePoint>& corners_copy = tagsCorners_copy[i];
578 REQUIRE(corners_ref.size() == corners_copy.size());
580 for (
size_t j = 0; j < corners_ref.size(); j++) {
583 CHECK(corner_ref == corner_copy);
586 int id_ref = tagsId[i];
587 int id_copy = tagsId_copy[i];
588 CHECK(id_ref == id_copy);
591 std::vector<vpHomogeneousMatrix> cMo_vec_copy;
592 detector_copy.detect(I, tagSize, cam, cMo_vec_copy);
593 REQUIRE(cMo_vec.size() == cMo_vec_copy.size());
594 for (
size_t idx = 0; idx < cMo_vec_copy.size(); idx++) {
597 for (
unsigned int i = 0; i < 3; i++) {
598 for (
unsigned int j = 0; j < 4; j++) {
600 std::numeric_limits<double>::epsilon()));
606 TEST_CASE(
"Apriltag assignment operator test",
"[apriltag_assignment_operator_test]") {
608 "AprilTag/benchmark/640x480/tag21_07_640x480.png");
613 REQUIRE(I.
getSize() == 640*480);
617 const double tagSize = 0.25 * 5/9;
618 const float quad_decimate = 1.0;
625 std::vector<vpHomogeneousMatrix> cMo_vec;
626 detector->
detect(I, tagSize, cam, cMo_vec);
627 std::vector<std::vector<vpImagePoint> > tagsCorners = detector->
getTagsCorners();
628 std::vector<int> tagsId = detector->
getTagsId();
635 std::vector<std::vector<vpImagePoint> > tagsCorners_copy = detector_copy.
getTagsCorners();
636 std::vector<int> tagsId_copy = detector_copy.
getTagsId();
637 REQUIRE(tagsCorners_copy.size() == tagsCorners.size());
638 REQUIRE(tagsId_copy.size() == tagsId.size());
639 REQUIRE(tagsCorners_copy.size() == tagsId_copy.size());
641 for (
size_t i = 0; i < tagsCorners.size(); i++) {
642 const std::vector<vpImagePoint>& corners_ref = tagsCorners[i];
643 const std::vector<vpImagePoint>& corners_copy = tagsCorners_copy[i];
644 REQUIRE(corners_ref.size() == corners_copy.size());
646 for (
size_t j = 0; j < corners_ref.size(); j++) {
649 CHECK(corner_ref == corner_copy);
652 int id_ref = tagsId[i];
653 int id_copy = tagsId_copy[i];
654 CHECK(id_ref == id_copy);
657 std::vector<vpHomogeneousMatrix> cMo_vec_copy;
658 detector_copy.
detect(I, tagSize, cam, cMo_vec_copy);
659 REQUIRE(cMo_vec.size() == cMo_vec_copy.size());
660 for (
size_t idx = 0; idx < cMo_vec_copy.size(); idx++) {
663 for (
unsigned int i = 0; i < 3; i++) {
664 for (
unsigned int j = 0; j < 4; j++) {
666 std::numeric_limits<double>::epsilon()));
672 TEST_CASE(
"Apriltag getTagsPoints3D test",
"[apriltag_get_tags_points3D_test]") {
674 "AprilTag/benchmark/640x480/tag21_07_640x480.png");
679 REQUIRE(I.
getSize() == 640*480);
683 const double familyScale = 5.0/9;
684 const double tagSize = 0.25;
685 std::map<int, double> tagsSize = {
686 {-1, tagSize * familyScale},
687 {3, tagSize / 2 * familyScale},
688 {4, tagSize / 2 * familyScale}
696 std::vector<vpHomogeneousMatrix> cMo_vec;
697 REQUIRE(detector.
detect(I));
700 std::vector<int> tagsId = detector.
getTagsId();
701 for (
size_t i = 0; i < tagsId.size(); i++) {
703 double size = tagsSize[-1];
704 if (tagsSize.find(
id) != tagsSize.end()) {
709 detector.
getPose(i, size, cam, cMo);
710 cMo_vec.push_back(cMo);
714 std::vector<std::vector<vpPoint>> tagsPoints = detector.
getTagsPoints3D(tagsId, tagsSize);
715 std::vector<std::vector<vpImagePoint>> tagsCorners = detector.
getTagsCorners();
716 REQUIRE(tagsPoints.size() == tagsCorners.size());
718 for (
size_t i = 0; i < tagsPoints.size(); i++) {
719 REQUIRE(tagsPoints[i].size() == tagsCorners[i].size());
721 for (
size_t j = 0; j < tagsPoints[i].size(); j++) {
722 vpPoint& pt = tagsPoints[i][j];
730 vpPose pose(tagsPoints[i]);
738 #ifdef VISP_HAVE_LAPACK 739 double epsilon = std::numeric_limits<double>::epsilon();
741 double epsilon = 1e-12;
743 for (
unsigned int row = 0; row < cMo.
getRows(); row++) {
744 for (
unsigned int col = 0; col < cMo.
getCols(); col++) {
745 CHECK(
vpMath::equal(cMo[row][col], cMo_manual[row][col], epsilon));
750 #endif // #if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3) 752 int main(
int argc,
const char *argv[])
754 Catch::Session session;
757 session.applyCommandLine(argc, argv);
759 int numFailed = session.run();
AprilTag Standard52h13 pattern.
Implementation of an homogeneous matrix and operations on such kind of matrices.
AprilTag 36h11 pattern (recommended)
static bool equal(double x, double y, double s=0.001)
AprilTag Circle21h7 pattern.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
std::vector< std::vector< vpImagePoint > > getTagsCorners() const
unsigned int getCols() const
bool getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2=NULL, double *projError=NULL, double *projError2=NULL)
size_t getNbObjects() const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void set_x(double x)
Set the point x coordinate in the image plane.
AprilTag Circle49h12 pattern.
void set_y(double y)
Set the point y coordinate in the image plane.
void setAprilTagQuadDecimate(float quadDecimate)
unsigned int getSize() const
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Generic class defining intrinsic camera parameters.
std::vector< std::vector< vpPoint > > getTagsPoints3D(const std::vector< int > &tagsId, const std::map< int, double > &tagsSize) const
unsigned int getRows() const
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
AprilTag Standard41h12 pattern.
static void read(vpImage< unsigned char > &I, const std::string &filename)
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
std::vector< int > getTagsId() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
AprilTag Custom48h12 pattern.
std::vector< std::string > & getMessage()
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
std::vector< std::vector< vpImagePoint > > & getPolygon()
bool detect(const vpImage< unsigned char > &I)