Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testAprilTag.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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) && (VISP_HAVE_DATASET_VERSION >= 0x030300)
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/vpPixelMeterConversion.h>
51#include <visp3/core/vpPoint.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()) + (a.get_j() - b.get_j()) * (a.get_j() - b.get_j());
91 }
92 } else {
93 return -1;
94 }
95
96 return sqrt(error / (2 * m_corners.size()));
97 }
98};
99
100#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
101std::ostream &operator<<(std::ostream &os, TagGroundTruth &t)
102{
103 os << t.m_message << std::endl;
104 for (size_t i = 0; i < t.m_corners.size(); i++) {
105 os << t.m_corners[i] << std::endl;
106 }
107
108 return os;
109}
110#endif
111
112struct FailedTestCase {
115 int m_tagId;
116
117 FailedTestCase(const vpDetectorAprilTag::vpAprilTagFamily &family,
118 const vpDetectorAprilTag::vpPoseEstimationMethod &method, int tagId)
119 : m_family(family), m_method(method), m_tagId(tagId)
120 {
121 }
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} // namespace
131
132TEST_CASE("Apriltag pose estimation test", "[apriltag_pose_estimation_test]")
133{
134 std::map<vpDetectorAprilTag::vpAprilTagFamily, std::string> apriltagMap = {
135 {vpDetectorAprilTag::TAG_16h5, "tag16_05"},
136 {vpDetectorAprilTag::TAG_25h9, "tag25_09"},
137 {vpDetectorAprilTag::TAG_36h11, "tag36_11"},
139#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
140 ,
145#endif
146 };
147
148 std::map<vpDetectorAprilTag::vpAprilTagFamily, double> tagSizeScales = {
153#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
154 ,
159#endif
160 };
161
162 std::vector<vpDetectorAprilTag::vpPoseEstimationMethod> poseMethods = {
165#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
170#endif
171 };
172 std::map<vpDetectorAprilTag::vpPoseEstimationMethod, std::string> methodNames = {
173 {vpDetectorAprilTag::HOMOGRAPHY, "HOMOGRAPHY"},
174 {vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS, "HOMOGRAPHY_VIRTUAL_VS"},
175 {vpDetectorAprilTag::HOMOGRAPHY_ORTHOGONAL_ITERATION, "HOMOGRAPHY_ORTHOGONAL_ITERATION"},
176#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
177 {vpDetectorAprilTag::DEMENTHON_VIRTUAL_VS, "DEMENTHON_VIRTUAL_VS"},
178 {vpDetectorAprilTag::LAGRANGE_VIRTUAL_VS, "LAGRANGE_VIRTUAL_VS"},
179 {vpDetectorAprilTag::BEST_RESIDUAL_VIRTUAL_VS, "BEST_RESIDUAL_VIRTUAL_VS"}
180#endif
181 };
182
183 const size_t nbTags = 5;
184 const double tagSize_ = 0.25;
185 std::map<int, double> tagsSize = {
186 {0, tagSize_}, {1, tagSize_}, {2, tagSize_}, {3, tagSize_ / 2}, {4, tagSize_ / 2},
187 };
188
189 std::map<int, double> errorTranslationThresh = {
190 {0, 0.025}, {1, 0.09}, {2, 0.05}, {3, 0.13}, {4, 0.09},
191 };
192 std::map<int, double> errorRotationThresh = {
193 {0, 0.04}, {1, 0.075}, {2, 0.07}, {3, 0.18}, {4, 0.13},
194 };
195 std::vector<FailedTestCase> ignoreTests = {
199
201 cam.initPersProjWithoutDistortion(700.0, 700.0, 320.0, 240.0);
202
203 std::map<int, vpHomogeneousMatrix> groundTruthPoses;
204 for (size_t i = 0; i < nbTags; i++) {
205 std::string filename =
206 vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), std::string("AprilTag/benchmark/640x480/cMo_") +
207 std::to_string(i) + std::string(".txt"));
208 std::ifstream file(filename);
209 groundTruthPoses[static_cast<int>(i)].load(file);
210 }
211
212 for (const auto &kv : apriltagMap) {
213 auto family = kv.first;
214 std::cout << "\nApriltag family: " << family << std::endl;
215 std::string filename =
217 std::string("AprilTag/benchmark/640x480/") + kv.second + std::string("_640x480.png"));
218 const double tagSize = tagSize_ * tagSizeScales[family];
219 REQUIRE(vpIoTools::checkFilename(filename));
220
222 vpImageIo::read(I, filename);
223 REQUIRE(I.getSize() == 640 * 480);
224
225 vpDetectorAprilTag apriltag_detector(family);
226
227 for (auto method : poseMethods) {
228 std::cout << "\tPose estimation method: " << method << std::endl;
229 apriltag_detector.setAprilTagPoseEstimationMethod(method);
230
231 // Same tags size
232 {
233 std::vector<vpHomogeneousMatrix> cMo_vec;
234 apriltag_detector.detect(I, tagSize, cam, cMo_vec);
235 CHECK(cMo_vec.size() == nbTags);
236
237 std::vector<std::vector<vpImagePoint> > tagsCorners = apriltag_detector.getPolygon();
238 CHECK(tagsCorners.size() == nbTags);
239
240 std::vector<std::string> messages = apriltag_detector.getMessage();
241 CHECK(messages.size() == nbTags);
242
243 std::vector<int> tagsId = apriltag_detector.getTagsId();
244 CHECK(tagsId.size() == nbTags);
245 std::map<int, int> idsCount;
246 for (size_t i = 0; i < tagsId.size(); i++) {
247 idsCount[tagsId[i]]++;
248 CHECK((tagsId[i] >= 0 && tagsId[i] < 5));
249 }
250 CHECK(idsCount.size() == nbTags);
251
252 for (size_t i = 0; i < cMo_vec.size(); i++) {
253 const vpHomogeneousMatrix &cMo = cMo_vec[i];
254 const vpPoseVector pose(cMo);
255 int id = tagsId[i];
256 if (id >= 3) {
257 continue;
258 }
259 std::cout << "\t\tSame size, Tag: " << i << std::endl;
260 const vpHomogeneousMatrix &cMo_truth = groundTruthPoses[id];
261 const vpPoseVector pose_truth(cMo_truth);
262
263 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
264 vpColVector error_thetau = vpColVector(pose.getThetaUVector()) - vpColVector(pose_truth.getThetaUVector());
265 double error_trans = sqrt(error_translation.sumSquare() / 3);
266 double error_orientation = sqrt(error_thetau.sumSquare() / 3);
267 std::cout << "\t\t\tTranslation error: " << error_trans << " / Rotation error: " << error_orientation
268 << std::endl;
269 CHECK((error_trans < errorTranslationThresh[id] && error_orientation < errorRotationThresh[id]));
270 }
271 }
272
273 // Custom tags size
274 {
275 apriltag_detector.detect(I);
276
277 std::vector<std::vector<vpImagePoint> > tagsCorners = apriltag_detector.getPolygon();
278 CHECK(tagsCorners.size() == nbTags);
279
280 std::vector<std::string> messages = apriltag_detector.getMessage();
281 CHECK(messages.size() == nbTags);
282
283 std::vector<int> tagsId = apriltag_detector.getTagsId();
284 CHECK(tagsId.size() == nbTags);
285 std::map<int, int> idsCount;
286 for (size_t i = 0; i < tagsId.size(); i++) {
287 idsCount[tagsId[i]]++;
288 CHECK((tagsId[i] >= 0 && tagsId[i] < 5));
289 }
290 CHECK(idsCount.size() == nbTags);
291
292 for (size_t idx = 0; idx < tagsId.size(); idx++) {
293 std::cout << "\t\tCustom size, Tag: " << idx << std::endl;
294 const int id = tagsId[idx];
296 apriltag_detector.getPose(idx, tagsSize[id] * tagSizeScales[family], cam, cMo);
297
298 const vpPoseVector pose(cMo);
299 const vpHomogeneousMatrix &cMo_truth = groundTruthPoses[id];
300 const vpPoseVector pose_truth(cMo_truth);
301
302 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
303 vpColVector error_thetau = vpColVector(pose.getThetaUVector()) - vpColVector(pose_truth.getThetaUVector());
304 double error_trans = sqrt(error_translation.sumSquare() / 3);
305 double error_orientation = sqrt(error_thetau.sumSquare() / 3);
306 std::cout << "\t\t\tTranslation error: " << error_trans << " / Rotation error: " << error_orientation
307 << std::endl;
308 if (std::find(ignoreTests.begin(), ignoreTests.end(),
309 FailedTestCase(family, method, static_cast<int>(idx))) == ignoreTests.end()) {
310 CHECK((error_trans < errorTranslationThresh[id] && error_orientation < errorRotationThresh[id]));
311 }
312 }
313 }
314
315 // Custom tags size + aligned Z-axis
316 {
317 apriltag_detector.detect(I);
318
319 std::vector<std::vector<vpImagePoint> > tagsCorners = apriltag_detector.getPolygon();
320 CHECK(tagsCorners.size() == nbTags);
321
322 std::vector<std::string> messages = apriltag_detector.getMessage();
323 CHECK(messages.size() == nbTags);
324
325 std::vector<int> tagsId = apriltag_detector.getTagsId();
326 CHECK(tagsId.size() == nbTags);
327 std::map<int, int> idsCount;
328 for (size_t i = 0; i < tagsId.size(); i++) {
329 idsCount[tagsId[i]]++;
330 CHECK((tagsId[i] >= 0 && tagsId[i] < 5));
331 }
332 CHECK(idsCount.size() == nbTags);
333
334 for (size_t idx = 0; idx < tagsId.size(); idx++) {
335 std::cout << "\t\tCustom size + aligned Z-axis, Tag: " << idx << std::endl;
336 const int id = tagsId[idx];
338 apriltag_detector.setZAlignedWithCameraAxis(true);
339 apriltag_detector.getPose(idx, tagsSize[id] * tagSizeScales[family], cam, cMo);
340 apriltag_detector.setZAlignedWithCameraAxis(false);
341
342 const vpPoseVector pose(cMo);
343 const vpHomogeneousMatrix oMo2(vpTranslationVector(), vpThetaUVector(M_PI, 0, 0));
344 const vpHomogeneousMatrix cMo_truth = groundTruthPoses[id] * oMo2;
345 const vpPoseVector pose_truth(cMo_truth);
346
347 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
348 vpColVector error_thetau = vpColVector(pose.getThetaUVector()) - vpColVector(pose_truth.getThetaUVector());
349 double error_trans = sqrt(error_translation.sumSquare() / 3);
350 double error_orientation = sqrt(error_thetau.sumSquare() / 3);
351 std::cout << "\t\t\tTranslation error: " << error_trans << " / Rotation error: " << error_orientation
352 << std::endl;
353 if (std::find(ignoreTests.begin(), ignoreTests.end(),
354 FailedTestCase(family, method, static_cast<int>(idx))) == ignoreTests.end()) {
355 CHECK((error_trans < errorTranslationThresh[id] && error_orientation < errorRotationThresh[id]));
356 }
357 }
358 }
359 }
360 }
361}
362
363TEST_CASE("Apriltag corners accuracy test", "[apriltag_corners_accuracy_test]")
364{
365 std::map<vpDetectorAprilTag::vpAprilTagFamily, std::string> apriltagMap = {
366 {vpDetectorAprilTag::TAG_16h5, "tag16_05"},
367 {vpDetectorAprilTag::TAG_25h9, "tag25_09"},
368 {vpDetectorAprilTag::TAG_36h11, "tag36_11"},
370#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
371 ,
376#endif
377 };
378
379 const size_t nbTags = 5;
380 std::map<vpDetectorAprilTag::vpAprilTagFamily, std::map<int, std::vector<vpImagePoint> > > groundTruthCorners;
381 for (const auto &kv : apriltagMap) {
382 std::string filename =
384 std::string("AprilTag/benchmark/640x480/corners_") + kv.second + std::string(".txt"));
385 std::ifstream file(filename);
386 REQUIRE(file.is_open());
387
388 int id = 0;
389 double p0x = 0, p0y = 0;
390 double p1x = 0, p1y = 0;
391 double p2x = 0, p2y = 0;
392 double p3x = 0, p3y = 0;
393 while (file >> id >> p0x >> p0y >> p1x >> p1y >> p2x >> p2y >> p3x >> p3y) {
394 groundTruthCorners[kv.first][id].push_back(vpImagePoint(p0y, p0x));
395 groundTruthCorners[kv.first][id].push_back(vpImagePoint(p1y, p1x));
396 groundTruthCorners[kv.first][id].push_back(vpImagePoint(p2y, p2x));
397 groundTruthCorners[kv.first][id].push_back(vpImagePoint(p3y, p3x));
398 REQUIRE(groundTruthCorners[kv.first][id].size() == 4);
399 }
400 }
401
402 for (const auto &kv : apriltagMap) {
403 auto family = kv.first;
404 std::cout << "\nApriltag family: " << family << std::endl;
405 std::string filename =
407 std::string("AprilTag/benchmark/640x480/") + kv.second + std::string("_640x480.png"));
408 REQUIRE(vpIoTools::checkFilename(filename));
409
411 vpImageIo::read(I, filename);
412 REQUIRE(I.getSize() == 640 * 480);
413
414 vpDetectorAprilTag apriltag_detector(family);
415 apriltag_detector.detect(I);
416 std::vector<int> tagsId = apriltag_detector.getTagsId();
417 std::vector<std::vector<vpImagePoint> > tagsCorners = apriltag_detector.getTagsCorners();
418
419 REQUIRE(tagsCorners.size() == nbTags);
420 REQUIRE(tagsId.size() == nbTags);
421 for (size_t i = 0; i < tagsCorners.size(); i++) {
422 const int tagId = tagsId[i];
423 REQUIRE(tagsCorners[i].size() == 4);
424
425 TagGroundTruth corners_ref("", groundTruthCorners[family][tagId]);
426 TagGroundTruth corners_cur("", tagsCorners[i]);
427 CHECK((corners_ref == corners_cur));
428
429 std::cout << "\tid: " << tagId << " - RMSE: " << corners_ref.rmse(corners_cur.m_corners) << std::endl;
430 }
431 }
432}
433
434#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
435TEST_CASE("Apriltag regression test", "[apriltag_regression_test]")
436{
437#if (VISP_HAVE_DATASET_VERSION >= 0x030600)
438 const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "AprilTag/AprilTag.png");
439#else
440 const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "AprilTag/AprilTag.pgm");
441#endif
442 REQUIRE(vpIoTools::checkFilename(filename));
443
445 vpImageIo::read(I, filename);
446 REQUIRE(I.getSize() == 640 * 480);
447
450 const double tagSize = 0.053;
451 const float quad_decimate = 1.0;
452 vpDetectorBase *detector = new vpDetectorAprilTag(tagFamily);
453 dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagQuadDecimate(quad_decimate);
454 dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagPoseEstimationMethod(poseEstimationMethod);
455
457 cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779);
458
459 std::vector<vpHomogeneousMatrix> cMo_vec;
460 dynamic_cast<vpDetectorAprilTag *>(detector)->detect(I, tagSize, cam, cMo_vec);
461
462 // Ground truth
463 std::map<std::string, TagGroundTruth> mapOfTagsGroundTruth;
464 {
465 std::string filename_ground_truth =
466 vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "AprilTag/ground_truth_detection.txt");
467 std::ifstream file_ground_truth(filename_ground_truth.c_str());
468 REQUIRE(file_ground_truth.is_open());
469 std::string message = "";
470 double v1 = 0.0, v2 = 0.0, v3 = 0.0, v4 = 0.0;
471 double u1 = 0.0, u2 = 0.0, u3 = 0.0, u4 = 0.0;
472 while (file_ground_truth >> message >> v1 >> u1 >> v2 >> u2 >> v3 >> u3 >> v4 >> u4) {
473 std::vector<vpImagePoint> tagCorners(4);
474 tagCorners[0].set_ij(v1, u1);
475 tagCorners[1].set_ij(v2, u2);
476 tagCorners[2].set_ij(v3, u3);
477 tagCorners[3].set_ij(v4, u4);
478 mapOfTagsGroundTruth.insert(std::make_pair(message, TagGroundTruth(message, tagCorners)));
479 }
480 }
481
482 std::map<std::string, vpPoseVector> mapOfPosesGroundTruth;
483 {
484 std::string filename_ground_truth =
485 vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "AprilTag/ground_truth_pose.txt");
486 std::ifstream file_ground_truth(filename_ground_truth.c_str());
487 REQUIRE(file_ground_truth.is_open());
488 std::string message = "";
489 double tx = 0.0, ty = 0.0, tz = 0.0;
490 double tux = 0.0, tuy = 0.0, tuz = 0.0;
491 while (file_ground_truth >> message >> tx >> ty >> tz >> tux >> tuy >> tuz) {
492 mapOfPosesGroundTruth.insert(std::make_pair(message, vpPoseVector(tx, ty, tz, tux, tuy, tuz)));
493 }
494 }
495
496 for (size_t i = 0; i < detector->getNbObjects(); i++) {
497 std::vector<vpImagePoint> p = detector->getPolygon(i);
498
499 std::string message = detector->getMessage(i);
500 std::replace(message.begin(), message.end(), ' ', '_');
501 std::map<std::string, TagGroundTruth>::iterator it = mapOfTagsGroundTruth.find(message);
502 TagGroundTruth current(message, p);
503 if (it == mapOfTagsGroundTruth.end()) {
504 std::cerr << "Problem with tag decoding (tag_family or id): " << message << std::endl;
505 } else if (it->second != current) {
506 std::cerr << "Problem, current detection:\n" << current << "\nReference:\n" << it->second << std::endl;
507 }
508 REQUIRE(it != mapOfTagsGroundTruth.end());
509 CHECK(it->second == current);
510 }
511
512 for (size_t i = 0; i < cMo_vec.size(); i++) {
513 vpPoseVector pose_vec(cMo_vec[i]);
514
515 std::string message = detector->getMessage(i);
516 std::replace(message.begin(), message.end(), ' ', '_');
517 std::map<std::string, vpPoseVector>::iterator it = mapOfPosesGroundTruth.find(message);
518 if (it == mapOfPosesGroundTruth.end()) {
519 std::cerr << "Problem with tag decoding (tag_family or id): " << message << std::endl;
520 }
521 REQUIRE(it != mapOfPosesGroundTruth.end());
522 std::cout << "Tag: " << message << std::endl;
523 std::cout << "\tEstimated pose: " << pose_vec.t() << std::endl;
524 std::cout << "\tReference pose: " << it->second.t() << std::endl;
525 for (unsigned int cpt = 0; cpt < 3; cpt++) {
526 if (!vpMath::equal(it->second[cpt], pose_vec[cpt], 0.005) ||
527 !vpMath::equal(it->second[cpt + 3], pose_vec[cpt + 3], 0.005)) {
528 std::cerr << "Problem, current pose: " << pose_vec.t() << "\nReference pose: " << it->second.t() << std::endl;
529 }
530 CHECK((vpMath::equal(it->second[cpt], pose_vec[cpt], 0.005) &&
531 vpMath::equal(it->second[cpt + 3], pose_vec[cpt + 3], 0.005)));
532 }
533 }
534
535 delete detector;
536}
537
538TEST_CASE("Apriltag copy constructor test", "[apriltag_copy_constructor_test]")
539{
540 const std::string filename =
541 vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "AprilTag/benchmark/640x480/tag21_07_640x480.png");
542 REQUIRE(vpIoTools::checkFilename(filename));
543
545 vpImageIo::read(I, filename);
546 REQUIRE(I.getSize() == 640 * 480);
547
550 const double tagSize = 0.25 * 5 / 9;
551 const float quad_decimate = 1.0;
552 vpDetectorAprilTag *detector = new vpDetectorAprilTag(tagFamily, poseEstimationMethod);
553 detector->setAprilTagQuadDecimate(quad_decimate);
554
556 cam.initPersProjWithoutDistortion(700, 700, 320, 240);
557
558 std::vector<vpHomogeneousMatrix> cMo_vec;
559 detector->detect(I, tagSize, cam, cMo_vec);
560 std::vector<std::vector<vpImagePoint> > tagsCorners = detector->getTagsCorners();
561 std::vector<int> tagsId = detector->getTagsId();
562
563 // Copy
564 vpDetectorAprilTag detector_copy(*detector);
565 // Delete old detector
566 delete detector;
567
568 std::vector<std::vector<vpImagePoint> > tagsCorners_copy = detector_copy.getTagsCorners();
569 std::vector<int> tagsId_copy = detector_copy.getTagsId();
570 REQUIRE(tagsCorners_copy.size() == tagsCorners.size());
571 REQUIRE(tagsId_copy.size() == tagsId.size());
572 REQUIRE(tagsCorners_copy.size() == tagsId_copy.size());
573
574 for (size_t i = 0; i < tagsCorners.size(); i++) {
575 const std::vector<vpImagePoint> &corners_ref = tagsCorners[i];
576 const std::vector<vpImagePoint> &corners_copy = tagsCorners_copy[i];
577 REQUIRE(corners_ref.size() == corners_copy.size());
578
579 for (size_t j = 0; j < corners_ref.size(); j++) {
580 const vpImagePoint &corner_ref = corners_ref[j];
581 const vpImagePoint &corner_copy = corners_copy[j];
582 CHECK(corner_ref == corner_copy);
583 }
584
585 int id_ref = tagsId[i];
586 int id_copy = tagsId_copy[i];
587 CHECK(id_ref == id_copy);
588 }
589
590 std::vector<vpHomogeneousMatrix> cMo_vec_copy;
591 detector_copy.detect(I, tagSize, cam, cMo_vec_copy);
592 REQUIRE(cMo_vec.size() == cMo_vec_copy.size());
593 for (size_t idx = 0; idx < cMo_vec_copy.size(); idx++) {
594 const vpHomogeneousMatrix &cMo = cMo_vec[idx];
595 const vpHomogeneousMatrix &cMo_copy = cMo_vec_copy[idx];
596 for (unsigned int i = 0; i < 3; i++) {
597 for (unsigned int j = 0; j < 4; j++) {
598 CHECK(vpMath::equal(cMo[i][j], cMo_copy[i][j], std::numeric_limits<double>::epsilon()));
599 }
600 }
601 }
602}
603
604TEST_CASE("Apriltag assignment operator test", "[apriltag_assignment_operator_test]")
605{
606 const std::string filename =
607 vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "AprilTag/benchmark/640x480/tag21_07_640x480.png");
608 REQUIRE(vpIoTools::checkFilename(filename));
609
611 vpImageIo::read(I, filename);
612 REQUIRE(I.getSize() == 640 * 480);
613
616 const double tagSize = 0.25 * 5 / 9;
617 const float quad_decimate = 1.0;
618 vpDetectorAprilTag *detector = new vpDetectorAprilTag(tagFamily, poseEstimationMethod);
619 detector->setAprilTagQuadDecimate(quad_decimate);
620
622 cam.initPersProjWithoutDistortion(700, 700, 320, 240);
623
624 std::vector<vpHomogeneousMatrix> cMo_vec;
625 detector->detect(I, tagSize, cam, cMo_vec);
626 std::vector<std::vector<vpImagePoint> > tagsCorners = detector->getTagsCorners();
627 std::vector<int> tagsId = detector->getTagsId();
628
629 // Copy
630 vpDetectorAprilTag detector_copy = *detector;
631 // Delete old detector
632 delete detector;
633
634 std::vector<std::vector<vpImagePoint> > tagsCorners_copy = detector_copy.getTagsCorners();
635 std::vector<int> tagsId_copy = detector_copy.getTagsId();
636 REQUIRE(tagsCorners_copy.size() == tagsCorners.size());
637 REQUIRE(tagsId_copy.size() == tagsId.size());
638 REQUIRE(tagsCorners_copy.size() == tagsId_copy.size());
639
640 for (size_t i = 0; i < tagsCorners.size(); i++) {
641 const std::vector<vpImagePoint> &corners_ref = tagsCorners[i];
642 const std::vector<vpImagePoint> &corners_copy = tagsCorners_copy[i];
643 REQUIRE(corners_ref.size() == corners_copy.size());
644
645 for (size_t j = 0; j < corners_ref.size(); j++) {
646 const vpImagePoint &corner_ref = corners_ref[j];
647 const vpImagePoint &corner_copy = corners_copy[j];
648 CHECK(corner_ref == corner_copy);
649 }
650
651 int id_ref = tagsId[i];
652 int id_copy = tagsId_copy[i];
653 CHECK(id_ref == id_copy);
654 }
655
656 std::vector<vpHomogeneousMatrix> cMo_vec_copy;
657 detector_copy.detect(I, tagSize, cam, cMo_vec_copy);
658 REQUIRE(cMo_vec.size() == cMo_vec_copy.size());
659 for (size_t idx = 0; idx < cMo_vec_copy.size(); idx++) {
660 const vpHomogeneousMatrix &cMo = cMo_vec[idx];
661 const vpHomogeneousMatrix &cMo_copy = cMo_vec_copy[idx];
662 for (unsigned int i = 0; i < 3; i++) {
663 for (unsigned int j = 0; j < 4; j++) {
664 CHECK(vpMath::equal(cMo[i][j], cMo_copy[i][j], std::numeric_limits<double>::epsilon()));
665 }
666 }
667 }
668}
669
670TEST_CASE("Apriltag getTagsPoints3D test", "[apriltag_get_tags_points3D_test]")
671{
672 const std::string filename =
673 vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "AprilTag/benchmark/640x480/tag21_07_640x480.png");
674 REQUIRE(vpIoTools::checkFilename(filename));
675
677 vpImageIo::read(I, filename);
678 REQUIRE(I.getSize() == 640 * 480);
679
682 const double familyScale = 5.0 / 9;
683 const double tagSize = 0.25;
684 std::map<int, double> tagsSize = {
685 {-1, tagSize * familyScale}, {3, tagSize / 2 * familyScale}, {4, tagSize / 2 * familyScale}};
686
687 vpDetectorAprilTag detector(tagFamily, poseEstimationMethod);
688
690 cam.initPersProjWithoutDistortion(700, 700, 320, 240);
691
692 std::vector<vpHomogeneousMatrix> cMo_vec;
693 REQUIRE(detector.detect(I));
694
695 // Compute pose with getPose
696 std::vector<int> tagsId = detector.getTagsId();
697 for (size_t i = 0; i < tagsId.size(); i++) {
698 int id = tagsId[i];
699 double size = tagsSize[-1];
700 if (tagsSize.find(id) != tagsSize.end()) {
701 size = tagsSize[id];
702 }
703
705 detector.getPose(i, size, cam, cMo);
706 cMo_vec.push_back(cMo);
707 }
708
709 // Compute pose manually
710 std::vector<std::vector<vpPoint> > tagsPoints = detector.getTagsPoints3D(tagsId, tagsSize);
711 std::vector<std::vector<vpImagePoint> > tagsCorners = detector.getTagsCorners();
712 REQUIRE(tagsPoints.size() == tagsCorners.size());
713
714 for (size_t i = 0; i < tagsPoints.size(); i++) {
715 REQUIRE(tagsPoints[i].size() == tagsCorners[i].size());
716
717 for (size_t j = 0; j < tagsPoints[i].size(); j++) {
718 vpPoint &pt = tagsPoints[i][j];
719 const vpImagePoint &imPt = tagsCorners[i][j];
720 double x = 0, y = 0;
722 pt.set_x(x);
723 pt.set_y(y);
724 }
725
726 vpPose pose(tagsPoints[i]);
727 vpHomogeneousMatrix cMo_manual;
728 pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo_manual);
729
730 const vpHomogeneousMatrix &cMo = cMo_vec[i];
731 // Note that using epsilon = std::numeric_limits<double>::epsilon() makes this test
732 // failing on Ubuntu 18.04 when none of the Lapack 3rd party libraries, nor the built-in are used.
733 // Admissible espilon value is 1e-14. Using 1e-15 makes the test failing.
734 // Again on Debian i386 where Lapack is enable, using std::numeric_limits<double>::epsilon()
735 // makes this test failing.
736 double epsilon = 1e-12;
737
738 for (unsigned int row = 0; row < cMo.getRows(); row++) {
739 for (unsigned int col = 0; col < cMo.getCols(); col++) {
740 CHECK(vpMath::equal(cMo[row][col], cMo_manual[row][col], epsilon));
741 }
742 }
743 }
744}
745#endif // #if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
746
747int main(int argc, const char *argv[])
748{
749 Catch::Session session; // There must be exactly one instance
750
751 // Let Catch (using Clara) parse the command line
752 session.applyCommandLine(argc, argv);
753
754 int numFailed = session.run();
755
756 // numFailed is clamped to 255 as some unices only use the lower 8 bits.
757 // This clamping has already been applied, so just return it here
758 // You can also do any post run clean-up here
759 return numFailed;
760}
761#else
762int main() { return EXIT_SUCCESS; }
763#endif
unsigned int getCols() const
Definition vpArray2D.h:280
unsigned int getRows() const
Definition vpArray2D.h:290
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
double get_j() const
double get_i() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getSize() const
Definition vpImage.h:223
static std::string getViSPImagesDataPath()
static bool checkFilename(const std::string &filename)
static std::string createFilePath(const std::string &parent, const std::string &child)
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
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:77
void set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
Implementation of a pose vector and operations on poses.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.