Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-pose-from-planar-object.cpp
1
2
3// Core
4#include <visp3/core/vpColorDepthConversion.h>
5#include <visp3/core/vpIoTools.h>
6#include <visp3/core/vpMeterPixelConversion.h>
7#include <visp3/core/vpXmlParserCamera.h>
8
9// Vision
10#include <visp3/vision/vpPlaneEstimation.h>
11#include <visp3/vision/vpPose.h>
12
13// IO
14#include <visp3/io/vpImageIo.h>
15
16// GUI
17#include <visp3/gui/vpDisplayD3D.h>
18#include <visp3/gui/vpDisplayGDI.h>
19#include <visp3/gui/vpDisplayGTK.h>
20#include <visp3/gui/vpDisplayOpenCV.h>
21#include <visp3/gui/vpDisplayX.h>
22
23#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
24 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) && \
25 defined(VISP_HAVE_DISPLAY)
26
27// Local helper
28namespace
29{
30// Display
31#if defined(VISP_HAVE_X11)
32using Display = vpDisplayX;
33#elif defined(VISP_HAVE_GDI)
34using Display = vpDisplayGDI;
35#elif defined(VISP_HAVE_OPENCV)
36using Display = vpDisplayOpenCV;
37#elif defined(VISP_HAVE_GTK)
38using Display = vpDisplayGTK;
39#elif defined(VISP_HAVE_D3D9)
40using Display = vpDisplayD3D;
41#endif
42
43constexpr auto DispScaleType{vpDisplay::SCALE_AUTO};
44
45// Model
46constexpr auto ModelCommentHeader{"#"};
47constexpr auto ModelKeypointsHeader{"Keypoints"};
48constexpr auto ModelBoundsHeader{"Bounds"};
49constexpr auto ModelDataHeader{"data:"};
50
51// Depth
52constexpr auto DepthScale{0.001};
53} // namespace
54
55#ifndef DOXYGEN_SHOULD_SKIP_THIS
56class Model
57{
58public:
59 Model() = delete;
60 ~Model() = default;
61 Model(const Model &) = default;
62 Model(Model &&) = default;
63 Model &operator=(const Model &) = default;
64 Model &operator=(Model &&) = default;
65
66 explicit Model(const std::string &model_filename);
67
68public:
69 using Id = unsigned long int;
70 static inline std::string to_string(const Id &id) { return std::to_string(id); };
71
72 std::map<Id, vpPoint> keypoints(const vpHomogeneousMatrix &cMo = {}) const;
73 std::map<Id, vpPoint> bounds(const vpHomogeneousMatrix &cMo = {}) const;
74
75private:
76 std::map<Id, vpPoint> m_keypoints{};
77 std::map<Id, vpPoint> m_bounds{};
78};
79
80inline Model::Model(const std::string &model_filename)
81{
82 std::fstream file;
83 file.open(model_filename.c_str(), std::fstream::in);
84
85 std::string line{}, subs{};
86 bool in_model_bounds{false};
87 bool in_model_keypoints{false};
88 unsigned int data_curr_line{0};
89 unsigned int data_line_start_pos{0};
90
91 auto reset = [&]() {
92 in_model_bounds = false;
93 in_model_keypoints = false;
94 data_curr_line = 0;
95 data_line_start_pos = 0;
96 };
97
98 while (getline(file, line)) {
99 if (line.substr(0, std::string(ModelCommentHeader).size()) == ModelCommentHeader || line == ModelDataHeader) {
100 continue;
101 } else if (line == ModelBoundsHeader) {
102 reset();
103 in_model_bounds = true;
104 continue;
105 } else if (line == ModelKeypointsHeader) {
106 reset();
107 in_model_keypoints = true;
108 continue;
109 }
110
111 if (data_curr_line == 0) {
112 // Get indentation level which is common to all lines
113 data_line_start_pos = (unsigned int)line.find("[") + 1;
114 }
115
116 try {
117 std::stringstream ss(line.substr(data_line_start_pos, line.find("]") - data_line_start_pos));
118 unsigned int data_on_curr_line = 0;
119 vpColVector oXYZ({0, 0, 0, 1});
120 while (getline(ss, subs, ',')) {
121 oXYZ[data_on_curr_line++] = std::atof(subs.c_str());
122 }
123 if (in_model_bounds) {
124 m_bounds.try_emplace(data_curr_line, oXYZ[0], oXYZ[1], oXYZ[2]);
125 } else if (in_model_keypoints) {
126 m_keypoints.try_emplace(data_curr_line, oXYZ[0], oXYZ[1], oXYZ[2]);
127 }
128 data_curr_line++;
129 } catch (...) {
130 // Line is empty or incomplete. We skeep it
131 }
132 }
133
134 file.close();
135}
136
137inline std::map<Model::Id, vpPoint> Model::bounds(const vpHomogeneousMatrix &cMo) const
138{
139 auto bounds = m_bounds;
140 std::for_each(begin(bounds), end(bounds), [&cMo](auto &bound) { bound.second.project(cMo); });
141
142 return bounds;
143}
144
145inline std::map<Model::Id, vpPoint> Model::keypoints(const vpHomogeneousMatrix &cMo) const
146{
147 auto keypoints = m_keypoints;
148 std::for_each(begin(keypoints), end(keypoints), [&cMo](auto &keypoint) { keypoint.second.project(cMo); });
149
150 return keypoints;
151}
152
153std::ostream &operator<<(std::ostream &os, const Model &model)
154{
155 os << "-Bounds:" << std::endl;
156 for (const auto &[id, bound] : model.bounds()) {
157 // clang-format off
158 os << std::setw(4) << std::setfill(' ') << id << ": "
159 << std::setw(6) << std::setfill(' ') << bound.get_X() << ", "
160 << std::setw(6) << std::setfill(' ') << bound.get_Y() << ", "
161 << std::setw(6) << std::setfill(' ') << bound.get_Z() << std::endl;
162 // clang-format on
163 }
164
165 os << "-Keypoints:" << std::endl;
166 for (const auto &[id, keypoint] : model.keypoints()) {
167 // clang-format off
168 os << std::setw(4) << std::setfill(' ') << id << ": "
169 << std::setw(6) << std::setfill(' ') << keypoint.get_X() << ", "
170 << std::setw(6) << std::setfill(' ') << keypoint.get_Y() << ", "
171 << std::setw(6) << std::setfill(' ') << keypoint.get_Z() << std::endl;
172 // clang-format on
173 }
174
175 return os;
176}
177
179readData(const std::string &input_directory, const unsigned int cpt = 0)
180{
181 char buffer[FILENAME_MAX];
182 std::stringstream ss;
183 ss << input_directory << "/color_image_%04d.jpg";
184 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
185 const std::string filename_color = buffer;
186
187 ss.str("");
188 ss << input_directory << "/depth_image_%04d.bin";
189 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
190 const std::string filename_depth = buffer;
191
192 // Read color
193 vpImage<vpRGBa> I_color{};
194 vpImageIo::read(I_color, filename_color);
195
196 // Read raw depth
197 vpImage<uint16_t> I_depth_raw{};
198 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
199 if (file_depth.is_open()) {
200 unsigned int height = 0, width = 0;
201 vpIoTools::readBinaryValueLE(file_depth, height);
202 vpIoTools::readBinaryValueLE(file_depth, width);
203 I_depth_raw.resize(height, width);
204
205 for (auto i = 0u; i < height; i++) {
206 for (auto j = 0u; j < width; j++) {
207 vpIoTools::readBinaryValueLE(file_depth, I_depth_raw[i][j]);
208 }
209 }
210 }
211
212 // Read camera parameters (intrinsics)
213 ss.str("");
214 ss << input_directory << "/camera.xml";
215
216 vpXmlParserCamera parser{};
217 vpCameraParameters color_param{}, depth_param{};
218 parser.parse(color_param, ss.str(), "color_camera", vpCameraParameters::perspectiveProjWithDistortion);
219 parser.parse(depth_param, ss.str(), "depth_camera", vpCameraParameters::perspectiveProjWithDistortion);
220
221 // Read camera parameters (extrinsics)
222 ss.str("");
223 ss << input_directory << "/depth_M_color.txt";
224 std::ifstream file_depth_M_color(ss.str().c_str(), std::ios::in | std::ios::binary);
225
226 vpHomogeneousMatrix depth_M_color{};
227 depth_M_color.load(file_depth_M_color);
228
229 return {I_color, I_depth_raw, color_param, depth_param, depth_M_color};
230}
231
232std::vector<vpImagePoint> getRoiFromUser(vpImage<vpRGBa> color_img)
233{
234 // Init displays
235 Display disp_color(color_img, 0, 0, "Roi bounds", DispScaleType);
236 disp_color.display(color_img);
237 disp_color.flush(color_img);
238
239 std::vector<vpImagePoint> v_ip{};
240 do {
241 // Prepare display
242 disp_color.display(color_img);
243 auto disp_lane{0};
244
245 vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Select point along the d435 box boundary", vpColor::green);
246 vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Left click to select a point", vpColor::green);
247 vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Middle click to remove the last point", vpColor::green);
248 vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Right click to finish/quit", vpColor::green);
249
250 // Display already selected points
251 for (auto j = 0u; j < v_ip.size(); j++) {
252 vpDisplay::displayCross(color_img, v_ip.at(j), 15, vpColor::green);
253 vpDisplay::displayText(color_img, v_ip.at(j) + vpImagePoint(10, 10), std::to_string(j), vpColor::green);
254 }
255 disp_color.flush(color_img);
256
257 // Wait for new point
258 vpImagePoint ip{};
260 vpDisplay::getClick(color_img, ip, button, true);
261
262 switch (button) {
264 if (v_ip.size() > 0) {
265 v_ip.erase(std::prev(end(v_ip)));
266 }
267 break;
268 }
269
271 return v_ip;
272 }
273
274 default: {
275 v_ip.push_back(ip);
276 break;
277 }
278 }
279
280 } while (1);
281}
282
283std::map<Model::Id, vpImagePoint> getKeypointsFromUser(vpImage<vpRGBa> color_img, const Model &model,
284 const std::string &parent_data)
285{
286 // Init displays
287 Display disp_color(color_img, 0, 0, "Keypoints", DispScaleType);
288 disp_color.display(color_img);
289 disp_color.flush(color_img);
290
291 vpImage<vpRGBa> I_help{};
292 vpImageIo::read(I_help, parent_data + "/data/d435_box_keypoints_user_helper.png");
293 Display disp_help(I_help, disp_color.getWindowXPosition() + color_img.getWidth(), disp_color.getWindowYPosition(),
294 "Keypoints [help]", DispScaleType);
295 disp_help.display(I_help);
296 disp_help.flush(I_help);
297
298 std::map<Model::Id, vpImagePoint> keypoints{};
299 // - The next line produces an internal compiler error with Visual Studio 2017:
300 // tutorial-pose-from-planar-object.cpp(304): fatal error C1001: An internal error has occurred in the compiler.
301 // [C:\projects\visp\build\tutorial\computer-vision\tutorial-pose-from-planar-object.vcxproj] (compiler file
302 // 'd:\agent\_work\8\s\src\vctools\compiler\cxxfe\sl\p1\cxx\grammar.y', line 12721)
303 // To work around this problem, try simplifying or changing the program near the locations listed above.
304 // Please choose the Technical Support command on the Visual C++
305 // Help menu, or open the Technical Support help file for more information
306 // - Note that the next line builds with Visual Studio 2022.
307 // for ([[maybe_unused]] const auto &[id, _] : model.keypoints()) {
308 for (const auto &[id, ip_unused] : model.keypoints()) {
309 (void)ip_unused;
310 // Prepare display
311 disp_color.display(color_img);
312 auto disp_lane{0};
313
314 vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Select the keypoints " + Model::to_string(id),
316 vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Click to select a point", vpColor::green);
317
318 // Display already selected points
319 for (const auto &[id, keypoint] : keypoints) {
320 vpDisplay::displayCross(color_img, keypoint, 15, vpColor::green);
321 vpDisplay::displayText(color_img, keypoint + vpImagePoint(10, 10), Model::to_string(id), vpColor::green);
322 }
323 disp_color.flush(color_img);
324
325 // Wait for new point
326 vpImagePoint ip{};
327 vpDisplay::getClick(color_img, ip, true);
328 keypoints.try_emplace(id, ip);
329 }
330
331 return keypoints;
332}
333#endif // DOXYGEN_SHOULD_SKIP_THIS
334#endif // #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >=
335// VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) && defined(VISP_HAVE_DISPLAY)
336
337int main(int, char *argv[])
338{
339#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
340 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
341
342#if defined(VISP_HAVE_DISPLAY)
343
344 // Get prior data
346 auto [color_img, depth_raw, color_param, depth_param, depth_M_color] =
347 readData(vpIoTools::getParent(argv[0]) + "/data/d435_not_align_depth", 0);
348 const auto model = Model(vpIoTools::getParent(argv[0]) + "/data/d435_box.model");
350
351 std::cout << "color_param:" << std::endl << color_param << std::endl;
352 std::cout << "depth_param:" << std::endl << depth_param << std::endl;
353 std::cout << "depth_M_color:" << std::endl << depth_M_color << std::endl;
354 std::cout << std::endl << "Model:" << std::endl << model << std::endl;
355
356 // Init display
357 Display display_color(color_img, 0, 0, "Color", DispScaleType);
358 display_color.display(color_img);
359 display_color.flush(color_img);
360
361 vpImage<unsigned char> depth_img{};
362 vpImageConvert::createDepthHistogram(depth_raw, depth_img);
363 Display display_depth(depth_img, display_color.getWindowXPosition() + display_color.getWidth(), 0, "Depth",
364 DispScaleType);
365 display_depth.display(depth_img);
366 display_depth.flush(depth_img);
367
368 // Ask roi for plane estimation
370 vpPolygon roi_color_img{};
371 roi_color_img.buildFrom(getRoiFromUser(color_img), true);
372
373 std::vector<vpImagePoint> roi_corners_depth_img{};
374 std::transform(
375 cbegin(roi_color_img.getCorners()), cend(roi_color_img.getCorners()), std::back_inserter(roi_corners_depth_img),
376 std::bind((vpImagePoint(*)(const vpImage<uint16_t> &, double, double, double, const vpCameraParameters &,
378 const vpImagePoint &)) &
380 depth_raw, DepthScale, 0.1, 0.6, depth_param, color_param, depth_M_color.inverse(), depth_M_color,
381 std::placeholders::_1));
382 const vpPolygon roi_depth_img{roi_corners_depth_img};
384
385 vpDisplay::displayPolygon(depth_img, roi_depth_img.getCorners(), vpColor::green);
386 display_depth.flush(depth_img);
387
388 // Estimate the plane
389 vpImage<vpRGBa> heat_map{};
391 const auto obj_plane_in_depth =
392 vpPlaneEstimation::estimatePlane(depth_raw, DepthScale, depth_param, roi_depth_img, 1000, heat_map);
393 if (!obj_plane_in_depth) {
394 return EXIT_FAILURE;
395 }
396
397 // Get the plane in color frame
398 auto obj_plane_in_color = *obj_plane_in_depth;
399 obj_plane_in_color.changeFrame(depth_M_color.inverse());
401
402 Display display_heat_map(heat_map, display_depth.getWindowXPosition(),
403 display_depth.getWindowYPosition() + display_depth.getHeight(), "Plane Estimation Heat map",
404 DispScaleType);
405 display_heat_map.display(heat_map);
406 display_heat_map.flush(heat_map);
407
408 // Ask user to click on keypoints
409 const auto keypoint_color_img = getKeypointsFromUser(color_img, model, vpIoTools::getParent(argv[0]));
410
412 const auto cMo = vpPose::computePlanarObjectPoseWithAtLeast3Points(obj_plane_in_color, model.keypoints(),
413 keypoint_color_img, color_param);
414 if (!cMo) {
415 return EXIT_FAILURE;
416 }
418
419 // Display the model
420 std::vector<vpImagePoint> d435_box_bound{};
421 // - The next line produces an internal compiler error with Visual Studio 2017:
422 // tutorial-pose-from-planar-object.cpp(428): fatal error C1001: An internal error has occurred in the compiler.
423 // [C:\projects\visp\build\tutorial\computer-vision\tutorial-pose-from-planar-object.vcxproj] (compiler file
424 // 'd:\agent\_work\8\s\src\vctools\compiler\cxxfe\sl\p1\cxx\grammar.y', line 12721)
425 // To work around this problem, try simplifying or changing the program near the locations listed above.
426 // Please choose the Technical Support command on the Visual C++
427 // Help menu, or open the Technical Support help file for more information
428 // - Note that the next line builds with Visual Studio 2022.
429 //
430 // for ([[maybe_unused]] const auto &[_, bound] : model.bounds(*cMo)) {
431 for (const auto &[id_unused, bound] : model.bounds(*cMo)) {
432 (void)id_unused;
433 vpImagePoint ip{};
434 vpMeterPixelConversion::convertPoint(color_param, bound.get_x(), bound.get_y(), ip);
435 d435_box_bound.push_back(ip);
436 }
437 vpDisplay::displayPolygon(color_img, d435_box_bound, vpColor::blue);
438
439 for (const auto &[id, keypoint] : model.keypoints(*cMo)) {
440 vpImagePoint ip{};
441 vpMeterPixelConversion::convertPoint(color_param, keypoint.get_x(), keypoint.get_y(), ip);
442 vpDisplay::displayCross(color_img, ip, 15, vpColor::orange);
443 vpDisplay::displayText(color_img, ip + vpImagePoint(10, 10), Model::to_string(id), vpColor::orange);
444 }
445 vpDisplay::flush(color_img);
446
447 // Display the frame
448 vpDisplay::displayFrame(color_img, *cMo, color_param, 0.05, vpColor::none, 3);
449
450 // Wait before exiting
451 auto disp_lane{0};
452 vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "D435 box boundary [from model]", vpColor::blue);
453 vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Keypoints [from model]", vpColor::orange);
454 vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Click to quit", vpColor::green);
455 vpDisplay::flush(color_img);
456
457 vpDisplay::getClick(color_img);
458
459#else
460 (void)argv;
461 std::cout << "There is no display available to run this tutorial." << std::endl;
462#endif // defined(VISP_HAVE_DISPLAY)
463#else
464 (void)argv;
465 std::cout << "c++17 should be enabled to run this tutorial." << std::endl;
466#endif // (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >=
467 // VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
468
469 return EXIT_SUCCESS;
470}
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
static vpImagePoint projectColorToDepth(const vpImage< uint16_t > &I_depth, double depth_scale, double depth_min, double depth_max, const vpCameraParameters &depth_intrinsics, const vpCameraParameters &color_intrinsics, const vpHomogeneousMatrix &color_M_depth, const vpHomogeneousMatrix &depth_M_color, const vpImagePoint &from_pixel)
static const vpColor none
Definition vpColor.h:223
static const vpColor orange
Definition vpColor.h:221
static const vpColor blue
Definition vpColor.h:217
static const vpColor green
Definition vpColor.h:214
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
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
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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 displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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)
static void displayPolygon(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &vip, const vpColor &color, unsigned int thickness=1, bool closed=true)
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
static void readBinaryValueLE(std::ifstream &file, int16_t &short_value)
static std::string getParent(const std::string &pathname)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static std::optional< vpPlane > estimatePlane(const vpImage< uint16_t > &I_depth_raw, double depth_scale, const vpCameraParameters &depth_intrinsics, const vpPolygon &roi, const unsigned int avg_nb_of_pts_to_estimate=500, std::optional< std::reference_wrapper< vpImage< vpRGBa > > > heat_map={})
Defines a generic 2D polygon.
Definition vpPolygon.h:97
void buildFrom(const std::vector< vpImagePoint > &corners, const bool create_convex_hull=false)
static std::optional< vpHomogeneousMatrix > computePlanarObjectPoseWithAtLeast3Points(const vpPlane &plane_in_camera_frame, const std::map< DataId, vpPoint > &pts, const std::map< DataId, vpImagePoint > &ips, const vpCameraParameters &camera_intrinsics, std::optional< vpHomogeneousMatrix > cMo_init=std::nullopt, bool enable_vvs=true)
Definition vpPose.h:388
XML parser to load and save intrinsic camera parameters.