Visual Servoing Platform  version 3.5.1 under development (2023-05-31)
vpPoseRGBD.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Pose computation from RGBD.
33  *
34  *****************************************************************************/
35 
36 #include <visp3/core/vpPixelMeterConversion.h>
37 #include <visp3/core/vpPlane.h>
38 #include <visp3/core/vpPolygon.h>
39 #include <visp3/core/vpRobust.h>
40 #include <visp3/vision/vpPose.h>
41 
42 namespace
43 {
44  // See also vpPlaneEstimation.cpp that implements the same functionaly in c++17
45 void estimatePlaneEquationSVD(const std::vector<double> &point_cloud_face, vpPlane &plane_equation_estimated,
46  vpColVector &centroid, double &normalized_weights)
47 {
48  unsigned int max_iter = 10;
49  double prev_error = 1e3;
50  double error = 1e3 - 1;
51  unsigned int nPoints = static_cast<unsigned int>(point_cloud_face.size() / 3);
52 
53  vpColVector weights(nPoints, 1.0);
54  vpColVector residues(nPoints);
55  vpMatrix M(nPoints, 3);
56  vpRobust tukey;
58  vpColVector normal;
59 
60  for (unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
61  if (iter != 0) {
62  tukey.MEstimator(vpRobust::TUKEY, residues, weights);
63  }
64 
65  // Compute centroid
66  double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
67  double total_w = 0.0;
68 
69  for (unsigned int i = 0; i < nPoints; i++) {
70  centroid_x += weights[i] * point_cloud_face[3 * i + 0];
71  centroid_y += weights[i] * point_cloud_face[3 * i + 1];
72  centroid_z += weights[i] * point_cloud_face[3 * i + 2];
73  total_w += weights[i];
74  }
75 
76  centroid_x /= total_w;
77  centroid_y /= total_w;
78  centroid_z /= total_w;
79 
80  // Minimization
81  for (unsigned int i = 0; i < nPoints; i++) {
82  M[static_cast<unsigned int>(i)][0] = weights[i] * (point_cloud_face[3 * i + 0] - centroid_x);
83  M[static_cast<unsigned int>(i)][1] = weights[i] * (point_cloud_face[3 * i + 1] - centroid_y);
84  M[static_cast<unsigned int>(i)][2] = weights[i] * (point_cloud_face[3 * i + 2] - centroid_z);
85  }
86 
87  vpColVector W;
88  vpMatrix V;
89  vpMatrix J = M.t() * M;
90  J.svd(W, V);
91 
92  double smallestSv = W[0];
93  unsigned int indexSmallestSv = 0;
94  for (unsigned int i = 1; i < W.size(); i++) {
95  if (W[i] < smallestSv) {
96  smallestSv = W[i];
97  indexSmallestSv = i;
98  }
99  }
100 
101  normal = V.getCol(indexSmallestSv);
102 
103  // Compute plane equation
104  double A = normal[0], B = normal[1], C = normal[2];
105  double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
106 
107  // Compute error points to estimated plane
108  prev_error = error;
109  error = 0.0;
110  for (unsigned int i = 0; i < nPoints; i++) {
111  residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
112  C * point_cloud_face[3 * i + 2] + D) /
113  sqrt(A * A + B * B + C * C);
114  error += weights[i] * residues[i];
115  }
116  error /= total_w;
117  }
118 
119  // Update final weights
120  tukey.MEstimator(vpRobust::TUKEY, residues, weights);
121 
122  // Update final centroid
123  centroid.resize(3, false);
124  double total_w = 0.0;
125 
126  for (unsigned int i = 0; i < nPoints; i++) {
127  centroid[0] += weights[i] * point_cloud_face[3 * i];
128  centroid[1] += weights[i] * point_cloud_face[3 * i + 1];
129  centroid[2] += weights[i] * point_cloud_face[3 * i + 2];
130  total_w += weights[i];
131  }
132 
133  centroid[0] /= total_w;
134  centroid[1] /= total_w;
135  centroid[2] /= total_w;
136 
137  // Compute final plane equation
138  double A = normal[0], B = normal[1], C = normal[2];
139  double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
140 
141  // Update final plane equation
142  plane_equation_estimated.setABCD(A, B, C, D);
143 
144  normalized_weights = total_w / nPoints;
145 }
146 
147 } // namespace
148 
169 bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
170  const vpCameraParameters &colorIntrinsics,
171  const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
172  double *confidence_index)
173 {
174  if (corners.size() != point3d.size()) {
176  "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
177  point3d.size(), corners.size()));
178  }
179  std::vector<vpPoint> pose_points;
180  if (confidence_index != NULL) {
181  *confidence_index = 0.0;
182  }
183 
184  for (size_t i = 0; i < point3d.size(); i++) {
185  pose_points.push_back(point3d[i]);
186  }
187 
188  vpPolygon polygon(corners);
189  vpRect bb = polygon.getBoundingBox();
190  unsigned int top = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getTop())));
191  unsigned int bottom =
192  static_cast<unsigned int>(std::min(static_cast<int>(depthMap.getHeight()) - 1, static_cast<int>(bb.getBottom())));
193  unsigned int left = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getLeft())));
194  unsigned int right =
195  static_cast<unsigned int>(std::min(static_cast<int>(depthMap.getWidth()) - 1, static_cast<int>(bb.getRight())));
196 
197  std::vector<double> points_3d;
198  points_3d.reserve((bottom - top) * (right - left));
199  for (unsigned int idx_i = top; idx_i < bottom; idx_i++) {
200  for (unsigned int idx_j = left; idx_j < right; idx_j++) {
201  vpImagePoint imPt(idx_i, idx_j);
202  if (depthMap[idx_i][idx_j] > 0 && polygon.isInside(imPt)) {
203  double x = 0, y = 0;
204  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
205  double Z = depthMap[idx_i][idx_j];
206  points_3d.push_back(x * Z);
207  points_3d.push_back(y * Z);
208  points_3d.push_back(Z);
209  }
210  }
211  }
212 
213  unsigned int nb_points_3d = static_cast<unsigned int>(points_3d.size() / 3);
214 
215  if (nb_points_3d > 4) {
216  std::vector<vpPoint> p, q;
217 
218  // Plane equation
219  vpPlane plane_equation;
220  vpColVector centroid;
221  double normalized_weights = 0;
222  estimatePlaneEquationSVD(points_3d, plane_equation, centroid, normalized_weights);
223 
224  for (size_t j = 0; j < corners.size(); j++) {
225  const vpImagePoint &imPt = corners[j];
226  double x = 0, y = 0;
227  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
228  double Z = plane_equation.computeZ(x, y);
229  if (Z < 0) {
230  Z = -Z;
231  }
232  p.push_back(vpPoint(x * Z, y * Z, Z));
233 
234  pose_points[j].set_x(x);
235  pose_points[j].set_y(y);
236  }
237 
238  for (size_t i = 0; i < point3d.size(); i++) {
239  q.push_back(point3d[i]);
240  }
241 
243 
244  if (cMo.isValid()) {
245  vpPose pose;
246  pose.addPoints(pose_points);
247  if (pose.computePose(vpPose::VIRTUAL_VS, cMo)) {
248  if (confidence_index != NULL) {
249  *confidence_index = std::min(1.0, normalized_weights * static_cast<double>(nb_points_3d) / polygon.getArea());
250  }
251  return true;
252  }
253  }
254  }
255 
256  return false;
257 }
258 
292  const std::vector<std::vector<vpImagePoint> > &corners,
293  const vpCameraParameters &colorIntrinsics,
294  const std::vector<std::vector<vpPoint> > &point3d,
295  vpHomogeneousMatrix &cMo, double *confidence_index, bool coplanar_points)
296 {
297 
298  if (corners.size() != point3d.size()) {
300  "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
301  point3d.size(), corners.size()));
302  }
303  std::vector<vpPoint> pose_points;
304  if (confidence_index != NULL) {
305  *confidence_index = 0.0;
306  }
307 
308  for (size_t i = 0; i < point3d.size(); i++) {
309  std::vector<vpPoint> tagPoint3d = point3d[i];
310  for (size_t j = 0; j < tagPoint3d.size(); j++) {
311  pose_points.push_back(tagPoint3d[j]);
312  }
313  }
314 
315  // Total area of the polygon to estimate confidence
316  double totalArea = 0.0;
317 
318  // If coplanar is true, the tags_points_3d will be used to compute one plane
319  std::vector<double> tag_points_3d;
320 
321  // Otherwise the vector of planes will be used to compute each plane for each vector
322  std::vector<std::vector<double> > tag_points_3d_nonplanar;
323  size_t nb_points_3d_non_planar = 0;
324 
325  // Loop through each object, compute 3d point cloud of each
326  for (size_t i = 0; i < corners.size(); i++) {
327  std::vector<double> points_3d;
328  vpPolygon polygon(corners[i]);
329  vpRect bb = polygon.getBoundingBox();
330 
331  // The area to calculate final confidence index should be total area of the tags
332  totalArea += polygon.getArea();
333 
334  unsigned int top = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getTop())));
335  unsigned int bottom = static_cast<unsigned int>(
336  std::min(static_cast<int>(depthMap.getHeight()) - 1, static_cast<int>(bb.getBottom())));
337  unsigned int left = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getLeft())));
338  unsigned int right =
339  static_cast<unsigned int>(std::min(static_cast<int>(depthMap.getWidth()) - 1, static_cast<int>(bb.getRight())));
340 
341  points_3d.reserve((bottom - top) * (right - left));
342  for (unsigned int idx_i = top; idx_i < bottom; idx_i++) {
343  for (unsigned int idx_j = left; idx_j < right; idx_j++) {
344  vpImagePoint imPt(idx_i, idx_j);
345  if (depthMap[idx_i][idx_j] > 0 && polygon.isInside(imPt)) {
346  double x = 0, y = 0;
347  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
348  double Z = depthMap[idx_i][idx_j];
349  points_3d.push_back(x * Z);
350  points_3d.push_back(y * Z);
351  points_3d.push_back(Z);
352  }
353  }
354  }
355 
356  // If coplanar_points is true, feed all 3d points to single vector
357  // Otherwise, each vector will hold 3d points for separate planes
358  if (coplanar_points) {
359  tag_points_3d.insert(tag_points_3d.end(), points_3d.begin(), points_3d.end());
360  } else {
361  tag_points_3d_nonplanar.push_back(points_3d);
362  nb_points_3d_non_planar += points_3d.size();
363  }
364  }
365 
366  size_t nb_points_3d = 0;
367 
368  if (coplanar_points) {
369  nb_points_3d = tag_points_3d.size() / 3;
370  } else {
371  nb_points_3d = nb_points_3d_non_planar / 3;
372  }
373 
374  if (nb_points_3d > 4) {
375  std::vector<vpPoint> p, q;
376 
377  // Plane equation
378  vpPlane plane_equation;
379  vpColVector centroid;
380  double normalized_weights = 0;
381 
382  if (coplanar_points) {
383  // If all objects are coplanar, use points insides tag_points_3d to estimate the plane
384  estimatePlaneEquationSVD(tag_points_3d, plane_equation, centroid, normalized_weights);
385  int count = 0;
386  for (size_t j = 0; j < corners.size(); j++) {
387  std::vector<vpImagePoint> tag_corner = corners[j];
388  for (size_t i = 0; i < tag_corner.size(); i++) {
389  const vpImagePoint &imPt = tag_corner[i];
390  double x = 0, y = 0;
391  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
392  double Z = plane_equation.computeZ(x, y);
393  std::cout << Z;
394  if (Z < 0) {
395  Z = -Z;
396  }
397  p.push_back(vpPoint(x * Z, y * Z, Z));
398  pose_points[count].set_x(x);
399  pose_points[count].set_y(y);
400  count++;
401  }
402  }
403  } else {
404  // If the tags is not coplanar, estimate the plane for each tags
405  size_t count = 0;
406 
407  for (size_t k = 0; k < tag_points_3d_nonplanar.size(); k++) {
408  std::vector<double> rec_points_3d = tag_points_3d_nonplanar[k];
409  double tag_normalized_weights = 0;
410 
411  if (rec_points_3d.size() >= 9) {
412  // The array must has at least 3 points for the function estimatePlaneEquationSVD not to crash
413  estimatePlaneEquationSVD(rec_points_3d, plane_equation, centroid, tag_normalized_weights);
414  normalized_weights += tag_normalized_weights;
415 
416  // Get the 2d points of the tag the plane just recomputed
417  std::vector<vpImagePoint> tag_corner = corners[k];
418 
419  for (size_t i = 0; i < tag_corner.size(); i++) {
420  const vpImagePoint &imPt = tag_corner[i];
421  double x = 0, y = 0;
422  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
423  double Z = plane_equation.computeZ(x, y);
424 
425  if (Z < 0) {
426  Z = -Z;
427  }
428  p.push_back(vpPoint(x * Z, y * Z, Z));
429  pose_points[count].set_x(x);
430  pose_points[count].set_y(y);
431  count++;
432  }
433  } else {
434  // Sometimes an object may do not have enough points registered due to small size or bad alignment btw depth
435  // and rgb. This behavior happens with Orbbec camera while Realsenses was fine. To prevent exception while
436  // computePose, skip recomputing the failed estimation tag's (4 point - corners)
437  count += corners[k].size();
438  }
439  }
440  normalized_weights = normalized_weights / tag_points_3d_nonplanar.size();
441  }
442 
443  for (size_t i = 0; i < point3d.size(); i++) {
444  std::vector<vpPoint> tagPoint3d = point3d[i];
445  // Sometimes an object may do not have enough points registered due to small size.
446  // The issue happens with Orbbec camera while Realsenses was fine.
447  // To prevent wrong estimation or exception (p and q sizes are differents),
448  // ignore the recomputer vector (tag_points_3d_nonplanar) when size = 0
449  if (coplanar_points) {
450  for (size_t j = 0; j < tagPoint3d.size(); j++) {
451  q.push_back(tagPoint3d[j]);
452  }
453  } else {
454  if (tag_points_3d_nonplanar[i].size() > 0) {
455  for (size_t j = 0; j < tagPoint3d.size(); j++) {
456  q.push_back(tagPoint3d[j]);
457  }
458  }
459  }
460  }
461 
462  // Due to the possibility of q's size might less than p's, check their size should be identical
463  if (p.size() == q.size()) {
465 
466  if (cMo.isValid()) {
467  vpPose pose;
468  pose.addPoints(pose_points);
469  if (pose.computePose(vpPose::VIRTUAL_VS, cMo)) {
470  if (confidence_index != NULL) {
471  *confidence_index = std::min(1.0, normalized_weights * static_cast<double>(nb_points_3d) / totalArea);
472  }
473  return true;
474  }
475  }
476  }
477  }
478  return false;
479 }
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:294
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:172
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:357
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of an homogeneous matrix and operations on such kind of matrices.
static vpHomogeneousMatrix compute3d3dTransformation(const std::vector< vpPoint > &p, const std::vector< vpPoint > &q)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
double get_u() const
Definition: vpImagePoint.h:143
double get_v() const
Definition: vpImagePoint.h:154
unsigned int getWidth() const
Definition: vpImage.h:247
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
Definition: vpImage.h:1359
unsigned int getHeight() const
Definition: vpImage.h:189
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2021
vpMatrix t() const
Definition: vpMatrix.cpp:462
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5170
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:59
double computeZ(double x, double y) const
Definition: vpPlane.cpp:235
void setABCD(double a, double b, double c, double d)
Definition: vpPlane.h:93
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
Defines a generic 2D polygon.
Definition: vpPolygon.h:106
vpRect getBoundingBox() const
Definition: vpPolygon.h:180
double getArea() const
Definition: vpPolygon.h:164
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:401
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:90
@ VIRTUAL_VS
Definition: vpPose.h:104
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:164
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:414
static bool computePlanarObjectPoseFromRGBD(const vpImage< float > &depthMap, const std::vector< vpImagePoint > &corners, const vpCameraParameters &colorIntrinsics, const std::vector< vpPoint > &point3d, vpHomogeneousMatrix &cMo, double *confidence_index=NULL)
Definition: vpPoseRGBD.cpp:169
Defines a rectangle in the plane.
Definition: vpRect.h:78
double getLeft() const
Definition: vpRect.h:172
double getRight() const
Definition: vpRect.h:178
double getBottom() const
Definition: vpRect.h:96
double getTop() const
Definition: vpRect.h:191
Contains an M-estimator and various influence function.
Definition: vpRobust.h:89
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:93
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161