Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
perfGenericTracker.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 * Benchmark generic tracker.
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 <visp3/core/vpIoTools.h>
44#include <visp3/io/vpImageIo.h>
45#include <visp3/mbt/vpMbGenericTracker.h>
46
47// #define DEBUG_DISPLAY // uncomment to check that the tracking is correct
48#ifdef DEBUG_DISPLAY
49#include <visp3/gui/vpDisplayX.h>
50#endif
51
52namespace
53{
54bool runBenchmark = false;
55
56template <typename Type>
57bool read_data(const std::string &input_directory, int cpt, const vpCameraParameters &cam_depth, vpImage<Type> &I,
58 vpImage<uint16_t> &I_depth, std::vector<vpColVector> &pointcloud, vpHomogeneousMatrix &cMo)
59{
60#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
61 static_assert(std::is_same<Type, unsigned char>::value || std::is_same<Type, vpRGBa>::value,
62 "Template function supports only unsigned char and vpRGBa images!");
63#endif
64#if VISP_HAVE_DATASET_VERSION >= 0x030600
65 std::string ext("png");
66#else
67 std::string ext("pgm");
68#endif
69 char buffer[FILENAME_MAX];
70 snprintf(buffer, FILENAME_MAX, std::string(input_directory + "/Images/Image_%04d." + ext).c_str(), cpt);
71 std::string image_filename = buffer;
72
73 snprintf(buffer, FILENAME_MAX, std::string(input_directory + "/Depth/Depth_%04d.bin").c_str(), cpt);
74 std::string depth_filename = buffer;
75
76 snprintf(buffer, FILENAME_MAX, std::string(input_directory + "/CameraPose/Camera_%03d.txt").c_str(), cpt);
77 std::string pose_filename = buffer;
78
79 if (!vpIoTools::checkFilename(image_filename) || !vpIoTools::checkFilename(depth_filename) ||
80 !vpIoTools::checkFilename(pose_filename))
81 return false;
82
83 vpImageIo::read(I, image_filename);
84
85 unsigned int depth_width = 0, depth_height = 0;
86 std::ifstream file_depth(depth_filename.c_str(), std::ios::in | std::ios::binary);
87 if (!file_depth.is_open())
88 return false;
89
90 vpIoTools::readBinaryValueLE(file_depth, depth_height);
91 vpIoTools::readBinaryValueLE(file_depth, depth_width);
92 I_depth.resize(depth_height, depth_width);
93 pointcloud.resize(depth_height * depth_width);
94
95 const float depth_scale = 0.000030518f;
96 for (unsigned int i = 0; i < I_depth.getHeight(); i++) {
97 for (unsigned int j = 0; j < I_depth.getWidth(); j++) {
98 vpIoTools::readBinaryValueLE(file_depth, I_depth[i][j]);
99 double x = 0.0, y = 0.0, Z = I_depth[i][j] * depth_scale;
100 vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
101 vpColVector pt3d(4, 1.0);
102 pt3d[0] = x * Z;
103 pt3d[1] = y * Z;
104 pt3d[2] = Z;
105 pointcloud[i * I_depth.getWidth() + j] = pt3d;
106 }
107 }
108
109 std::ifstream file_pose(pose_filename.c_str());
110 if (!file_pose.is_open()) {
111 return false;
112 }
113
114 for (unsigned int i = 0; i < 4; i++) {
115 for (unsigned int j = 0; j < 4; j++) {
116 file_pose >> cMo[i][j];
117 }
118 }
119
120 return true;
121}
122} // anonymous namespace
123
124TEST_CASE("Benchmark generic tracker", "[benchmark]")
125{
126 if (runBenchmark) {
127 std::vector<int> tracker_type(2);
128 tracker_type[0] = vpMbGenericTracker::EDGE_TRACKER;
130 vpMbGenericTracker tracker(tracker_type);
131
132 const std::string input_directory =
134 const std::string configFileCam1 = input_directory + std::string("/Config/chateau.xml");
135 const std::string configFileCam2 = input_directory + std::string("/Config/chateau_depth.xml");
136 REQUIRE(vpIoTools::checkFilename(configFileCam1));
137 REQUIRE(vpIoTools::checkFilename(configFileCam2));
138 const bool verbose = false;
139 tracker.loadConfigFile(configFileCam1, configFileCam2, verbose);
140 REQUIRE(vpIoTools::checkFilename(input_directory + "/Models/chateau.cao"));
141 tracker.loadModel(input_directory + "/Models/chateau.cao", input_directory + "/Models/chateau.cao", verbose);
142
144 T[0][0] = -1;
145 T[0][3] = -0.2;
146 T[1][1] = 0;
147 T[1][2] = 1;
148 T[1][3] = 0.12;
149 T[2][1] = 1;
150 T[2][2] = 0;
151 T[2][3] = -0.15;
152 tracker.loadModel(input_directory + "/Models/cube.cao", verbose, T);
153
155 vpImage<uint16_t> I_depth_raw;
156 vpHomogeneousMatrix cMo_truth;
157 std::vector<vpColVector> pointcloud;
158
159 vpCameraParameters cam_color, cam_depth;
160 tracker.getCameraParameters(cam_color, cam_depth);
161
162 vpHomogeneousMatrix depth_M_color;
163 depth_M_color[0][3] = -0.05;
164 tracker.setCameraTransformationMatrix("Camera2", depth_M_color);
165
166 // load all the data in memory to not take into account I/O from disk
167 std::vector<vpImage<unsigned char> > images;
168 std::vector<vpImage<uint16_t> > depth_raws;
169 std::vector<std::vector<vpColVector> > pointclouds;
170 std::vector<vpHomogeneousMatrix> cMo_truth_all;
171 // forward
172 for (int i = 1; i <= 40; i++) {
173 if (read_data(input_directory, i, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
174 images.push_back(I);
175 depth_raws.push_back(I_depth_raw);
176 pointclouds.push_back(pointcloud);
177 cMo_truth_all.push_back(cMo_truth);
178 }
179 }
180 // backward
181 for (int i = 40; i >= 1; i--) {
182 if (read_data(input_directory, i, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
183 images.push_back(I);
184 depth_raws.push_back(I_depth_raw);
185 pointclouds.push_back(pointcloud);
186 cMo_truth_all.push_back(cMo_truth);
187 }
188 }
189
190 // Stereo MBT
191 {
192 std::vector<std::map<std::string, int> > mapOfTrackerTypes;
193 mapOfTrackerTypes.push_back(
195 mapOfTrackerTypes.push_back(
197#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
198 mapOfTrackerTypes.push_back(
200 mapOfTrackerTypes.push_back({{"Camera1", vpMbGenericTracker::EDGE_TRACKER | vpMbGenericTracker::KLT_TRACKER},
202 mapOfTrackerTypes.push_back({{"Camera1", vpMbGenericTracker::EDGE_TRACKER | vpMbGenericTracker::KLT_TRACKER},
204#endif
205
206 std::vector<std::string> benchmarkNames = {
207 "Edge MBT",
208 "Edge + Depth dense MBT",
209#if defined(VISP_HAVE_OPENCV)
210 "KLT MBT",
211 "KLT + depth dense MBT",
212 "Edge + KLT + depth dense MBT"
213#endif
214 };
215
216 std::vector<bool> monoculars = {
217 true,
218 false,
219#if defined(VISP_HAVE_OPENCV)
220 true,
221 false,
222 false
223#endif
224 };
225
226 for (size_t idx = 0; idx < mapOfTrackerTypes.size(); idx++) {
227 tracker.resetTracker();
228 tracker.setTrackerType(mapOfTrackerTypes[idx]);
229
230 const bool verbose = false;
231 tracker.loadConfigFile(configFileCam1, configFileCam2, verbose);
232 tracker.loadModel(input_directory + "/Models/chateau.cao", input_directory + "/Models/chateau.cao", verbose);
233 tracker.loadModel(input_directory + "/Models/cube.cao", verbose, T);
234 tracker.initFromPose(images.front(), cMo_truth_all.front());
235
236 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
237 mapOfWidths["Camera2"] = monoculars[idx] ? 0 : I_depth_raw.getWidth();
238 mapOfHeights["Camera2"] = monoculars[idx] ? 0 : I_depth_raw.getHeight();
239
241#ifndef DEBUG_DISPLAY
242 BENCHMARK(benchmarkNames[idx].c_str())
243#else
245 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
246
247 vpDisplayX d_color(I, 0, 0, "Color image");
248 vpDisplayX d_depth(I_depth, I.getWidth(), 0, "Depth image");
249 tracker.setDisplayFeatures(true);
250#endif
251 {
252 tracker.initFromPose(images.front(), cMo_truth_all.front());
253
254 for (size_t i = 0; i < images.size(); i++) {
255 const vpImage<unsigned char> &I_current = images[i];
256 const std::vector<vpColVector> &pointcloud_current = pointclouds[i];
257
258#ifdef DEBUG_DISPLAY
259 vpImageConvert::createDepthHistogram(depth_raws[i], I_depth);
260 I = I_current;
262 vpDisplay::display(I_depth);
263#endif
264
265 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
266 mapOfImages["Camera1"] = &I_current;
267
268 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
269 mapOfPointclouds["Camera2"] = &pointcloud_current;
270
271 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
272 cMo = tracker.getPose();
273
274#ifdef DEBUG_DISPLAY
275 tracker.display(I, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth, vpColor::red, 3);
276 vpDisplay::displayFrame(I, cMo, cam_color, 0.05, vpColor::none, 3);
277 vpDisplay::displayFrame(I_depth, depth_M_color * cMo, cam_depth, 0.05, vpColor::none, 3);
278 vpDisplay::displayText(I, 20, 20, benchmarkNames[idx], vpColor::red);
280 I, 40, 20, std::string("Nb features: " + std::to_string(tracker.getError().getRows())), vpColor::red);
281
283 vpDisplay::flush(I_depth);
284 vpTime::wait(33);
285#endif
286 }
287
288#ifndef DEBUG_DISPLAY
289 return cMo;
290 };
291#else
292 }
293#endif
294
295 vpPoseVector pose_est(cMo);
296 vpPoseVector pose_truth(cMo_truth);
297 vpColVector t_err(3), tu_err(3);
298 for (unsigned int i = 0; i < 3; i++) {
299 t_err[i] = pose_est[i] - pose_truth[i];
300 tu_err[i] = pose_est[i + 3] - pose_truth[i + 3];
301 }
302
303 const double max_translation_error = 0.006;
304 const double max_rotation_error = 0.03;
305 CHECK(sqrt(t_err.sumSquare()) < max_translation_error);
306 CHECK(sqrt(tu_err.sumSquare()) < max_rotation_error);
307 }
308 }
309 } // if (runBenchmark)
310}
311
312int main(int argc, char *argv[])
313{
314 Catch::Session session; // There must be exactly one instance
315
316 // Build a new parser on top of Catch's
317 using namespace Catch::clara;
318 auto cli = session.cli() // Get Catch's composite command line parser
319 | Opt(runBenchmark) // bind variable to a new option, with a hint string
320 ["--benchmark"] // the option names it will respond to
321 ("run benchmark comparing naive code with ViSP implementation"); // description string for the help output
322
323 // Now pass the new composite back to Catch so it uses that
324 session.cli(cli);
325
326 // Let Catch (using Clara) parse the command line
327 session.applyCommandLine(argc, argv);
328
329 int numFailed = session.run();
330
331 // numFailed is clamped to 255 as some unices only use the lower 8 bits.
332 // This clamping has already been applied, so just return it here
333 // You can also do any post run clean-up here
334 return numFailed;
335}
336#else
337#include <iostream>
338
339int main() { return EXIT_SUCCESS; }
340#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
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)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
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
unsigned int getWidth() const
Definition vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
unsigned int getHeight() const
Definition vpImage.h:184
static std::string getViSPImagesDataPath()
static bool checkFilename(const std::string &filename)
static void readBinaryValueLE(std::ifstream &file, int16_t &short_value)
static std::string createFilePath(const std::string &parent, const std::string &child)
Real-time 6D object pose tracking using its CAD model.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Implementation of a pose vector and operations on poses.
VISP_EXPORT int wait(double t0, double t)