Visual Servoing Platform version 3.5.0
tutorial-mb-generic-tracker-rgbd-blender.cpp
1
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/gui/vpDisplayX.h>
8#include <visp3/gui/vpDisplayGDI.h>
9#include <visp3/gui/vpDisplayOpenCV.h>
10#include <visp3/mbt/vpMbGenericTracker.h>
11
12#if (VISP_HAVE_OPENCV_VERSION >= 0x020403)
13namespace {
14bool read_data(unsigned int cpt, const std::string &input_directory, vpImage<unsigned char> &I,
15 vpImage<uint16_t> &I_depth_raw, unsigned int &depth_width, unsigned int &depth_height,
16 std::vector<vpColVector> &pointcloud, const vpCameraParameters &cam,
17 vpHomogeneousMatrix &cMo_ground_truth)
18{
19 char buffer[FILENAME_MAX];
20 // Read color
21 std::stringstream ss;
22 ss << input_directory << "/images/%04d.jpg";
23 sprintf(buffer, ss.str().c_str(), cpt);
24 std::string filename_img = buffer;
25
26 if (!vpIoTools::checkFilename(filename_img)) {
27 std::cerr << "Cannot read: " << filename_img << std::endl;
28 return false;
29 }
30 vpImageIo::read(I, filename_img);
31
32 // Read depth
33 ss.str("");
34 ss << input_directory << "/depth/Image%04d.exr";
35 sprintf(buffer, ss.str().c_str(), cpt);
36 std::string filename_depth = buffer;
37
38 cv::Mat depth_raw = cv::imread(filename_depth, cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);
39 if (depth_raw.empty()) {
40 std::cerr << "Cannot read: " << filename_depth << std::endl;
41 return false;
42 }
43
44 depth_width = static_cast<unsigned int>(depth_raw.cols);
45 depth_height = static_cast<unsigned int>(depth_raw.rows);
46 I_depth_raw.resize(depth_height, depth_width);
47 pointcloud.resize(depth_width*depth_height);
48
49 for (int i = 0; i < depth_raw.rows; i++) {
50 for (int j = 0; j < depth_raw.cols; j++) {
51 I_depth_raw[i][j] = static_cast<uint16_t>(32767.5f * depth_raw.at<cv::Vec3f>(i, j)[0]);
52 double x = 0.0, y = 0.0;
53 // Manually limit the field of view of the depth camera
54 double Z = depth_raw.at<cv::Vec3f>(i, j)[0] > 2.0f ? 0.0 : static_cast<double>(depth_raw.at<cv::Vec3f>(i, j)[0]);
56 size_t idx = static_cast<size_t>(i*depth_raw.cols + j);
57 pointcloud[idx].resize(3);
58 pointcloud[idx][0] = x*Z;
59 pointcloud[idx][1] = y*Z;
60 pointcloud[idx][2] = Z;
61 }
62 }
63
64 // Read ground truth
65 ss.str("");
66 ss << input_directory << "/camera_poses/Camera_%03d.txt";
67 sprintf(buffer, ss.str().c_str(), cpt);
68 std::string filename_pose = buffer;
69
70 std::ifstream f_pose;
71 f_pose.open(filename_pose.c_str()); // .c_str() to keep compat when c++11 not available
72 if (!f_pose.is_open()) {
73 std::cerr << "Cannot read: " << filename_pose << std::endl;
74 return false;
75 }
76
77 cMo_ground_truth.load(f_pose);
78
79 return true;
80}
81}
82
83int main(int argc, char *argv[])
84{
85 std::string input_directory = "."; // location of the data (images, depth maps, camera poses)
86 std::string config_color = "teabox.xml", config_depth = "teabox_depth.xml";
87 std::string model_color = "teabox.cao", model_depth = "teabox.cao";
88 std::string init_file = "teabox.init";
89 std::string extrinsic_file = "depth_M_color.txt";
90 unsigned int first_frame_index = 1;
91 bool disable_depth = false;
92 bool display_ground_truth = false;
93 bool click = false;
94
95 for (int i = 1; i < argc; i++) {
96 if (std::string(argv[i]) == "--input_directory" && i + 1 < argc) {
97 input_directory = std::string(argv[i + 1]);
98 } else if (std::string(argv[i]) == "--config_color" && i + 1 < argc) {
99 config_color = std::string(argv[i + 1]);
100 } else if (std::string(argv[i]) == "--config_depth" && i + 1 < argc) {
101 config_depth = std::string(argv[i + 1]);
102 } else if (std::string(argv[i]) == "--model_color" && i + 1 < argc) {
103 model_color = std::string(argv[i + 1]);
104 } else if (std::string(argv[i]) == "--model_depth" && i + 1 < argc) {
105 model_depth = std::string(argv[i + 1]);
106 } else if (std::string(argv[i]) == "--init_file" && i + 1 < argc) {
107 init_file = std::string(argv[i + 1]);
108 } else if (std::string(argv[i]) == "--extrinsics" && i + 1 < argc) {
109 extrinsic_file = std::string(argv[i + 1]);
110 } else if (std::string(argv[i]) == "--disable_depth") {
111 disable_depth = true;
112 } else if (std::string(argv[i]) == "--display_ground_truth") {
113 display_ground_truth = true;
114 } else if (std::string(argv[i]) == "--click") {
115 click = true;
116 } else if (std::string(argv[i]) == "--first_frame_index" && i+1 < argc) {
117 first_frame_index = static_cast<unsigned int>(atoi(argv[i+1]));
118 }
119 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
120 std::cout << "Usage: \n" << argv[0] << " [--input_directory <data directory> (default: .)]"
121 " [--config_color <object.xml> (default: teabox.xml)] [--config_depth <object.xml> (default: teabox_depth.xml)]"
122 " [--model_color <object.cao> (default: teabox.cao)] [--model_depth <object.cao> (default: teabox.cao)]"
123 " [--init_file <object.init> (default: teabox.init)]"
124 " [--extrinsics <depth to color transformation> (default: depth_M_color.txt)] [--disable_depth]"
125 " [--display_ground_truth] [--click] [--first_frame_index <index> (default: 1)]" << std::endl;
126 return EXIT_SUCCESS;
127 }
128 }
129
130 std::cout << "input_directory: " << input_directory << std::endl;
131 std::cout << "config_color: " << config_color << std::endl;
132 std::cout << "config_depth: " << config_depth << std::endl;
133 std::cout << "model_color: " << model_color << std::endl;
134 std::cout << "model_depth: " << model_depth << std::endl;
135 std::cout << "init_file: " << model_depth << std::endl;
136 std::cout << "extrinsic_file: " << extrinsic_file << std::endl;
137 std::cout << "first_frame_index: " << first_frame_index << std::endl;
138 std::cout << "disable_depth: " << disable_depth << std::endl;
139 std::cout << "display_ground_truth: " << display_ground_truth << std::endl;
140 std::cout << "click: " << click << std::endl;
141
142 std::vector<int> tracker_types;
144 if (!disable_depth)
145 tracker_types.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER);
146
147 vpMbGenericTracker tracker(tracker_types);
148 if (!disable_depth)
149 tracker.loadConfigFile(config_color, config_depth);
150 else
151 tracker.loadConfigFile(config_color);
152 tracker.loadModel(model_color);
153 vpCameraParameters cam_color, cam_depth;
154 if (!disable_depth)
155 tracker.getCameraParameters(cam_color, cam_depth);
156 else
157 tracker.getCameraParameters(cam_color);
158 tracker.setDisplayFeatures(true);
159 std::cout << "cam_color:\n" << cam_color << std::endl;
160 std::cout << "cam_depth:\n" << cam_depth << std::endl;
161
162 vpImage<uint16_t> I_depth_raw;
163 vpImage<unsigned char> I, I_depth;
164 unsigned int depth_width = 0, depth_height = 0;
165 std::vector<vpColVector> pointcloud;
166 vpHomogeneousMatrix cMo_ground_truth;
167
168 unsigned int frame_cpt = first_frame_index;
169 read_data(frame_cpt, input_directory, I, I_depth_raw, depth_width, depth_height, pointcloud, cam_depth, cMo_ground_truth);
170 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
171
172#if defined(VISP_HAVE_X11)
173 vpDisplayX d1, d2;
174#elif defined(VISP_HAVE_GDI)
175 vpDisplayGDI d1, d2;
176#else
177 vpDisplayOpenCV d1, d2;
178#endif
179
180 d1.init(I, 0, 0, "Color image");
181 d2.init(I_depth, static_cast<int>(I.getWidth()), 0, "Depth image");
182
183 vpHomogeneousMatrix depthMcolor;
184 if (!disable_depth) {
185 std::ifstream f_extrinsics;
186 f_extrinsics.open(extrinsic_file.c_str()); // .c_str() to keep compat when c++11 not available
187
188 depthMcolor.load(f_extrinsics);
189 tracker.setCameraTransformationMatrix("Camera2", depthMcolor);
190 std::cout << "depthMcolor:\n" << depthMcolor << std::endl;
191 }
192
193 if (display_ground_truth) {
194 tracker.initFromPose(I, cMo_ground_truth); //I and I_depth must be the same size when using depth features!
195 } else
196 tracker.initClick(I, init_file, true); //I and I_depth must be the same size when using depth features!
197
198 try {
199 bool quit = false;
200 while (!quit && read_data(frame_cpt, input_directory, I, I_depth_raw, depth_width, depth_height, pointcloud, cam_depth, cMo_ground_truth)) {
201 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
203 vpDisplay::display(I_depth);
204
205 if (display_ground_truth) {
206 tracker.initFromPose(I, cMo_ground_truth); //I and I_depth must be the same size when using depth features!
207 } else {
208 if (!disable_depth) {
209 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
210 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
211 std::map<std::string, unsigned int> mapOfPointCloudWidths;
212 std::map<std::string, unsigned int> mapOfPointCloudHeights;
213
214 mapOfImages["Camera1"] = &I;
215 mapOfPointClouds["Camera2"] = &pointcloud;
216 mapOfPointCloudWidths["Camera2"] = depth_width;
217 mapOfPointCloudHeights["Camera2"] = depth_height;
218 tracker.track(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
219 } else {
220 tracker.track(I);
221 }
222 }
223
224 vpHomogeneousMatrix cMo = tracker.getPose();
225 std::cout << "\nFrame: " << frame_cpt << std::endl;
226 if (!display_ground_truth)
227 std::cout << "cMo:\n" << cMo << std::endl;
228 std::cout << "cMo ground truth:\n" << cMo_ground_truth << std::endl;
229 if (!disable_depth) {
230 tracker.display(I, I_depth, cMo, depthMcolor*cMo, cam_color, cam_depth, vpColor::red, 2);
231 vpDisplay::displayFrame(I_depth, depthMcolor*cMo, cam_depth, 0.05, vpColor::none, 2);
232 }
233 else {
234 tracker.display(I, cMo, cam_color, vpColor::red, 2);
235 }
236
237 vpDisplay::displayFrame(I, cMo, cam_color, 0.05, vpColor::none, 2);
238 std::ostringstream oss;
239 oss << "Frame: " << frame_cpt;
240 vpDisplay::displayText(I, 20, 20, oss.str(), vpColor::red);
241
242 if (!display_ground_truth) {
243 {
244 std::stringstream ss;
245 ss << "Nb features: " << tracker.getError().size();
246 vpDisplay::displayText(I, I.getHeight() - 50, 20, ss.str(), vpColor::red);
247 }
248 {
249 std::stringstream ss;
250 ss << "Features: edges " << tracker.getNbFeaturesEdge()
251 << ", klt " << tracker.getNbFeaturesKlt()
252 << ", depth " << tracker.getNbFeaturesDepthDense();
253 vpDisplay::displayText(I, I.getHeight() - 30, 20, ss.str(), vpColor::red);
254 }
255 }
256
258 vpDisplay::flush(I_depth);
259
261 if (vpDisplay::getClick(I, button, click)) {
262 switch (button) {
264 quit = !click;
265 break;
267 click = !click;
268 break;
269
270 default:
271 break;
272 }
273 }
274
275 frame_cpt++;
276 }
277
278 vpDisplay::displayText(I, 40, 20, "Click to quit.", vpColor::red);
281 } catch (std::exception& e) {
282 std::cerr << "Catch exception: " << e.what() << std::endl;
283 }
284
285 return EXIT_SUCCESS;
286}
287#else
288int main()
289{
290 std::cout << "To run this tutorial, ViSP should be built with OpenCV and pugixml libraries." << std::endl;
291 return EXIT_SUCCESS;
292}
293#endif
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
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 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.
void load(std::ifstream &f)
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
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 bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:802
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)