Visual Servoing Platform  version 3.5.1 under development (2022-07-06)
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 #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
28 namespace
29 {
30 // Display
31 #if defined(VISP_HAVE_X11)
32 using Display = vpDisplayX;
33 #elif defined(VISP_HAVE_GDI)
34 using Display = vpDisplayGDI;
35 #elif defined(VISP_HAVE_OPENCV)
36 using Display = vpDisplayOpenCV;
37 #elif defined(VISP_HAVE_GTK)
38 using Display = vpDisplayGTK;
39 #elif defined(VISP_HAVE_D3D9)
40 using Display = vpDisplayD3D;
41 #endif
42 
43 constexpr auto DispScaleType{vpDisplay::SCALE_AUTO};
44 
45 // Model
46 constexpr auto ModelCommentHeader{"#"};
47 constexpr auto ModelKeypointsHeader{"Keypoints"};
48 constexpr auto ModelBoundsHeader{"Bounds"};
49 constexpr auto ModelDataHeader{"data:"};
50 
51 // Depth
52 constexpr auto DepthScale{0.001};
53 } // namespace
54 
55 #ifndef DOXYGEN_SHOULD_SKIP_THIS
56 class Model
57 {
58 public:
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 
68 public:
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 
75 private:
76  std::map<Id, vpPoint> m_keypoints{};
77  std::map<Id, vpPoint> m_bounds{};
78 };
79 
80 inline 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 
137 inline 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 
145 inline 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 
153 std::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 
179 readData(const std::string &input_directory, const unsigned int cpt = 0)
180 {
181  char buffer[256];
182  std::stringstream ss;
183  ss << input_directory << "/color_image_%04d.jpg";
184  sprintf(buffer, 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  sprintf(buffer, 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 
232 std::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) {
263  case vpMouseButton::button2: {
264  if (v_ip.size() > 0) {
265  v_ip.erase(std::prev(end(v_ip)));
266  }
267  break;
268  }
269 
270  case vpMouseButton::button3: {
271  return v_ip;
272  }
273 
274  default: {
275  v_ip.push_back(ip);
276  break;
277  }
278  }
279 
280  } while (1);
281 }
282 
283 std::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 
337 int 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 }
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:494
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
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:229
static const vpColor orange
Definition: vpColor.h:227
static const vpColor blue
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:220
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:107
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
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
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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 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))
@ SCALE_AUTO
Definition: vpDisplay.h:183
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:148
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
Definition of the vpImage class member functions.
Definition: vpImage.h:73
unsigned int getWidth() const
Definition: vpImage.h:246
static void readBinaryValueLE(std::ifstream &file, int16_t &short_value)
Definition: vpIoTools.cpp:2010
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:1620
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:106
void buildFrom(const std::vector< vpImagePoint > &corners, const bool create_convex_hull=false)
Definition: vpPolygon.cpp:198
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:386
XML parser to load and save intrinsic camera parameters.