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