Visual Servoing Platform version 3.5.0
testAprilTag.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Test AprilTag detection.
33 *
34 *****************************************************************************/
41#include <visp3/core/vpConfig.h>
42
43#if defined(VISP_HAVE_CATCH2) && defined(VISP_HAVE_APRILTAG)
44#define CATCH_CONFIG_RUNNER
45#include <catch.hpp>
46
47#include <iostream>
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>
55namespace
56{
57struct TagGroundTruth {
58 std::string m_message;
59 std::vector<vpImagePoint> m_corners;
60
61 TagGroundTruth(const std::string &msg, const std::vector<vpImagePoint> &c) : m_message(msg), m_corners(c) {}
62
63 bool operator==(const TagGroundTruth &b) const
64 {
65 if (m_message != b.m_message || m_corners.size() != b.m_corners.size()) {
66 return false;
67 }
68
69 for (size_t i = 0; i < m_corners.size(); i++) {
70 // Allow 0.5 pixel of difference
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)) {
73 return false;
74 }
75 }
76
77 return true;
78 }
79
80 bool operator!=(const TagGroundTruth &b) const { return !(*this == b); }
81
82 double rmse(const std::vector<vpImagePoint> &c)
83 {
84 double error = 0;
85
86 if (m_corners.size() == c.size()) {
87 for (size_t i = 0; i < m_corners.size(); i++) {
88 const vpImagePoint& a = m_corners[i];
89 const vpImagePoint& b = c[i];
90 error += (a.get_i()-b.get_i())*(a.get_i()-b.get_i()) +
91 (a.get_j()-b.get_j())*(a.get_j()-b.get_j());
92 }
93 } else {
94 return -1;
95 }
96
97 return sqrt(error / (2*m_corners.size()));
98 }
99};
100
101#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
102std::ostream &operator<<(std::ostream &os, TagGroundTruth &t)
103{
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;
107 }
108
109 return os;
110}
111#endif
112
113struct FailedTestCase {
116 int m_tagId;
117
118 FailedTestCase(const vpDetectorAprilTag::vpAprilTagFamily &family,
120 int tagId)
121 : m_family(family), m_method(method), m_tagId(tagId) {}
122
123 bool operator==(const FailedTestCase &b) const
124 {
125 return m_family == b.m_family && m_method == b.m_method && m_tagId == b.m_tagId;
126 }
127
128 bool operator!=(const FailedTestCase &b) const { return !(*this == b); }
129};
130}
131
132TEST_CASE("Apriltag pose estimation test", "[apriltag_pose_estimation_test]") {
133 std::map<vpDetectorAprilTag::vpAprilTagFamily, std::string> apriltagMap = {
134 {vpDetectorAprilTag::TAG_16h5, "tag16_05"},
135 {vpDetectorAprilTag::TAG_25h9, "tag25_09"},
136 {vpDetectorAprilTag::TAG_36h11, "tag36_11"},
138 #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
143 #endif
144 };
145
146 std::map<vpDetectorAprilTag::vpAprilTagFamily, double> tagSizeScales = {
151 #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
156 #endif
157 };
158
159 std::vector<vpDetectorAprilTag::vpPoseEstimationMethod> poseMethods = {vpDetectorAprilTag::HOMOGRAPHY,
161#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
166#endif
167 };
168 std::map<vpDetectorAprilTag::vpPoseEstimationMethod, std::string> methodNames = {
169 {vpDetectorAprilTag::HOMOGRAPHY, "HOMOGRAPHY"},
170 {vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS, "HOMOGRAPHY_VIRTUAL_VS"},
171 {vpDetectorAprilTag::HOMOGRAPHY_ORTHOGONAL_ITERATION, "HOMOGRAPHY_ORTHOGONAL_ITERATION"},
172#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
173 {vpDetectorAprilTag::DEMENTHON_VIRTUAL_VS, "DEMENTHON_VIRTUAL_VS"},
174 {vpDetectorAprilTag::LAGRANGE_VIRTUAL_VS, "LAGRANGE_VIRTUAL_VS"},
175 {vpDetectorAprilTag::BEST_RESIDUAL_VIRTUAL_VS, "BEST_RESIDUAL_VIRTUAL_VS"}
176#endif
177 };
178
179 const size_t nbTags = 5;
180 const double tagSize_ = 0.25;
181 std::map<int, double> tagsSize = {
182 {0, tagSize_},
183 {1, tagSize_},
184 {2, tagSize_},
185 {3, tagSize_/2},
186 {4, tagSize_/2},
187 };
188
189 std::map<int, double> errorTranslationThresh = {
190 {0, 0.025},
191 {1, 0.09},
192 {2, 0.05},
193 {3, 0.13},
194 {4, 0.09},
195 };
196 std::map<int, double> errorRotationThresh = {
197 {0, 0.04},
198 {1, 0.075},
199 {2, 0.07},
200 {3, 0.18},
201 {4, 0.13},
202 };
203 std::vector<FailedTestCase> ignoreTests = {
207 };
208
210 cam.initPersProjWithoutDistortion(700.0, 700.0, 320.0, 240.0);
211
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);
219 }
220
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];
228 REQUIRE(vpIoTools::checkFilename(filename));
229
231 vpImageIo::read(I, filename);
232 REQUIRE(I.getSize() == 640*480);
233
234 vpDetectorAprilTag apriltag_detector(family);
235
236 for (auto method : poseMethods) {
237 std::cout << "\tPose estimation method: " << method << std::endl;
238 apriltag_detector.setAprilTagPoseEstimationMethod(method);
239
240 //Same tags size
241 {
242 std::vector<vpHomogeneousMatrix> cMo_vec;
243 apriltag_detector.detect(I, tagSize, cam, cMo_vec);
244 CHECK(cMo_vec.size() == nbTags);
245
246 std::vector<std::vector<vpImagePoint>> tagsCorners = apriltag_detector.getPolygon();
247 CHECK(tagsCorners.size() == nbTags);
248
249 std::vector<std::string> messages = apriltag_detector.getMessage();
250 CHECK(messages.size() == nbTags);
251
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));
258 }
259 CHECK(idsCount.size() == nbTags);
260
261 for (size_t i = 0; i < cMo_vec.size(); i++) {
262 const vpHomogeneousMatrix& cMo = cMo_vec[i];
263 const vpPoseVector pose(cMo);
264 int id = tagsId[i];
265 if (id >= 3) {
266 continue;
267 }
268 std::cout << "\t\tSame size, Tag: " << i << std::endl;
269 const vpHomogeneousMatrix& cMo_truth = groundTruthPoses[id];
270 const vpPoseVector pose_truth(cMo_truth);
271
272 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
273 vpColVector error_thetau = vpColVector(pose.getThetaUVector()) - vpColVector(pose_truth.getThetaUVector());
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]));
279 }
280 }
281
282 //Custom tags size
283 {
284 apriltag_detector.detect(I);
285
286 std::vector<std::vector<vpImagePoint>> tagsCorners = apriltag_detector.getPolygon();
287 CHECK(tagsCorners.size() == nbTags);
288
289 std::vector<std::string> messages = apriltag_detector.getMessage();
290 CHECK(messages.size() == nbTags);
291
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));
298 }
299 CHECK(idsCount.size() == nbTags);
300
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);
306
307 const vpPoseVector pose(cMo);
308 const vpHomogeneousMatrix& cMo_truth = groundTruthPoses[id];
309 const vpPoseVector pose_truth(cMo_truth);
310
311 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
312 vpColVector error_thetau = vpColVector(pose.getThetaUVector()) - vpColVector(pose_truth.getThetaUVector());
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]));
319 }
320 }
321 }
322
323 //Custom tags size + aligned Z-axis
324 {
325 apriltag_detector.detect(I);
326
327 std::vector<std::vector<vpImagePoint>> tagsCorners = apriltag_detector.getPolygon();
328 CHECK(tagsCorners.size() == nbTags);
329
330 std::vector<std::string> messages = apriltag_detector.getMessage();
331 CHECK(messages.size() == nbTags);
332
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));
339 }
340 CHECK(idsCount.size() == nbTags);
341
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);
349
350 const vpPoseVector pose(cMo);
351 const vpHomogeneousMatrix oMo2(vpTranslationVector(), vpThetaUVector(M_PI, 0, 0));
352 const vpHomogeneousMatrix cMo_truth = groundTruthPoses[id] * oMo2;
353 const vpPoseVector pose_truth(cMo_truth);
354
355 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
356 vpColVector error_thetau = vpColVector(pose.getThetaUVector()) - vpColVector(pose_truth.getThetaUVector());
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]));
363 }
364 }
365 }
366 }
367 }
368}
369
370TEST_CASE("Apriltag corners accuracy test", "[apriltag_corners_accuracy_test]") {
371 std::map<vpDetectorAprilTag::vpAprilTagFamily, std::string> apriltagMap = {
372 {vpDetectorAprilTag::TAG_16h5, "tag16_05"},
373 {vpDetectorAprilTag::TAG_25h9, "tag25_09"},
374 {vpDetectorAprilTag::TAG_36h11, "tag36_11"},
376 #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
381 #endif
382 };
383
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());
392
393 int id = 0;
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);
405 }
406 }
407
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"));
414 REQUIRE(vpIoTools::checkFilename(filename));
415
417 vpImageIo::read(I, filename);
418 REQUIRE(I.getSize() == 640*480);
419
420 vpDetectorAprilTag apriltag_detector(family);
421 apriltag_detector.detect(I);
422 std::vector<int> tagsId = apriltag_detector.getTagsId();
423 std::vector<std::vector<vpImagePoint>> tagsCorners = apriltag_detector.getTagsCorners();
424
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);
430
431 TagGroundTruth corners_ref("", groundTruthCorners[family][tagId]);
432 TagGroundTruth corners_cur("", tagsCorners[i]);
433 CHECK((corners_ref == corners_cur));
434
435 std::cout << "\tid: " << tagId << " - RMSE: " << corners_ref.rmse(corners_cur.m_corners) << std::endl;
436 }
437 }
438}
439
440#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
441TEST_CASE("Apriltag regression test", "[apriltag_regression_test]") {
442 const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "AprilTag/AprilTag.pgm");
443 REQUIRE(vpIoTools::checkFilename(filename));
444
446 vpImageIo::read(I, filename);
447 REQUIRE(I.getSize() == 640*480);
448
451 const double tagSize = 0.053;
452 const float quad_decimate = 1.0;
453 vpDetectorBase *detector = new vpDetectorAprilTag(tagFamily);
454 dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagQuadDecimate(quad_decimate);
455 dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagPoseEstimationMethod(poseEstimationMethod);
456
458 cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779);
459
460 std::vector<vpHomogeneousMatrix> cMo_vec;
461 dynamic_cast<vpDetectorAprilTag *>(detector)->detect(I, tagSize, cam, cMo_vec);
462
463 // Ground truth
464 std::map<std::string, TagGroundTruth> mapOfTagsGroundTruth;
465 {
466 std::string filename_ground_truth = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(),
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)));
480 }
481 }
482
483 std::map<std::string, vpPoseVector> mapOfPosesGroundTruth;
484 {
485 std::string filename_ground_truth = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(),
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)));
494 }
495 }
496
497 for (size_t i = 0; i < detector->getNbObjects(); i++) {
498 std::vector<vpImagePoint> p = detector->getPolygon(i);
499
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;
508 }
509 REQUIRE(it != mapOfTagsGroundTruth.end());
510 CHECK(it->second == current);
511 }
512
513 for (size_t i = 0; i < cMo_vec.size(); i++) {
514 vpPoseVector pose_vec(cMo_vec[i]);
515
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;
521 }
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++) {
527 if (!vpMath::equal(it->second[cpt], pose_vec[cpt], 0.005) ||
528 !vpMath::equal(it->second[cpt+3], pose_vec[cpt+3], 0.005)) {
529 std::cerr << "Problem, current pose: " << pose_vec.t() << "\nReference pose: " << it->second.t()
530 << std::endl;
531 }
532 CHECK((vpMath::equal(it->second[cpt], pose_vec[cpt], 0.005) &&
533 vpMath::equal(it->second[cpt+3], pose_vec[cpt+3], 0.005)));
534 }
535 }
536
537 delete detector;
538}
539
540TEST_CASE("Apriltag copy constructor test", "[apriltag_copy_constructor_test]") {
541 const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(),
542 "AprilTag/benchmark/640x480/tag21_07_640x480.png");
543 REQUIRE(vpIoTools::checkFilename(filename));
544
546 vpImageIo::read(I, filename);
547 REQUIRE(I.getSize() == 640*480);
548
551 const double tagSize = 0.25 * 5/9;
552 const float quad_decimate = 1.0;
553 vpDetectorAprilTag *detector = new vpDetectorAprilTag(tagFamily, poseEstimationMethod);
554 detector->setAprilTagQuadDecimate(quad_decimate);
555
557 cam.initPersProjWithoutDistortion(700, 700, 320, 240);
558
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();
563
564 //Copy
565 vpDetectorAprilTag detector_copy(*detector);
566 //Delete old detector
567 delete detector;
568
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());
574
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());
579
580 for (size_t j = 0; j < corners_ref.size(); j++) {
581 const vpImagePoint& corner_ref = corners_ref[j];
582 const vpImagePoint& corner_copy = corners_copy[j];
583 CHECK(corner_ref == corner_copy);
584 }
585
586 int id_ref = tagsId[i];
587 int id_copy = tagsId_copy[i];
588 CHECK(id_ref == id_copy);
589 }
590
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++) {
595 const vpHomogeneousMatrix& cMo = cMo_vec[idx];
596 const vpHomogeneousMatrix& cMo_copy = cMo_vec_copy[idx];
597 for (unsigned int i = 0; i < 3; i++) {
598 for (unsigned int j = 0; j < 4; j++) {
599 CHECK(vpMath::equal(cMo[i][j], cMo_copy[i][j],
600 std::numeric_limits<double>::epsilon()));
601 }
602 }
603 }
604}
605
606TEST_CASE("Apriltag assignment operator test", "[apriltag_assignment_operator_test]") {
607 const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(),
608 "AprilTag/benchmark/640x480/tag21_07_640x480.png");
609 REQUIRE(vpIoTools::checkFilename(filename));
610
612 vpImageIo::read(I, filename);
613 REQUIRE(I.getSize() == 640*480);
614
617 const double tagSize = 0.25 * 5/9;
618 const float quad_decimate = 1.0;
619 vpDetectorAprilTag *detector = new vpDetectorAprilTag(tagFamily, poseEstimationMethod);
620 detector->setAprilTagQuadDecimate(quad_decimate);
621
623 cam.initPersProjWithoutDistortion(700, 700, 320, 240);
624
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();
629
630 //Copy
631 vpDetectorAprilTag detector_copy = *detector;
632 //Delete old detector
633 delete detector;
634
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());
640
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());
645
646 for (size_t j = 0; j < corners_ref.size(); j++) {
647 const vpImagePoint& corner_ref = corners_ref[j];
648 const vpImagePoint& corner_copy = corners_copy[j];
649 CHECK(corner_ref == corner_copy);
650 }
651
652 int id_ref = tagsId[i];
653 int id_copy = tagsId_copy[i];
654 CHECK(id_ref == id_copy);
655 }
656
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++) {
661 const vpHomogeneousMatrix& cMo = cMo_vec[idx];
662 const vpHomogeneousMatrix& cMo_copy = cMo_vec_copy[idx];
663 for (unsigned int i = 0; i < 3; i++) {
664 for (unsigned int j = 0; j < 4; j++) {
665 CHECK(vpMath::equal(cMo[i][j], cMo_copy[i][j],
666 std::numeric_limits<double>::epsilon()));
667 }
668 }
669 }
670}
671
672TEST_CASE("Apriltag getTagsPoints3D test", "[apriltag_get_tags_points3D_test]") {
673 const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(),
674 "AprilTag/benchmark/640x480/tag21_07_640x480.png");
675 REQUIRE(vpIoTools::checkFilename(filename));
676
678 vpImageIo::read(I, filename);
679 REQUIRE(I.getSize() == 640*480);
680
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}
689 };
690
691 vpDetectorAprilTag detector(tagFamily, poseEstimationMethod);
692
694 cam.initPersProjWithoutDistortion(700, 700, 320, 240);
695
696 std::vector<vpHomogeneousMatrix> cMo_vec;
697 REQUIRE(detector.detect(I));
698
699 //Compute pose with getPose
700 std::vector<int> tagsId = detector.getTagsId();
701 for (size_t i = 0; i < tagsId.size(); i++) {
702 int id = tagsId[i];
703 double size = tagsSize[-1];
704 if (tagsSize.find(id) != tagsSize.end()) {
705 size = tagsSize[id];
706 }
707
709 detector.getPose(i, size, cam, cMo);
710 cMo_vec.push_back(cMo);
711 }
712
713 //Compute pose manually
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());
717
718 for (size_t i = 0; i < tagsPoints.size(); i++) {
719 REQUIRE(tagsPoints[i].size() == tagsCorners[i].size());
720
721 for (size_t j = 0; j < tagsPoints[i].size(); j++) {
722 vpPoint& pt = tagsPoints[i][j];
723 const vpImagePoint& imPt = tagsCorners[i][j];
724 double x = 0, y = 0;
726 pt.set_x(x);
727 pt.set_y(y);
728 }
729
730 vpPose pose(tagsPoints[i]);
731 vpHomogeneousMatrix cMo_manual;
732 pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo_manual);
733
734 const vpHomogeneousMatrix& cMo = cMo_vec[i];
735 // Note that using epsilon = std::numeric_limits<double>::epsilon() makes this test
736 // failing on Ubuntu 18.04 when none of the Lapack 3rd party libraries, nor the built-in are used.
737 // Admissible espilon value is 1e-14. Using 1e-15 makes the test failing.
738 // Again on Debian i386 where Lapack is enable, using std::numeric_limits<double>::epsilon()
739 // makes this test failing.
740 double epsilon = 1e-12;
741
742 for (unsigned int row = 0; row < cMo.getRows(); row++) {
743 for (unsigned int col = 0; col < cMo.getCols(); col++) {
744 CHECK(vpMath::equal(cMo[row][col], cMo_manual[row][col], epsilon));
745 }
746 }
747 }
748}
749#endif // #if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
750
751int main(int argc, const char *argv[])
752{
753 Catch::Session session; // There must be exactly one instance
754
755 // Let Catch (using Clara) parse the command line
756 session.applyCommandLine(argc, argv);
757
758 int numFailed = session.run();
759
760 // numFailed is clamped to 255 as some unices only use the lower 8 bits.
761 // This clamping has already been applied, so just return it here
762 // You can also do any post run clean-up here
763 return numFailed;
764}
765#else
766int main()
767{
768 return 0;
769}
770#endif
unsigned int getCols() const
Definition: vpArray2D.h:279
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
unsigned int getRows() const
Definition: vpArray2D.h:289
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
std::vector< std::vector< vpImagePoint > > getTagsCorners() const
void setAprilTagQuadDecimate(float quadDecimate)
std::vector< std::vector< vpPoint > > getTagsPoints3D(const std::vector< int > &tagsId, const std::map< int, double > &tagsSize) const
bool getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2=NULL, double *projError=NULL, double *projError2=NULL)
@ TAG_CIRCLE21h7
AprilTag Circle21h7 pattern.
@ TAG_25h9
AprilTag 25h9 pattern.
@ TAG_CUSTOM48h12
AprilTag Custom48h12 pattern.
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
@ TAG_STANDARD52h13
AprilTag Standard52h13 pattern.
@ TAG_16h5
AprilTag 16h5 pattern.
@ TAG_STANDARD41h12
AprilTag Standard41h12 pattern.
@ TAG_CIRCLE49h12
AprilTag Circle49h12 pattern.
bool detect(const vpImage< unsigned char > &I)
std::vector< int > getTagsId() const
std::vector< std::vector< vpImagePoint > > & getPolygon()
std::vector< std::string > & getMessage()
size_t getNbObjects() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:149
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double get_j() const
Definition: vpImagePoint.h:214
double get_i() const
Definition: vpImagePoint.h:203
unsigned int getSize() const
Definition: vpImage.h:227
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1365
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:802
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1670
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:511
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:513
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:81
@ DEMENTHON_VIRTUAL_VS
Definition: vpPose.h:97
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.