Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
testAprilTag.cpp

Test AprilTag detection.

/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2019 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See http://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Test AprilTag detection.
*
*****************************************************************************/
#include <iostream>
#include <map>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/detection/vpDetectorAprilTag.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#if defined(VISP_HAVE_APRILTAG)
// List of allowed command line options
#define GETOPTARGS "cdi:p:C:T:h"
namespace
{
/*
Print the program options.
\param name : Program name.
\param badparam : Bad parameter name.
\param ipath: Input image path.
*/
void usage(const char *name, const char *badparam, std::string ipath)
{
fprintf(stdout, "\n\
Test AprilTag detection.\n\
\n\
SYNOPSIS\n\
%s [-c] [-d] [-i <input image path>] [-p <personal image path>] \
[-C <tag color>] [-T <tag thickness>]\n\
[-h]\n \
", name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read \"AprilTag/AprilTag.pgm image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
\n\
-p <personal image path> \n\
Path to an image used to test image reading function.\n\
Example: -p /my_path_to/image.png\n\
\n\
-c \n\
Disable the mouse click. Useful to automate the \n\
execution of this program without human intervention.\n\
\n\
-d \n\
Turn off the display.\n\
\n\
-C <color (0, 1, ...)> \n\
Color for tag detection display.\n\
\n\
-T <thickness> \n\
Thickness for tag detection display.\n\
\n\
-h\n\
Print the help.\n\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, bool &click_allowed, bool &display,
int &color_id, unsigned int &thickness)
{
const char *optarg_;
int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
case 'i':
ipath = optarg_;
break;
case 'p':
ppath = optarg_;
break;
case 'h':
usage(argv[0], NULL, ipath);
return false;
break;
case 'c':
click_allowed = false;
break;
case 'd':
display = false;
break;
case 'C':
color_id = atoi(optarg_);
break;
case 'T':
thickness = (unsigned int) atoi(optarg_);
break;
default:
usage(argv[0], optarg_, ipath);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], NULL, ipath);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
return true;
}
struct TagGroundTruth {
std::string message;
std::vector<vpImagePoint> corners;
TagGroundTruth(const std::string &msg, const std::vector<vpImagePoint> &c) : message(msg), corners(c) {}
bool operator==(const TagGroundTruth &b) const
{
if (message != b.message || corners.size() != b.corners.size())
return false;
for (size_t i = 0; i < corners.size(); i++) {
// Allow 0.5 pixel of difference
if (!vpMath::equal(corners[i].get_u(), b.corners[i].get_u(), 0.5) ||
!vpMath::equal(corners[i].get_v(), b.corners[i].get_v(), 0.5)) {
return false;
}
}
return true;
}
bool operator!=(const TagGroundTruth &b) const { return !(*this == b); }
};
std::ostream &operator<<(std::ostream &os, TagGroundTruth &t)
{
os << t.message << std::endl;
for (size_t i = 0; i < t.corners.size(); i++)
os << t.corners[i] << std::endl;
return os;
}
}
int main(int argc, const char *argv[])
{
try {
std::string env_ipath;
std::string opt_ipath = "";
std::string opt_ppath = "";
std::string ipath = "";
std::string filename = "";
bool opt_click_allowed = true;
bool opt_display = true;
int opt_color_id = -1;
unsigned int opt_thickness = 2;
// Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
// environment variable value
// Set the default input path
if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed, opt_display,
opt_color_id, opt_thickness) == false) {
exit(EXIT_FAILURE);
}
// Get the option values
if (!opt_ipath.empty())
ipath = opt_ipath;
// Compare ipath and env_ipath. If they differ, we take into account
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
}
}
//
// Here starts really the test
//
if (opt_ppath.empty()) {
filename = vpIoTools::createFilePath(ipath, "AprilTag/AprilTag.pgm");
} else {
filename = opt_ppath;
}
if (!vpIoTools::checkFilename(filename)) {
std::cerr << "Filename: " << filename << " does not exist." << std::endl;
return EXIT_SUCCESS;
}
vpImageIo::read(I, filename);
#ifdef VISP_HAVE_X11
#elif defined(VISP_HAVE_GDI)
#elif defined(VISP_HAVE_OPENCV)
#else
opt_display = false;
#endif
double tagSize = 0.053;
float quad_decimate = 1.0;
int nThreads = 1;
bool display_tag = true;
vpDetectorBase *detector = new vpDetectorAprilTag(tagFamily);
dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagQuadDecimate(quad_decimate);
dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagPoseEstimationMethod(poseEstimationMethod);
dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagNbThreads(nThreads);
dynamic_cast<vpDetectorAprilTag *>(detector)->setDisplayTag(display_tag,
opt_color_id < 0 ? vpColor::none : vpColor::getColor(opt_color_id),
opt_thickness);
cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779);
if (opt_display) {
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
d.init(I, 0, 0, "AprilTag detection");
#endif
}
std::vector<vpHomogeneousMatrix> cMo_vec;
dynamic_cast<vpDetectorAprilTag *>(detector)->detect(I, tagSize, cam, cMo_vec);
// Ground truth
std::map<std::string, TagGroundTruth> mapOfTagsGroundTruth;
bool use_detection_ground_truth = false;
{
std::string filename_ground_truth = vpIoTools::createFilePath(ipath, "AprilTag/ground_truth_detection.txt");
std::ifstream file_ground_truth(filename_ground_truth.c_str());
if (file_ground_truth.is_open() && opt_ppath.empty()) {
use_detection_ground_truth = true;
std::string message = "";
double v1 = 0.0, v2 = 0.0, v3 = 0.0, v4 = 0.0;
double u1 = 0.0, u2 = 0.0, u3 = 0.0, u4 = 0.0;
while (file_ground_truth >> message >> v1 >> u1 >> v2 >> u2 >> v3 >> u3 >> v4 >> u4) {
std::vector<vpImagePoint> tagCorners(4);
tagCorners[0].set_ij(v1, u1);
tagCorners[1].set_ij(v2, u2);
tagCorners[2].set_ij(v3, u3);
tagCorners[3].set_ij(v4, u4);
mapOfTagsGroundTruth.insert(std::make_pair(message, TagGroundTruth(message, tagCorners)));
}
}
}
std::map<std::string, vpPoseVector> mapOfPosesGroundTruth;
bool use_pose_ground_truth = false;
{
std::string filename_ground_truth = vpIoTools::createFilePath(ipath, "AprilTag/ground_truth_pose.txt");
std::ifstream file_ground_truth(filename_ground_truth.c_str());
if (file_ground_truth.is_open() && opt_ppath.empty()) {
use_pose_ground_truth = true;
std::string message = "";
double tx = 0.0, ty = 0.0, tz = 0.0;
double tux = 0.0, tuy = 0.0, tuz = 0.0;
while (file_ground_truth >> message >> tx >> ty >> tz >> tux >> tuy >> tuz) {
mapOfPosesGroundTruth.insert(std::make_pair(message, vpPoseVector(tx, ty, tz, tux, tuy, tuz)));
}
}
}
std::cout << "use_pose_ground_truth: " << use_pose_ground_truth << std::endl;
for (size_t i = 0; i < detector->getNbObjects(); i++) {
std::vector<vpImagePoint> p = detector->getPolygon(i);
if (use_detection_ground_truth) {
std::string message = detector->getMessage(i);
std::replace(message.begin(), message.end(), ' ', '_');
std::map<std::string, TagGroundTruth>::iterator it = mapOfTagsGroundTruth.find(message);
TagGroundTruth current(message, p);
if (it == mapOfTagsGroundTruth.end()) {
std::cerr << "Problem with tag decoding (tag_family or id): " << message << std::endl;
return EXIT_FAILURE;
} else if (it->second != current) {
std::cerr << "Problem, current detection:\n" << current << "\nGround truth:\n" << it->second << std::endl;
return EXIT_FAILURE;
}
}
if (opt_display) {
vpRect bbox = detector->getBBox(i);
vpDisplay::displayText(I, (int)(bbox.getTop() - 10), (int)bbox.getLeft(), detector->getMessage(i),
}
}
if (opt_display) {
vpDisplay::displayText(I, 20, 20, "Click to display tag poses", vpColor::red);
if (opt_click_allowed)
}
for (size_t i = 0; i < cMo_vec.size(); i++) {
if (opt_display)
vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
if (use_pose_ground_truth) {
vpPoseVector pose_vec(cMo_vec[i]);
std::string message = detector->getMessage(i);
std::replace(message.begin(), message.end(), ' ', '_');
std::map<std::string, vpPoseVector>::iterator it = mapOfPosesGroundTruth.find(message);
if (it == mapOfPosesGroundTruth.end()) {
std::cerr << "Problem with tag decoding (tag_family or id): " << message << std::endl;
return EXIT_FAILURE;
} else {
for (unsigned int cpt = 0; cpt < 6; cpt++) {
if (!vpMath::equal(it->second[cpt], pose_vec[cpt], 0.005)) {
std::cerr << "Problem, current pose: " << pose_vec.t() << "\nGround truth pose: " << it->second.t()
<< std::endl;
return EXIT_FAILURE;
}
}
}
}
}
if (opt_display) {
vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
if (opt_click_allowed)
}
delete detector;
} catch (const vpException &e) {
std::cerr << "Catch an exception: " << e.what() << std::endl;
return EXIT_FAILURE;
}
std::cout << "\ntestAprilTag is ok." << std::endl;
return EXIT_SUCCESS;
}
#else
int main()
{
std::cout << "Need ViSP AprilTag." << std::endl;
return 0;
}
#endif