Visual Servoing Platform version 3.5.0
testGenericTrackerDeterminist.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 <thread>
44#include <future>
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 const std::string env_ipath = vpIoTools::getViSPImagesDataPath();
59 const std::string ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d.pgm");
60
61 char buffer[256];
62 sprintf(buffer, ipath.c_str(), cpt);
63 std::string image_filename = buffer;
64
65 if (!vpIoTools::checkFilename(image_filename)) {
66 return false;
67 }
68
69 vpImageIo::read(I, image_filename);
70 return true;
71}
72
73void checkPoses(const vpHomogeneousMatrix& cMo1, const vpHomogeneousMatrix& cMo2)
74{
75 for (unsigned int i = 0; i < 3; i++) {
76 for (unsigned int j = 0; j < 4; j++) {
77 CHECK(cMo1[i][j] == Approx(cMo2[i][j]).epsilon(std::numeric_limits<double>::epsilon()));
78 }
79 }
80}
81
82void configureTracker(vpMbGenericTracker& tracker, vpCameraParameters& cam)
83{
84 const std::string env_ipath = vpIoTools::getViSPImagesDataPath();
85 const std::string configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
86 const std::string modelFile = vpIoTools::createFilePath(env_ipath, "mbt/cube_and_cylinder.cao");
87 const bool verbose = false;
88 tracker.loadConfigFile(configFile, verbose);
89 tracker.getCameraParameters(cam);
90 tracker.loadModel(modelFile);
91 tracker.setDisplayFeatures(true);
92
93 const vpPoseVector initPose(0.02231950571, 0.1071368004, 0.5071128378, 2.100485509, 1.146812236, -0.4560126437);
95 read_data(0, I);
96 tracker.initFromPose(I, vpHomogeneousMatrix(initPose));
97}
98} //anonymous namespace
99
100TEST_CASE("Check MBT determinism sequential", "[MBT_determinism]") {
101 // First tracker
102 vpMbGenericTracker tracker1;
104 configureTracker(tracker1, cam);
105
107 read_data(0, I);
108#ifdef DEBUG_DISPLAY
109 vpDisplayX d(I);
110#endif
111
113 for (int cpt = 0; read_data(cpt, I); cpt++) {
114 tracker1.track(I);
115 tracker1.getPose(cMo1);
116
117#ifdef DEBUG_DISPLAY
119 tracker1.display(I, cMo1, cam, vpColor::red, 3);
120 vpDisplay::displayFrame(I, cMo1, cam, 0.05, vpColor::none, 3);
122#endif
123 }
124 std::cout << "First tracker, final cMo:\n" << cMo1 << std::endl;
125
126 // Second tracker
127 vpMbGenericTracker tracker2;
128 configureTracker(tracker2, cam);
130 for (int cpt = 0; read_data(cpt, I); cpt++) {
131 tracker2.track(I);
132 tracker2.getPose(cMo2);
133
134#ifdef DEBUG_DISPLAY
136 tracker2.display(I, cMo2, cam, vpColor::red, 3);
137 vpDisplay::displayFrame(I, cMo2, cam, 0.05, vpColor::none, 3);
139#endif
140 }
141 std::cout << "Second tracker, final cMo:\n" << cMo2 << std::endl;
142
143 // Check that both poses are identical
144 checkPoses(cMo1, cMo2);
145}
146
147TEST_CASE("Check MBT determinism parallel", "[MBT_determinism]") {
148 // First tracker
149 std::future<vpHomogeneousMatrix> res_cMo1 = std::async(std::launch::async, []() {
150 vpMbGenericTracker tracker1;
152 configureTracker(tracker1, cam);
153
156 for (int cpt = 0; read_data(cpt, I); cpt++) {
157 tracker1.track(I);
158 tracker1.getPose(cMo1);
159 }
160 return cMo1;
161 });
162
163 // Second tracker
164 std::future<vpHomogeneousMatrix> res_cMo2 = std::async(std::launch::async, []() {
165 vpMbGenericTracker tracker2;
167 configureTracker(tracker2, cam);
168
171 for (int cpt = 0; read_data(cpt, I); cpt++) {
172 tracker2.track(I);
173 tracker2.getPose(cMo2);
174 }
175 return cMo2;
176 });
177
178 vpHomogeneousMatrix cMo1 = res_cMo1.get();
179 vpHomogeneousMatrix cMo2 = res_cMo2.get();
180 std::cout << "Run both trackers in seperate threads" << std::endl;
181 std::cout << "First tracker, final cMo:\n" << cMo1 << std::endl;
182 std::cout << "Second tracker, final cMo:\n" << cMo2 << std::endl;
183
184 // Check that both poses are identical
185 checkPoses(cMo1, cMo2);
186}
187
188TEST_CASE("Check Stereo MBT determinism parallel", "[MBT_determinism]") {
189 // First tracker
190 std::future<vpHomogeneousMatrix> res_cMo1 = std::async(std::launch::async, []() {
191 vpMbGenericTracker tracker1(2);
193 configureTracker(tracker1, cam);
194
197 for (int cpt = 0; read_data(cpt, I); cpt++) {
198 tracker1.track(I, I);
199 tracker1.getPose(cMo1);
200 }
201 return cMo1;
202 });
203
204
205 // Second tracker
206 std::future<vpHomogeneousMatrix> res_cMo2 = std::async(std::launch::async, []() {
207 vpMbGenericTracker tracker2(2);
209 configureTracker(tracker2, cam);
210
213 for (int cpt = 0; read_data(cpt, I); cpt++) {
214 tracker2.track(I, I);
215 tracker2.getPose(cMo2);
216 }
217 return cMo2;
218 });
219
220 vpHomogeneousMatrix cMo1 = res_cMo1.get();
221 vpHomogeneousMatrix cMo2 = res_cMo2.get();
222 std::cout << "Run both stereo trackers in seperate threads" << std::endl;
223 std::cout << "First tracker, final cMo:\n" << cMo1 << std::endl;
224 std::cout << "Second tracker, final cMo:\n" << cMo2 << std::endl;
225
226 // Check that both poses are identical
227 checkPoses(cMo1, cMo2);
228}
229
230int main(int argc, char *argv[])
231{
232 Catch::Session session; // There must be exactly one instance
233
234 // Let Catch (using Clara) parse the command line
235 session.applyCommandLine(argc, argv);
236
237 int numFailed = session.run();
238
239 // numFailed is clamped to 255 as some unices only use the lower 8 bits.
240 // This clamping has already been applied, so just return it here
241 // You can also do any post run clean-up here
242 return numFailed;
243}
244#else
245#include <iostream>
246
247int main()
248{
249 return 0;
250}
251#endif
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
static void display(const vpImage< unsigned char > &I)
static void flush(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))
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: vpImageIo.cpp:149
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1365
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:802
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1670
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.
Definition: vpPoseVector.h:152