41#include <visp3/core/vpConfig.h>
43#if defined(VISP_HAVE_CATCH2) && defined(VISP_HAVE_APRILTAG) && (VISP_HAVE_DATASET_VERSION >= 0x030300)
44#define CATCH_CONFIG_RUNNER
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>
57struct 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++) {
96 return sqrt(error / (2 * m_corners.size()));
100#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
101std::ostream &operator<<(std::ostream &os, TagGroundTruth &t)
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;
112struct FailedTestCase {
119 : 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); }
132TEST_CASE(
"Apriltag pose estimation test",
"[apriltag_pose_estimation_test]")
134 std::map<vpDetectorAprilTag::vpAprilTagFamily, std::string> apriltagMap = {
139#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
148 std::map<vpDetectorAprilTag::vpAprilTagFamily, double> tagSizeScales = {
153#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
162 std::vector<vpDetectorAprilTag::vpPoseEstimationMethod> poseMethods = {
165#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
172 std::map<vpDetectorAprilTag::vpPoseEstimationMethod, std::string> methodNames = {
176#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
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},
189 std::map<int, double> errorTranslationThresh = {
190 {0, 0.025}, {1, 0.09}, {2, 0.05}, {3, 0.13}, {4, 0.09},
192 std::map<int, double> errorRotationThresh = {
193 {0, 0.04}, {1, 0.075}, {2, 0.07}, {3, 0.18}, {4, 0.13},
195 std::vector<FailedTestCase> ignoreTests = {
203 std::map<int, vpHomogeneousMatrix> groundTruthPoses;
204 for (
size_t i = 0; i < nbTags; i++) {
205 std::string filename =
207 std::to_string(i) + std::string(
".txt"));
208 std::ifstream file(filename);
209 groundTruthPoses[
static_cast<int>(i)].load(file);
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];
223 REQUIRE(I.
getSize() == 640 * 480);
227 for (
auto method : poseMethods) {
228 std::cout <<
"\tPose estimation method: " << method << std::endl;
229 apriltag_detector.setAprilTagPoseEstimationMethod(method);
233 std::vector<vpHomogeneousMatrix> cMo_vec;
234 apriltag_detector.detect(I, tagSize, cam, cMo_vec);
235 CHECK(cMo_vec.size() == nbTags);
237 std::vector<std::vector<vpImagePoint> > tagsCorners = apriltag_detector.getPolygon();
238 CHECK(tagsCorners.size() == nbTags);
240 std::vector<std::string> messages = apriltag_detector.getMessage();
241 CHECK(messages.size() == nbTags);
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));
250 CHECK(idsCount.size() == nbTags);
252 for (
size_t i = 0; i < cMo_vec.size(); i++) {
259 std::cout <<
"\t\tSame size, Tag: " << i << std::endl;
263 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
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
269 CHECK((error_trans < errorTranslationThresh[
id] && error_orientation < errorRotationThresh[
id]));
275 apriltag_detector.detect(I);
277 std::vector<std::vector<vpImagePoint> > tagsCorners = apriltag_detector.getPolygon();
278 CHECK(tagsCorners.size() == nbTags);
280 std::vector<std::string> messages = apriltag_detector.getMessage();
281 CHECK(messages.size() == nbTags);
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));
290 CHECK(idsCount.size() == nbTags);
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);
302 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
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
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]));
317 apriltag_detector.detect(I);
319 std::vector<std::vector<vpImagePoint> > tagsCorners = apriltag_detector.getPolygon();
320 CHECK(tagsCorners.size() == nbTags);
322 std::vector<std::string> messages = apriltag_detector.getMessage();
323 CHECK(messages.size() == nbTags);
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));
332 CHECK(idsCount.size() == nbTags);
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);
347 vpColVector error_translation = pose.getTranslationVector() - pose_truth.getTranslationVector();
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
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]));
363TEST_CASE(
"Apriltag corners accuracy test",
"[apriltag_corners_accuracy_test]")
365 std::map<vpDetectorAprilTag::vpAprilTagFamily, std::string> apriltagMap = {
370#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
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());
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);
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"));
412 REQUIRE(I.
getSize() == 640 * 480);
415 apriltag_detector.detect(I);
416 std::vector<int> tagsId = apriltag_detector.getTagsId();
417 std::vector<std::vector<vpImagePoint> > tagsCorners = apriltag_detector.getTagsCorners();
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);
425 TagGroundTruth corners_ref(
"", groundTruthCorners[family][tagId]);
426 TagGroundTruth corners_cur(
"", tagsCorners[i]);
427 CHECK((corners_ref == corners_cur));
429 std::cout <<
"\tid: " << tagId <<
" - RMSE: " << corners_ref.rmse(corners_cur.m_corners) << std::endl;
434#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_EIGEN3)
435TEST_CASE(
"Apriltag regression test",
"[apriltag_regression_test]")
437#if (VISP_HAVE_DATASET_VERSION >= 0x030600)
446 REQUIRE(I.
getSize() == 640 * 480);
450 const double tagSize = 0.053;
451 const float quad_decimate = 1.0;
454 dynamic_cast<vpDetectorAprilTag *
>(detector)->setAprilTagPoseEstimationMethod(poseEstimationMethod);
459 std::vector<vpHomogeneousMatrix> cMo_vec;
463 std::map<std::string, TagGroundTruth> mapOfTagsGroundTruth;
465 std::string filename_ground_truth =
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)));
482 std::map<std::string, vpPoseVector> mapOfPosesGroundTruth;
484 std::string filename_ground_truth =
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)));
497 std::vector<vpImagePoint> p = detector->
getPolygon(i);
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;
508 REQUIRE(it != mapOfTagsGroundTruth.end());
509 CHECK(it->second == current);
512 for (
size_t i = 0; i < cMo_vec.size(); i++) {
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;
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++) {
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;
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)));
538TEST_CASE(
"Apriltag copy constructor test",
"[apriltag_copy_constructor_test]")
540 const std::string filename =
546 REQUIRE(I.
getSize() == 640 * 480);
550 const double tagSize = 0.25 * 5 / 9;
551 const float quad_decimate = 1.0;
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();
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());
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());
579 for (
size_t j = 0; j < corners_ref.size(); j++) {
582 CHECK(corner_ref == corner_copy);
585 int id_ref = tagsId[i];
586 int id_copy = tagsId_copy[i];
587 CHECK(id_ref == id_copy);
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++) {
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()));
604TEST_CASE(
"Apriltag assignment operator test",
"[apriltag_assignment_operator_test]")
606 const std::string filename =
612 REQUIRE(I.
getSize() == 640 * 480);
616 const double tagSize = 0.25 * 5 / 9;
617 const float quad_decimate = 1.0;
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();
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());
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());
645 for (
size_t j = 0; j < corners_ref.size(); j++) {
648 CHECK(corner_ref == corner_copy);
651 int id_ref = tagsId[i];
652 int id_copy = tagsId_copy[i];
653 CHECK(id_ref == id_copy);
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++) {
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()));
670TEST_CASE(
"Apriltag getTagsPoints3D test",
"[apriltag_get_tags_points3D_test]")
672 const std::string filename =
678 REQUIRE(I.
getSize() == 640 * 480);
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}};
692 std::vector<vpHomogeneousMatrix> cMo_vec;
693 REQUIRE(detector.
detect(I));
696 std::vector<int> tagsId = detector.
getTagsId();
697 for (
size_t i = 0; i < tagsId.size(); i++) {
699 double size = tagsSize[-1];
700 if (tagsSize.find(
id) != tagsSize.end()) {
705 detector.
getPose(i, size, cam, cMo);
706 cMo_vec.push_back(cMo);
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());
714 for (
size_t i = 0; i < tagsPoints.size(); i++) {
715 REQUIRE(tagsPoints[i].size() == tagsCorners[i].size());
717 for (
size_t j = 0; j < tagsPoints[i].size(); j++) {
718 vpPoint &pt = tagsPoints[i][j];
726 vpPose pose(tagsPoints[i]);
736 double epsilon = 1e-12;
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));
747int main(
int argc,
const char *argv[])
749 Catch::Session session;
752 session.applyCommandLine(argc, argv);
754 int numFailed = session.run();
762int main() {
return EXIT_SUCCESS; }
unsigned int getCols() const
unsigned int getRows() const
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
@ BEST_RESIDUAL_VIRTUAL_VS
@ HOMOGRAPHY_ORTHOGONAL_ITERATION
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 ...
Definition of the vpImage class member functions.
unsigned int getSize() const
static bool equal(double x, double y, double threshold=0.001)
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 ...
void set_x(double x)
Set the point x coordinate in the image plane.
void set_y(double y)
Set the point y coordinate in the image plane.
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...
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.