1 //! \example tutorial-mb-generic-tracker-rgbd.cpp
2 #include <iostream>
3
4 #include <visp3/core/vpDisplay.h>
5 #include <visp3/core/vpIoTools.h>
6 #include <visp3/io/vpImageIo.h>
7 #include <visp3/core/vpXmlParserCamera.h>
8 #include <visp3/gui/vpDisplayGDI.h>
9 #include <visp3/gui/vpDisplayX.h>
10 //! [Include]
11 #include <visp3/mbt/vpMbGenericTracker.h>
12 //! [Include]
13
14 #if defined (VISP_HAVE_PCL)
15 #include <pcl/common/common.h>
16 #include <pcl/io/pcd_io.h>
17
18 namespace {
19 struct rs_intrinsics {
20 float ppx; /**< Horizontal coordinate of the principal point of the image,
21 as a pixel offset from the left edge */
22 float ppy; /**< Vertical coordinate of the principal point of the image, as
23 a pixel offset from the top edge */
24 float fx; /**< Focal length of the image plane, as a multiple of pixel width
25 */
26 float fy; /**< Focal length of the image plane, as a multiple of pixel
27 height */
28 float coeffs[5]; /**< Distortion coefficients */
29 };
30
rs_deproject_pixel_to_point(float point[3],const rs_intrinsics & intrin,const float pixel[2],float depth)31 void rs_deproject_pixel_to_point(float point[3], const rs_intrinsics &intrin, const float pixel[2], float depth) {
32 float x = (pixel[0] - intrin.ppx) / intrin.fx;
33 float y = (pixel[1] - intrin.ppy) / intrin.fy;
34
35 float r2 = x * x + y * y;
36 float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
37 float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
38 float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
39
40 x = ux;
41 y = uy;
42
43 point[0] = depth * x;
44 point[1] = depth * y;
45 point[2] = depth;
46 }
47
48 //! [Read data function]
read_data(unsigned int cpt,const std::string & input_directory,vpImage<vpRGBa> & I_color,vpImage<uint16_t> & I_depth_raw,pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud)49 bool read_data(unsigned int cpt, const std::string &input_directory, vpImage<vpRGBa> &I_color,
50 vpImage<uint16_t> &I_depth_raw, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
51 //! [Read data function]
52 {
53 char buffer[FILENAME_MAX];
54 // Read color
55 std::stringstream ss;
56 ss << input_directory << "/color_image_%04d.jpg";
57 sprintf(buffer, ss.str().c_str(), cpt);
58 std::string filename_color = buffer;
59
60 if (!vpIoTools::checkFilename(filename_color)) {
61 std::cerr << "Cannot read: " << filename_color << std::endl;
62 return false;
63 }
64 vpImageIo::read(I_color, filename_color);
65
66 // Read raw depth
67 ss.str("");
68 ss << input_directory << "/depth_image_%04d.bin";
69 sprintf(buffer, ss.str().c_str(), cpt);
70 std::string filename_depth = buffer;
71
72 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
73 if (!file_depth.is_open()) {
74 return false;
75 }
76
77 unsigned int height = 0, width = 0;
78 vpIoTools::readBinaryValueLE(file_depth, height);
79 vpIoTools::readBinaryValueLE(file_depth, width);
80 I_depth_raw.resize(height, width);
81
82 uint16_t depth_value = 0;
83 for (unsigned int i = 0; i < height; i++) {
84 for (unsigned int j = 0; j < width; j++) {
85 vpIoTools::readBinaryValueLE(file_depth, depth_value);
86 I_depth_raw[i][j] = depth_value;
87 }
88 }
89
90 // Transform pointcloud
91 pointcloud->width = width;
92 pointcloud->height = height;
93 pointcloud->resize((size_t)width * height);
94
95 // Only for Creative SR300
96 const float depth_scale = 0.00100000005f;
97 rs_intrinsics depth_intrinsic;
98 depth_intrinsic.ppx = 320.503509521484f;
99 depth_intrinsic.ppy = 235.602951049805f;
100 depth_intrinsic.fx = 383.970001220703f;
101 depth_intrinsic.fy = 383.970001220703f;
102 depth_intrinsic.coeffs[0] = 0.0f;
103 depth_intrinsic.coeffs[1] = 0.0f;
104 depth_intrinsic.coeffs[2] = 0.0f;
105 depth_intrinsic.coeffs[3] = 0.0f;
106 depth_intrinsic.coeffs[4] = 0.0f;
107
108 for (unsigned int i = 0; i < height; i++) {
109 for (unsigned int j = 0; j < width; j++) {
110 float scaled_depth = I_depth_raw[i][j] * depth_scale;
111 float point[3];
112 float pixel[2] = {(float)j, (float)i};
113 rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
114 pointcloud->points[(size_t) (i*width + j)].x = point[0];
115 pointcloud->points[(size_t) (i*width + j)].y = point[1];
116 pointcloud->points[(size_t) (i*width + j)].z = point[2];
117 }
118 }
119
120 return true;
121 }
122 }
123
main(int argc,char * argv[])124 int main(int argc, char *argv[])
125 {
126 std::string input_directory = "data"; // location of the data (images, depth_map, point_cloud)
127 std::string config_color = "model/cube/cube.xml", config_depth = "model/cube/cube_depth.xml";
128 std::string model_color = "model/cube/cube.cao", model_depth = "model/cube/cube.cao";
129 std::string init_file = "model/cube/cube.init";
130 unsigned int frame_cpt = 0;
131 bool disable_depth = false;
132
133 for (int i = 1; i < argc; i++) {
134 if (std::string(argv[i]) == "--input_directory" && i+1 < argc) {
135 input_directory = std::string(argv[i+1]);
136 } else if (std::string(argv[i]) == "--config_color" && i+1 < argc) {
137 config_color = std::string(argv[i+1]);
138 } else if (std::string(argv[i]) == "--config_depth" && i+1 < argc) {
139 config_depth = std::string(argv[i+1]);
140 } else if (std::string(argv[i]) == "--model_color" && i+1 < argc) {
141 model_color = std::string(argv[i+1]);
142 } else if (std::string(argv[i]) == "--model_depth" && i+1 < argc) {
143 model_depth = std::string(argv[i+1]);
144 } else if (std::string(argv[i]) == "--init_file" && i+1 < argc) {
145 init_file = std::string(argv[i+1]);
146 } else if (std::string(argv[i]) == "--disable_depth") {
147 disable_depth = true;
148 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
149 std::cout << "Usage: \n" << argv[0] << " --input_directory <data directory> --config_color <object.xml> --config_depth <object.xml>"
150 " --model_color <object.cao> --model_depth <object.cao> --init_file <object.init> --disable_depth" << std::endl;
151 std::cout << "\nExample:\n" << argv[0] << " --config_color model/cube/cube.xml --config_depth model/cube/cube.xml"
152 " --model_color model/cube/cube.cao --model_depth model/cube/cube.cao --init_file model/cube/cube.init\n" << std::endl;
153 return 0;
154 }
155 }
156
157 std::cout << "Tracked features: " << std::endl;
158 #ifdef VISP_HAVE_OPENCV
159 std::cout << " Use edges : 1" << std::endl;
160 std::cout << " Use klt : 1" << std::endl;
161 std::cout << " Use depth : " << ! disable_depth << std::endl;
162 #else
163 std::cout << " Use edges : 1" << std::endl;
164 std::cout << " Use klt : 0" << std::endl;
165 std::cout << " Use depth : 0" << std::endl;
166 #endif
167 std::cout << "Config files: " << std::endl;
168 std::cout << " Input directory: " << "\"" << input_directory << "\"" << std::endl;
169 std::cout << " Config color: " << "\"" << config_color << "\"" << std::endl;
170 std::cout << " Config depth: " << "\"" << config_depth << "\"" << std::endl;
171 std::cout << " Model color : " << "\"" << model_color << "\"" << std::endl;
172 std::cout << " Model depth : " << "\"" << model_depth << "\"" << std::endl;
173 std::cout << " Init file : " << "\"" << init_file << "\"" << std::endl;
174
175 vpImage<vpRGBa> I_color;
176 //! [Images]
177 vpImage<unsigned char> I_gray, I_depth;
178 //! [Images]
179 vpImage<uint16_t> I_depth_raw;
180 //! [Point cloud]
181 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
182 //! [Point cloud]
183 vpCameraParameters cam_color, cam_depth;
184 unsigned int _posx = 100, _posy = 50, _posdx = 10;
185
186 read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
187 vpImageConvert::convert(I_color, I_gray);
188 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
189
190 #if defined(VISP_HAVE_X11)
191 vpDisplayX d1, d2;
192 #elif defined(VISP_HAVE_GDI)
193 vpDisplayGDI d1, d2;
194 #endif
195 d1.init(I_gray, _posx, _posy, "Color stream");
196 d2.init(I_depth, _posx + I_gray.getWidth()+_posdx, _posy, "Depth stream");
197 vpDisplay::display(I_gray);
198 vpDisplay::display(I_depth);
199 vpDisplay::flush(I_gray);
200 vpDisplay::flush(I_depth);
201
202 //! [Constructor]
203 std::vector<int> trackerTypes;
204 #ifdef VISP_HAVE_OPENCV
205 trackerTypes.push_back(vpMbGenericTracker::EDGE_TRACKER | vpMbGenericTracker::KLT_TRACKER);
206 #else
207 trackerTypes.push_back(vpMbGenericTracker::EDGE_TRACKER);
208 #endif
209 trackerTypes.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER);
210 vpMbGenericTracker tracker(trackerTypes);
211 //! [Constructor]
212 //! [Load config file]
213 tracker.loadConfigFile(config_color, config_depth);
214 //! [Load config file]
215 //! [Load cao]
216 tracker.loadModel(model_color, model_depth);
217 //! [Load cao]
218
219 tracker.getCameraParameters(cam_color, cam_depth);
220
221 std::cout << "Camera parameters for color camera (from XML file): " << cam_color << std::endl;
222 std::cout << "Camera parameters for depth camera (from XML file): " << cam_depth << std::endl;
223
224 //! [Set display features]
225 tracker.setDisplayFeatures(true);
226 //! [Set display features]
227
228 //! [Map transformations]
229 vpHomogeneousMatrix depth_M_color;
230 {
231 std::ifstream file( (std::string(input_directory + "/depth_M_color.txt")).c_str() );
232 depth_M_color.load(file);
233 file.close();
234 }
235 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
236 mapOfCameraTransformations["Camera1"] = vpHomogeneousMatrix();
237 mapOfCameraTransformations["Camera2"] = depth_M_color;
238 tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
239 //! [Map transformations]
240 std::cout << "depth_M_color: \n" << depth_M_color << std::endl;
241
242 //! [Map images]
243 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
244 mapOfImages["Camera1"] = &I_gray;
245 mapOfImages["Camera2"] = &I_depth;
246 //! [Map images]
247
248 //! [Map init]
249 std::map<std::string, std::string> mapOfInitFiles;
250 mapOfInitFiles["Camera1"] = init_file;
251 tracker.initClick(mapOfImages, mapOfInitFiles, true);
252 //! [Map init]
253
254 mapOfImages.clear();
255 pcl::PointCloud<pcl::PointXYZ>::Ptr empty_pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
256 std::vector<double> times_vec;
257
258 try {
259 bool quit = false;
260 while (! quit) {
261 double t = vpTime::measureTimeMs();
262 quit = ! read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
263 vpImageConvert::convert(I_color, I_gray);
264 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
265
266 vpDisplay::display(I_gray);
267 vpDisplay::display(I_depth);
268
269 mapOfImages["Camera1"] = &I_gray;
270 std::map<std::string, pcl::PointCloud< pcl::PointXYZ >::ConstPtr> mapOfPointclouds;
271 if (disable_depth)
272 mapOfPointclouds["Camera2"] = empty_pointcloud;
273 else
274 mapOfPointclouds["Camera2"] = pointcloud;
275
276 //! [Track]
277 tracker.track(mapOfImages, mapOfPointclouds);
278 //! [Track]
279
280 //! [Get pose]
281 vpHomogeneousMatrix cMo = tracker.getPose();
282 //! [Get pose]
283
284 std::cout << "iter: " << frame_cpt << " cMo:\n" << cMo << std::endl;
285
286 //! [Display]
287 tracker.display(I_gray, I_depth, cMo, depth_M_color*cMo, cam_color, cam_depth, vpColor::red, 3);
288 vpDisplay::displayFrame(I_gray, cMo, cam_color, 0.05, vpColor::none, 3);
289 vpDisplay::displayFrame(I_depth, depth_M_color*cMo, cam_depth, 0.05, vpColor::none, 3);
290 //! [Display]
291
292 t = vpTime::measureTimeMs() - t;
293 times_vec.push_back(t);
294
295 std::stringstream ss;
296 ss << "Computation time: " << t << " ms";
297 vpDisplay::displayText(I_gray, 20, 20, ss.str(), vpColor::red);
298 {
299 std::stringstream ss;
300 ss << "Nb features: " << tracker.getError().size();
301 vpDisplay::displayText(I_gray, I_gray.getHeight() - 50, 20, ss.str(), vpColor::red);
302 }
303 {
304 std::stringstream ss;
305 ss << "Features: edges " << tracker.getNbFeaturesEdge()
306 << ", klt " << tracker.getNbFeaturesKlt()
307 << ", depth " << tracker.getNbFeaturesDepthDense();
308 vpDisplay::displayText(I_gray, I_gray.getHeight() - 30, 20, ss.str(), vpColor::red);
309 }
310
311 vpDisplay::flush(I_gray);
312 vpDisplay::flush(I_depth);
313
314 vpMouseButton::vpMouseButtonType button;
315 if (vpDisplay::getClick(I_gray, button, false)) {
316 quit = true;
317 }
318
319 frame_cpt ++;
320 }
321 } catch (const vpException &e) {
322 std::cout << "Catch exception: " << e.getStringMessage() << std::endl;
323 }
324
325 std::cout << "\nProcessing time, Mean: " << vpMath::getMean(times_vec) << " ms ; Median: " << vpMath::getMedian(times_vec)
326 << " ; Std: " << vpMath::getStdev(times_vec) << " ms" << std::endl;
327
328 vpDisplay::displayText(I_gray, 60, 20, "Click to quit", vpColor::red);
329 vpDisplay::flush(I_gray);
330 vpDisplay::getClick(I_gray);
331
332 return EXIT_SUCCESS;
333 }
334 #else
main()335 int main()
336 {
337 std::cout << "To run this tutorial, ViSP should be build with PCL library."
338 " Install libpcl, configure and build again ViSP..." << std::endl;
339 return EXIT_SUCCESS;
340 }
341 #endif
342