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