Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testGenericTrackerDeterminist.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 * Check that MBT is deterministic.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#if defined(VISP_HAVE_CATCH2)
39#define CATCH_CONFIG_ENABLE_BENCHMARKING
40#define CATCH_CONFIG_RUNNER
41#include <catch.hpp>
42
43#include <future>
44#include <thread>
45#include <visp3/core/vpIoTools.h>
46#include <visp3/io/vpImageIo.h>
47#include <visp3/mbt/vpMbGenericTracker.h>
48
49// #define DEBUG_DISPLAY // uncomment to check that the tracking is correct
50#ifdef DEBUG_DISPLAY
51#include <visp3/gui/vpDisplayX.h>
52#endif
53
54namespace
55{
56bool read_data(int cpt, vpImage<unsigned char> &I)
57{
58#if VISP_HAVE_DATASET_VERSION >= 0x030600
59 std::string ext("png");
60#else
61 std::string ext("pgm");
62#endif
63 const std::string env_ipath = vpIoTools::getViSPImagesDataPath();
64 const std::string ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d." + ext);
65
66 char buffer[FILENAME_MAX];
67 snprintf(buffer, FILENAME_MAX, ipath.c_str(), cpt);
68 std::string image_filename = buffer;
69
70 if (!vpIoTools::checkFilename(image_filename)) {
71 return false;
72 }
73
74 vpImageIo::read(I, image_filename);
75 return true;
76}
77
78void checkPoses(const vpHomogeneousMatrix &cMo1, const vpHomogeneousMatrix &cMo2)
79{
80 for (unsigned int i = 0; i < 3; i++) {
81 for (unsigned int j = 0; j < 4; j++) {
82 CHECK(cMo1[i][j] == Approx(cMo2[i][j]).epsilon(std::numeric_limits<double>::epsilon()));
83 }
84 }
85}
86
87void configureTracker(vpMbGenericTracker &tracker, vpCameraParameters &cam)
88{
89 const std::string env_ipath = vpIoTools::getViSPImagesDataPath();
90 const std::string configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
91 const std::string modelFile = vpIoTools::createFilePath(env_ipath, "mbt/cube_and_cylinder.cao");
92 const bool verbose = false;
93 tracker.loadConfigFile(configFile, verbose);
94 tracker.getCameraParameters(cam);
95 tracker.loadModel(modelFile);
96 tracker.setDisplayFeatures(true);
97
98 const vpPoseVector initPose(0.02231950571, 0.1071368004, 0.5071128378, 2.100485509, 1.146812236, -0.4560126437);
100 read_data(0, I);
101 tracker.initFromPose(I, vpHomogeneousMatrix(initPose));
102}
103} // anonymous namespace
104
105TEST_CASE("Check MBT determinism sequential", "[MBT_determinism]")
106{
107 // First tracker
108 vpMbGenericTracker tracker1;
110 configureTracker(tracker1, cam);
111
113 read_data(0, I);
114#ifdef DEBUG_DISPLAY
115 vpDisplayX d(I);
116#endif
117
119 for (int cpt = 0; read_data(cpt, I); cpt++) {
120 tracker1.track(I);
121 tracker1.getPose(cMo1);
122
123#ifdef DEBUG_DISPLAY
125 tracker1.display(I, cMo1, cam, vpColor::red, 3);
126 vpDisplay::displayFrame(I, cMo1, cam, 0.05, vpColor::none, 3);
128#endif
129 }
130 std::cout << "First tracker, final cMo:\n" << cMo1 << std::endl;
131
132 // Second tracker
133 vpMbGenericTracker tracker2;
134 configureTracker(tracker2, cam);
136 for (int cpt = 0; read_data(cpt, I); cpt++) {
137 tracker2.track(I);
138 tracker2.getPose(cMo2);
139
140#ifdef DEBUG_DISPLAY
142 tracker2.display(I, cMo2, cam, vpColor::red, 3);
143 vpDisplay::displayFrame(I, cMo2, cam, 0.05, vpColor::none, 3);
145#endif
146 }
147 std::cout << "Second tracker, final cMo:\n" << cMo2 << std::endl;
148
149 // Check that both poses are identical
150 checkPoses(cMo1, cMo2);
151}
152
153TEST_CASE("Check MBT determinism parallel", "[MBT_determinism]")
154{
155 // First tracker
156 std::future<vpHomogeneousMatrix> res_cMo1 = std::async(std::launch::async, []() {
157 vpMbGenericTracker tracker1;
159 configureTracker(tracker1, cam);
160
163 for (int cpt = 0; read_data(cpt, I); cpt++) {
164 tracker1.track(I);
165 tracker1.getPose(cMo1);
166 }
167 return cMo1;
168 });
169
170 // Second tracker
171 std::future<vpHomogeneousMatrix> res_cMo2 = std::async(std::launch::async, []() {
172 vpMbGenericTracker tracker2;
174 configureTracker(tracker2, cam);
175
178 for (int cpt = 0; read_data(cpt, I); cpt++) {
179 tracker2.track(I);
180 tracker2.getPose(cMo2);
181 }
182 return cMo2;
183 });
184
185 vpHomogeneousMatrix cMo1 = res_cMo1.get();
186 vpHomogeneousMatrix cMo2 = res_cMo2.get();
187 std::cout << "Run both trackers in separate threads" << std::endl;
188 std::cout << "First tracker, final cMo:\n" << cMo1 << std::endl;
189 std::cout << "Second tracker, final cMo:\n" << cMo2 << std::endl;
190
191 // Check that both poses are identical
192 checkPoses(cMo1, cMo2);
193}
194
195TEST_CASE("Check Stereo MBT determinism parallel", "[MBT_determinism]")
196{
197 // First tracker
198 std::future<vpHomogeneousMatrix> res_cMo1 = std::async(std::launch::async, []() {
199 vpMbGenericTracker tracker1(2);
201 configureTracker(tracker1, cam);
202
205 for (int cpt = 0; read_data(cpt, I); cpt++) {
206 tracker1.track(I, I);
207 tracker1.getPose(cMo1);
208 }
209 return cMo1;
210 });
211
212 // Second tracker
213 std::future<vpHomogeneousMatrix> res_cMo2 = std::async(std::launch::async, []() {
214 vpMbGenericTracker tracker2(2);
216 configureTracker(tracker2, cam);
217
220 for (int cpt = 0; read_data(cpt, I); cpt++) {
221 tracker2.track(I, I);
222 tracker2.getPose(cMo2);
223 }
224 return cMo2;
225 });
226
227 vpHomogeneousMatrix cMo1 = res_cMo1.get();
228 vpHomogeneousMatrix cMo2 = res_cMo2.get();
229 std::cout << "Run both stereo trackers in separate threads" << std::endl;
230 std::cout << "First tracker, final cMo:\n" << cMo1 << std::endl;
231 std::cout << "Second tracker, final cMo:\n" << cMo2 << std::endl;
232
233 // Check that both poses are identical
234 checkPoses(cMo1, cMo2);
235}
236
237int main(int argc, char *argv[])
238{
239 Catch::Session session; // There must be exactly one instance
240
241 // Let Catch (using Clara) parse the command line
242 session.applyCommandLine(argc, argv);
243
244 int numFailed = session.run();
245
246 // numFailed is clamped to 255 as some unices only use the lower 8 bits.
247 // This clamping has already been applied, so just return it here
248 // You can also do any post run clean-up here
249 return numFailed;
250}
251#else
252#include <iostream>
253
254int main() { return EXIT_SUCCESS; }
255#endif
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
static void display(const vpImage< unsigned char > &I)
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), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void flush(const vpImage< unsigned char > &I)
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 of the vpImage class member functions.
Definition vpImage.h:135
static std::string getViSPImagesDataPath()
static bool checkFilename(const std::string &filename)
static std::string createFilePath(const std::string &parent, const std::string &child)
Real-time 6D object pose tracking using its CAD model.
virtual void getPose(vpHomogeneousMatrix &cMo) const
virtual void setDisplayFeatures(bool displayF)
virtual void initFromPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual void getCameraParameters(vpCameraParameters &camera) const
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
virtual void track(const vpImage< unsigned char > &I)
Implementation of a pose vector and operations on poses.