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