43 #include <visp3/core/vpDisplay.h> 44 #include <visp3/core/vpIoTools.h> 45 #include <visp3/detection/vpDetectorAprilTag.h> 46 #include <visp3/gui/vpDisplayGDI.h> 47 #include <visp3/gui/vpDisplayOpenCV.h> 48 #include <visp3/gui/vpDisplayX.h> 49 #include <visp3/io/vpImageIo.h> 50 #include <visp3/io/vpParseArgv.h> 52 #if defined(VISP_HAVE_APRILTAG) 55 #define GETOPTARGS "cdi:p:h" 66 void usage(
const char *name,
const char *badparam, std::string ipath)
69 Test AprilTag detection.\n\ 72 %s [-c] [-d] [-i <input image path>] [-p <personal image path>]\n\ 78 -i <input image path> %s\n\ 79 Set image input path.\n\ 80 From this path read \"AprilTag/AprilTag.pgm image.\n\ 81 Setting the VISP_INPUT_IMAGE_PATH environment\n\ 82 variable produces the same behaviour than using\n\ 85 -p <personal image path> \n\ 86 Path to an image used to test image reading function.\n\ 87 Example: -p /my_path_to/image.png\n\ 90 Disable the mouse click. Useful to automaze the \n\ 91 execution of this program without human intervention.\n\ 94 Turn off the display.\n\ 97 Print the help.\n\n", ipath.c_str());
100 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
114 bool getOptions(
int argc,
const char **argv, std::string &ipath, std::string &ppath,
bool &click_allowed,
bool &display)
128 usage(argv[0], NULL, ipath);
132 click_allowed =
false;
139 usage(argv[0], optarg_, ipath);
145 if ((c == 1) || (c == -1)) {
147 usage(argv[0], NULL, ipath);
148 std::cerr <<
"ERROR: " << std::endl;
149 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
156 struct TagGroundTruth {
158 std::vector<vpImagePoint> corners;
160 TagGroundTruth(
const std::string &msg,
const std::vector<vpImagePoint> &c) : message(msg), corners(c) {}
162 bool operator==(
const TagGroundTruth &b)
const 164 if (message != b.message || corners.size() != b.corners.size())
167 for (
size_t i = 0; i < corners.size(); i++) {
169 if (!
vpMath::equal(corners[i].get_u(), b.corners[i].get_u(), 0.5) ||
170 !
vpMath::equal(corners[i].get_v(), b.corners[i].get_v(), 0.5)) {
178 bool operator!=(
const TagGroundTruth &b)
const {
return !(*
this == b); }
181 std::ostream &operator<<(std::ostream &os, TagGroundTruth &t)
183 os << t.message << std::endl;
184 for (
size_t i = 0; i < t.corners.size(); i++)
185 os << t.corners[i] << std::endl;
191 int main(
int argc,
const char *argv[])
194 std::string env_ipath;
195 std::string opt_ipath;
196 std::string opt_ppath;
198 std::string filename;
199 bool opt_click_allowed =
true;
200 bool opt_display =
true;
207 if (!env_ipath.empty())
211 if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed, opt_display) ==
false) {
216 if (!opt_ipath.empty())
221 if (!opt_ipath.empty() && !env_ipath.empty()) {
222 if (ipath != env_ipath) {
223 std::cout << std::endl <<
"WARNING: " << std::endl;
224 std::cout <<
" Since -i <visp image path=" << ipath <<
"> " 225 <<
" is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
226 <<
" we skip the environment variable." << std::endl;
235 if (opt_ppath.empty()) {
238 filename = opt_ppath;
242 std::cerr <<
"Filename: " << filename <<
" does not exist." << std::endl;
249 #elif defined(VISP_HAVE_GDI) 251 #elif defined(VISP_HAVE_OPENCV) 259 double tagSize = 0.053;
260 float quad_decimate = 1.0;
262 bool display_tag =
true;
266 dynamic_cast<vpDetectorAprilTag *
>(detector)->setAprilTagPoseEstimationMethod(poseEstimationMethod);
274 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) 275 d.
init(I, 0, 0,
"AprilTag detection");
280 std::vector<vpHomogeneousMatrix> cMo_vec;
284 std::map<std::string, TagGroundTruth> mapOfTagsGroundTruth;
285 bool use_detection_ground_truth =
false;
288 std::ifstream file_ground_truth(filename_ground_truth.c_str());
289 if (file_ground_truth.is_open() && opt_ppath.empty()) {
290 use_detection_ground_truth =
true;
292 std::string message =
"";
293 double v1 = 0.0, v2 = 0.0, v3 = 0.0, v4 = 0.0;
294 double u1 = 0.0, u2 = 0.0, u3 = 0.0, u4 = 0.0;
295 while (file_ground_truth >> message >> v1 >> u1 >> v2 >> u2 >> v3 >> u3 >> v4 >> u4) {
296 std::vector<vpImagePoint> tagCorners(4);
297 tagCorners[0].set_ij(v1, u1);
298 tagCorners[1].set_ij(v2, u2);
299 tagCorners[2].set_ij(v3, u3);
300 tagCorners[3].set_ij(v4, u4);
301 mapOfTagsGroundTruth.insert(std::make_pair(message, TagGroundTruth(message, tagCorners)));
306 std::map<std::string, vpPoseVector> mapOfPosesGroundTruth;
307 bool use_pose_ground_truth =
false;
310 std::ifstream file_ground_truth(filename_ground_truth.c_str());
311 if (file_ground_truth.is_open() && opt_ppath.empty()) {
312 use_pose_ground_truth =
true;
314 std::string message =
"";
315 double tx = 0.0, ty = 0.0, tz = 0.0;
316 double tux = 0.0, tuy = 0.0, tuz = 0.0;
317 while (file_ground_truth >> message >> tx >> ty >> tz >> tux >> tuy >> tuz) {
318 mapOfPosesGroundTruth.insert(std::make_pair(message,
vpPoseVector(tx, ty, tz, tux, tuy, tuz)));
324 std::vector<vpImagePoint> p = detector->
getPolygon(i);
326 if (use_detection_ground_truth) {
327 std::string message = detector->
getMessage(i);
328 std::replace(message.begin(), message.end(),
' ',
'_');
329 std::map<std::string, TagGroundTruth>::iterator it = mapOfTagsGroundTruth.find(message);
330 TagGroundTruth current(message, p);
331 if (it == mapOfTagsGroundTruth.end()) {
332 std::cerr <<
"Problem with tag decoding (tag_family or id): " << message << std::endl;
334 }
else if (it->second != current) {
335 std::cerr <<
"Problem, current detection:\n" << current <<
"\nGround truth:\n" << it->second << std::endl;
351 if (opt_click_allowed)
357 for (
size_t i = 0; i < cMo_vec.size(); i++) {
361 if (use_pose_ground_truth) {
364 std::string message = detector->
getMessage(i);
365 std::replace(message.begin(), message.end(),
' ',
'_');
366 std::map<std::string, vpPoseVector>::iterator it = mapOfPosesGroundTruth.find(message);
367 if (it == mapOfPosesGroundTruth.end()) {
368 std::cerr <<
"Problem with tag decoding (tag_family or id): " << message << std::endl;
371 for (
unsigned int cpt = 0; cpt < 6; cpt++) {
373 std::cerr <<
"Problem, current pose: " << pose_vec.t() <<
"\nGround truth pose: " << it->second.t()
385 if (opt_click_allowed)
391 std::cerr <<
"Catch an exception: " << e.
what() << std::endl;
395 std::cout <<
"\ntestAprilTag is ok." << std::endl;
401 std::cout <<
"Need ViSP AprilTag." << std::endl;
vpRect getBBox(size_t i) const
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Display for windows using GDI (available on any windows 32 platform).
static bool equal(double x, double y, double s=0.001)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
static const vpColor none
error that can be emited by ViSP classes.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
size_t getNbObjects() const
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
static void displayRectangle(const vpImage< unsigned char > &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill=false, unsigned int thickness=1)
const char * what() const
static void read(vpImage< unsigned char > &I, const std::string &filename)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
Implementation of a pose vector and operations on poses.
Defines a rectangle in the plane.
std::vector< std::string > & getMessage()
std::vector< std::vector< vpImagePoint > > & getPolygon()