Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-mb-generic-tracker-rgbd.cpp
1
2#include <iostream>
3
4#include <visp3/core/vpDisplay.h>
5#include <visp3/core/vpIoTools.h>
6#include <visp3/core/vpXmlParserCamera.h>
7#include <visp3/gui/vpDisplayGDI.h>
8#include <visp3/gui/vpDisplayX.h>
9#include <visp3/io/vpImageIo.h>
11#include <visp3/mbt/vpMbGenericTracker.h>
13
14#if defined(VISP_HAVE_PCL)
15#include <pcl/common/common.h>
16#include <pcl/io/pcd_io.h>
17
18namespace
19{
20struct rs_intrinsics {
21 float ppx;
23 float ppy;
25 float fx;
27 float fy;
29 float coeffs[5];
30};
31
32void rs_deproject_pixel_to_point(float point[3], const rs_intrinsics &intrin, const float pixel[2], float depth)
33{
34 float x = (pixel[0] - intrin.ppx) / intrin.fx;
35 float y = (pixel[1] - intrin.ppy) / intrin.fy;
36
37 float r2 = x * x + y * y;
38 float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
39 float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
40 float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
41
42 x = ux;
43 y = uy;
44
45 point[0] = depth * x;
46 point[1] = depth * y;
47 point[2] = depth;
48}
49
51bool read_data(unsigned int cpt, const std::string &input_directory, vpImage<vpRGBa> &I_color,
52 vpImage<uint16_t> &I_depth_raw, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
54{
55 char buffer[FILENAME_MAX];
56 // Read color
57 std::stringstream ss;
58 ss << input_directory << "/color_image_%04d.jpg";
59 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
60 std::string filename_color = buffer;
61
62 if (!vpIoTools::checkFilename(filename_color)) {
63 std::cerr << "Cannot read: " << filename_color << std::endl;
64 return false;
65 }
66 vpImageIo::read(I_color, filename_color);
67
68 // Read raw depth
69 ss.str("");
70 ss << input_directory << "/depth_image_%04d.bin";
71 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
72 std::string filename_depth = buffer;
73
74 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
75 if (!file_depth.is_open()) {
76 return false;
77 }
78
79 unsigned int height = 0, width = 0;
80 vpIoTools::readBinaryValueLE(file_depth, height);
81 vpIoTools::readBinaryValueLE(file_depth, width);
82 I_depth_raw.resize(height, width);
83
84 uint16_t depth_value = 0;
85 for (unsigned int i = 0; i < height; i++) {
86 for (unsigned int j = 0; j < width; j++) {
87 vpIoTools::readBinaryValueLE(file_depth, depth_value);
88 I_depth_raw[i][j] = depth_value;
89 }
90 }
91
92 // Transform pointcloud
93 pointcloud->width = width;
94 pointcloud->height = height;
95 pointcloud->resize((size_t)width * height);
96
97 // Only for Creative SR300
98 const float depth_scale = 0.00100000005f;
99 rs_intrinsics depth_intrinsic;
100 depth_intrinsic.ppx = 320.503509521484f;
101 depth_intrinsic.ppy = 235.602951049805f;
102 depth_intrinsic.fx = 383.970001220703f;
103 depth_intrinsic.fy = 383.970001220703f;
104 depth_intrinsic.coeffs[0] = 0.0f;
105 depth_intrinsic.coeffs[1] = 0.0f;
106 depth_intrinsic.coeffs[2] = 0.0f;
107 depth_intrinsic.coeffs[3] = 0.0f;
108 depth_intrinsic.coeffs[4] = 0.0f;
109
110 for (unsigned int i = 0; i < height; i++) {
111 for (unsigned int j = 0; j < width; j++) {
112 float scaled_depth = I_depth_raw[i][j] * depth_scale;
113 float point[3];
114 float pixel[2] = {(float)j, (float)i};
115 rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
116 pointcloud->points[(size_t)(i * width + j)].x = point[0];
117 pointcloud->points[(size_t)(i * width + j)].y = point[1];
118 pointcloud->points[(size_t)(i * width + j)].z = point[2];
119 }
120 }
121
122 return true;
123}
124} // namespace
125
126int main(int argc, char *argv[])
127{
128 std::string input_directory = "data"; // location of the data (images, depth_map, point_cloud)
129 std::string config_color = "model/cube/cube.xml", config_depth = "model/cube/cube_depth.xml";
130 std::string model_color = "model/cube/cube.cao", model_depth = "model/cube/cube.cao";
131 std::string init_file = "model/cube/cube.init";
132 unsigned int frame_cpt = 0;
133 bool disable_depth = false;
134
135 for (int i = 1; i < argc; i++) {
136 if (std::string(argv[i]) == "--input_directory" && i + 1 < argc) {
137 input_directory = std::string(argv[i + 1]);
138 } else if (std::string(argv[i]) == "--config_color" && i + 1 < argc) {
139 config_color = std::string(argv[i + 1]);
140 } else if (std::string(argv[i]) == "--config_depth" && i + 1 < argc) {
141 config_depth = std::string(argv[i + 1]);
142 } else if (std::string(argv[i]) == "--model_color" && i + 1 < argc) {
143 model_color = std::string(argv[i + 1]);
144 } else if (std::string(argv[i]) == "--model_depth" && i + 1 < argc) {
145 model_depth = std::string(argv[i + 1]);
146 } else if (std::string(argv[i]) == "--init_file" && i + 1 < argc) {
147 init_file = std::string(argv[i + 1]);
148 } else if (std::string(argv[i]) == "--disable_depth") {
149 disable_depth = true;
150 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
151 std::cout << "Usage: \n"
152 << argv[0]
153 << " --input_directory <data directory> --config_color <object.xml> --config_depth <object.xml>"
154 " --model_color <object.cao> --model_depth <object.cao> --init_file <object.init> --disable_depth"
155 << std::endl;
156 std::cout
157 << "\nExample:\n"
158 << argv[0]
159 << " --config_color model/cube/cube.xml --config_depth model/cube/cube.xml"
160 " --model_color model/cube/cube.cao --model_depth model/cube/cube.cao --init_file model/cube/cube.init\n"
161 << std::endl;
162 return EXIT_SUCCESS;
163 }
164 }
165
166 std::cout << "Tracked features: " << std::endl;
167#if defined(VISP_HAVE_OPENCV)
168 std::cout << " Use edges : 1" << std::endl;
169#if defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
170 std::cout << " Use klt : 1" << std::endl;
171#else
172 std::cout << " Use klt : 0" << std::endl;
173#endif
174 std::cout << " Use depth : " << !disable_depth << std::endl;
175#else
176 std::cout << " Use edges : 1" << std::endl;
177 std::cout << " Use klt : 0" << std::endl;
178 std::cout << " Use depth : 0" << std::endl;
179#endif
180 std::cout << "Config files: " << std::endl;
181 std::cout << " Input directory: "
182 << "\"" << input_directory << "\"" << std::endl;
183 std::cout << " Config color: "
184 << "\"" << config_color << "\"" << std::endl;
185 std::cout << " Config depth: "
186 << "\"" << config_depth << "\"" << std::endl;
187 std::cout << " Model color : "
188 << "\"" << model_color << "\"" << std::endl;
189 std::cout << " Model depth : "
190 << "\"" << model_depth << "\"" << std::endl;
191 std::cout << " Init file : "
192 << "\"" << init_file << "\"" << std::endl;
193
194 vpImage<vpRGBa> I_color;
196 vpImage<unsigned char> I_gray, I_depth;
198 vpImage<uint16_t> I_depth_raw;
200 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
202 vpCameraParameters cam_color, cam_depth;
203
204 read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
205 vpImageConvert::convert(I_color, I_gray);
206 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
207
208#if defined(VISP_HAVE_X11)
209 vpDisplayX d1, d2;
210#elif defined(VISP_HAVE_GDI)
211 vpDisplayGDI d1, d2;
212#endif
213
214#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
215 unsigned int _posx = 100, _posy = 50, _posdx = 10;
216 d1.init(I_gray, _posx, _posy, "Color stream");
217 d2.init(I_depth, _posx + I_gray.getWidth() + _posdx, _posy, "Depth stream");
218#endif
219
220 vpDisplay::display(I_gray);
221 vpDisplay::display(I_depth);
222 vpDisplay::flush(I_gray);
223 vpDisplay::flush(I_depth);
224
226 std::vector<int> trackerTypes;
227#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
229#else
230 trackerTypes.push_back(vpMbGenericTracker::EDGE_TRACKER);
231#endif
232 trackerTypes.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER);
233 vpMbGenericTracker tracker(trackerTypes);
236 tracker.loadConfigFile(config_color, config_depth);
239 tracker.loadModel(model_color, model_depth);
241
242 tracker.getCameraParameters(cam_color, cam_depth);
243
244 std::cout << "Camera parameters for color camera (from XML file): " << cam_color << std::endl;
245 std::cout << "Camera parameters for depth camera (from XML file): " << cam_depth << std::endl;
246
248 tracker.setDisplayFeatures(true);
250
252 vpHomogeneousMatrix depth_M_color;
253 {
254 std::ifstream file((std::string(input_directory + "/depth_M_color.txt")).c_str());
255 depth_M_color.load(file);
256 file.close();
257 }
258 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
259 mapOfCameraTransformations["Camera1"] = vpHomogeneousMatrix();
260 mapOfCameraTransformations["Camera2"] = depth_M_color;
261 tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
263 std::cout << "depth_M_color: \n" << depth_M_color << std::endl;
264
266 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
267 mapOfImages["Camera1"] = &I_gray;
268 mapOfImages["Camera2"] = &I_depth;
270
272 std::map<std::string, std::string> mapOfInitFiles;
273 mapOfInitFiles["Camera1"] = init_file;
274 tracker.initClick(mapOfImages, mapOfInitFiles, true);
276
277 mapOfImages.clear();
278 pcl::PointCloud<pcl::PointXYZ>::Ptr empty_pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
279 std::vector<double> times_vec;
280
281 try {
282 bool quit = false;
283 while (!quit) {
284 double t = vpTime::measureTimeMs();
285 quit = !read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
286 vpImageConvert::convert(I_color, I_gray);
287 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
288
289 vpDisplay::display(I_gray);
290 vpDisplay::display(I_depth);
291
292 mapOfImages["Camera1"] = &I_gray;
293 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> mapOfPointclouds;
294 if (disable_depth)
295 mapOfPointclouds["Camera2"] = empty_pointcloud;
296 else
297 mapOfPointclouds["Camera2"] = pointcloud;
298
300 tracker.track(mapOfImages, mapOfPointclouds);
302
304 vpHomogeneousMatrix cMo = tracker.getPose();
306
307 std::cout << "iter: " << frame_cpt << " cMo:\n" << cMo << std::endl;
308
310 tracker.display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth, vpColor::red, 3);
311 vpDisplay::displayFrame(I_gray, cMo, cam_color, 0.05, vpColor::none, 3);
312 vpDisplay::displayFrame(I_depth, depth_M_color * cMo, cam_depth, 0.05, vpColor::none, 3);
314
315 t = vpTime::measureTimeMs() - t;
316 times_vec.push_back(t);
317
318 std::stringstream ss;
319 ss << "Computation time: " << t << " ms";
320 vpDisplay::displayText(I_gray, 20, 20, ss.str(), vpColor::red);
321 {
322 std::stringstream ss;
323 ss << "Nb features: " << tracker.getError().size();
324 vpDisplay::displayText(I_gray, I_gray.getHeight() - 50, 20, ss.str(), vpColor::red);
325 }
326 {
327 std::stringstream ss;
328 ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt() << ", depth "
329 << tracker.getNbFeaturesDepthDense();
330 vpDisplay::displayText(I_gray, I_gray.getHeight() - 30, 20, ss.str(), vpColor::red);
331 }
332
333 vpDisplay::flush(I_gray);
334 vpDisplay::flush(I_depth);
335
337 if (vpDisplay::getClick(I_gray, button, false)) {
338 quit = true;
339 }
340
341 frame_cpt++;
342 }
343 } catch (const vpException &e) {
344 std::cout << "Catch exception: " << e.getStringMessage() << std::endl;
345 }
346
347 std::cout << "\nProcessing time, Mean: " << vpMath::getMean(times_vec)
348 << " ms ; Median: " << vpMath::getMedian(times_vec) << " ; Std: " << vpMath::getStdev(times_vec) << " ms"
349 << std::endl;
350
351 vpDisplay::displayText(I_gray, 60, 20, "Click to quit", vpColor::red);
352 vpDisplay::flush(I_gray);
353 vpDisplay::getClick(I_gray);
354
355 return EXIT_SUCCESS;
356}
357#else
358int main()
359{
360 std::cout << "To run this tutorial, ViSP should be build with PCL library."
361 " Install libpcl, configure and build again ViSP..."
362 << std::endl;
363 return EXIT_SUCCESS;
364}
365#endif
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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)
error that can be emitted by ViSP classes.
Definition vpException.h:59
const std::string & getStringMessage() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
void load(std::ifstream &f)
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
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 bool checkFilename(const std::string &filename)
static void readBinaryValueLE(std::ifstream &file, int16_t &short_value)
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:314
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition vpMath.cpp:345
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:294
Real-time 6D object pose tracking using its CAD model.
VISP_EXPORT double measureTimeMs()