Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpPoseRGBD.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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/vpPolygon.h>
38 #include <visp3/core/vpRobust.h>
39 #include <visp3/vision/vpPose.h>
40 
41 namespace
42 {
43 vpHomogeneousMatrix compute3d3dTransformation(const std::vector<vpPoint> &p, const std::vector<vpPoint> &q)
44 {
45  double N = static_cast<double>(p.size());
46 
47  vpColVector p_bar(3, 0.0);
48  vpColVector q_bar(3, 0.0);
49  for (size_t i = 0; i < p.size(); i++) {
50  for (unsigned int j = 0; j < 3; j++) {
51  p_bar[j] += p[i].oP[j];
52  q_bar[j] += q[i].oP[j];
53  }
54  }
55 
56  for (unsigned int j = 0; j < 3; j++) {
57  p_bar[j] /= N;
58  q_bar[j] /= N;
59  }
60 
61  vpMatrix pc(static_cast<unsigned int>(p.size()), 3);
62  vpMatrix qc(static_cast<unsigned int>(q.size()), 3);
63 
64  for (unsigned int i = 0; i < static_cast<unsigned int>(p.size()); i++) {
65  for (unsigned int j = 0; j < 3; j++) {
66  pc[i][j] = p[i].oP[j] - p_bar[j];
67  qc[i][j] = q[i].oP[j] - q_bar[j];
68  }
69  }
70 
71  vpMatrix pct_qc = pc.t() * qc;
72  vpMatrix U = pct_qc, V;
73  vpColVector W;
74  U.svd(W, V);
75 
76  vpMatrix Vt = V.t();
77  vpMatrix R = U * Vt;
78 
79  double det = R.det();
80  if (det < 0) {
81  Vt[2][0] *= -1;
82  Vt[2][1] *= -1;
83  Vt[2][2] *= -1;
84 
85  R = U * Vt;
86  }
87 
88  vpColVector t = p_bar - R * q_bar;
89 
91  for (unsigned int i = 0; i < 3; i++) {
92  for (unsigned int j = 0; j < 3; j++) {
93  cMo[i][j] = R[i][j];
94  }
95  cMo[i][3] = t[i];
96  }
97 
98  return cMo;
99 }
100 
101 void estimatePlaneEquationSVD(const std::vector<double> &point_cloud_face, vpColVector &plane_equation_estimated,
102  vpColVector &centroid, double &normalized_weights)
103 {
104  unsigned int max_iter = 10;
105  double prev_error = 1e3;
106  double error = 1e3 - 1;
107  unsigned int nPoints = static_cast<unsigned int>(point_cloud_face.size() / 3);
108 
109  vpColVector weights(nPoints, 1.0);
110  vpColVector residues(nPoints);
111  vpMatrix M(nPoints, 3);
112  vpRobust tukey;
113  tukey.setMinMedianAbsoluteDeviation(1e-4);
114  vpColVector normal;
115 
116  plane_equation_estimated.resize(4, false);
117  for (unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
118  if (iter != 0) {
119  tukey.MEstimator(vpRobust::TUKEY, residues, weights);
120  }
121 
122  // Compute centroid
123  double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
124  double total_w = 0.0;
125 
126  for (unsigned int i = 0; i < nPoints; i++) {
127  centroid_x += weights[i] * point_cloud_face[3 * i + 0];
128  centroid_y += weights[i] * point_cloud_face[3 * i + 1];
129  centroid_z += weights[i] * point_cloud_face[3 * i + 2];
130  total_w += weights[i];
131  }
132 
133  centroid_x /= total_w;
134  centroid_y /= total_w;
135  centroid_z /= total_w;
136 
137  // Minimization
138  for (unsigned int i = 0; i < nPoints; i++) {
139  M[static_cast<unsigned int>(i)][0] = weights[i] * (point_cloud_face[3 * i + 0] - centroid_x);
140  M[static_cast<unsigned int>(i)][1] = weights[i] * (point_cloud_face[3 * i + 1] - centroid_y);
141  M[static_cast<unsigned int>(i)][2] = weights[i] * (point_cloud_face[3 * i + 2] - centroid_z);
142  }
143 
144  vpColVector W;
145  vpMatrix V;
146  vpMatrix J = M.t() * M;
147  J.svd(W, V);
148 
149  double smallestSv = W[0];
150  unsigned int indexSmallestSv = 0;
151  for (unsigned int i = 1; i < W.size(); i++) {
152  if (W[i] < smallestSv) {
153  smallestSv = W[i];
154  indexSmallestSv = i;
155  }
156  }
157 
158  normal = V.getCol(indexSmallestSv);
159 
160  // Compute plane equation
161  double A = normal[0], B = normal[1], C = normal[2];
162  double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
163 
164  // Update plane equation
165  plane_equation_estimated[0] = A;
166  plane_equation_estimated[1] = B;
167  plane_equation_estimated[2] = C;
168  plane_equation_estimated[3] = D;
169 
170  // Compute error points to estimated plane
171  prev_error = error;
172  error = 0.0;
173  for (unsigned int i = 0; i < nPoints; i++) {
174  residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
175  C * point_cloud_face[3 * i + 2] + D) /
176  sqrt(A * A + B * B + C * C);
177  error += weights[i] * residues[i];
178  }
179  error /= total_w;
180  }
181 
182  // Update final weights
183  tukey.MEstimator(vpRobust::TUKEY, residues, weights);
184 
185  // Update final centroid
186  centroid.resize(3, false);
187  double total_w = 0.0;
188 
189  for (unsigned int i = 0; i < nPoints; i++) {
190  centroid[0] += weights[i] * point_cloud_face[3 * i];
191  centroid[1] += weights[i] * point_cloud_face[3 * i + 1];
192  centroid[2] += weights[i] * point_cloud_face[3 * i + 2];
193  total_w += weights[i];
194  }
195 
196  centroid[0] /= total_w;
197  centroid[1] /= total_w;
198  centroid[2] /= total_w;
199 
200  // Compute final plane equation
201  double A = normal[0], B = normal[1], C = normal[2];
202  double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
203 
204  // Update final plane equation
205  plane_equation_estimated[0] = A;
206  plane_equation_estimated[1] = B;
207  plane_equation_estimated[2] = C;
208  plane_equation_estimated[3] = D;
209 
210  normalized_weights = total_w / nPoints;
211 }
212 
213 double computeZMethod1(const vpColVector &plane_equation, double x, double y)
214 {
215  return -plane_equation[3] / (plane_equation[0] * x + plane_equation[1] * y + plane_equation[2]);
216 }
217 
218 bool validPose(const vpHomogeneousMatrix &cMo)
219 {
220  bool valid = true;
221 
222  for (unsigned int i = 0; i < cMo.getRows() && valid; i++) {
223  for (unsigned int j = 0; j < cMo.getCols() && valid; j++) {
224  if (vpMath::isNaN(cMo[i][j])) {
225  valid = false;
226  }
227  }
228  }
229 
230  return valid;
231 }
232 } // namespace
233 
254 bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
255  const vpCameraParameters &colorIntrinsics,
256  const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
257  double *confidence_index)
258 {
259  if (corners.size() != point3d.size()) {
261  "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
262  point3d.size(), corners.size()));
263  }
264  std::vector<vpPoint> pose_points;
265  if (confidence_index != NULL) {
266  *confidence_index = 0.0;
267  }
268 
269  for (size_t i = 0; i < point3d.size(); i++) {
270  pose_points.push_back(point3d[i]);
271  }
272 
273  vpPolygon polygon(corners);
274  vpRect bb = polygon.getBoundingBox();
275  unsigned int top = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getTop())));
276  unsigned int bottom =
277  static_cast<unsigned int>(std::min(static_cast<int>(depthMap.getHeight()) - 1, static_cast<int>(bb.getBottom())));
278  unsigned int left = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getLeft())));
279  unsigned int right =
280  static_cast<unsigned int>(std::min(static_cast<int>(depthMap.getWidth()) - 1, static_cast<int>(bb.getRight())));
281 
282  std::vector<double> points_3d;
283  points_3d.reserve((bottom - top) * (right - left));
284  for (unsigned int idx_i = top; idx_i < bottom; idx_i++) {
285  for (unsigned int idx_j = left; idx_j < right; idx_j++) {
286  vpImagePoint imPt(idx_i, idx_j);
287  if (depthMap[idx_i][idx_j] > 0 && polygon.isInside(imPt)) {
288  double x = 0, y = 0;
289  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
290  double Z = depthMap[idx_i][idx_j];
291  points_3d.push_back(x * Z);
292  points_3d.push_back(y * Z);
293  points_3d.push_back(Z);
294  }
295  }
296  }
297 
298  unsigned int nb_points_3d = static_cast<unsigned int>(points_3d.size() / 3);
299 
300  if (nb_points_3d > 4) {
301  std::vector<vpPoint> p, q;
302 
303  // Plane equation
304  vpColVector plane_equation, centroid;
305  double normalized_weights = 0;
306  estimatePlaneEquationSVD(points_3d, plane_equation, centroid, normalized_weights);
307 
308  for (size_t j = 0; j < corners.size(); j++) {
309  const vpImagePoint &imPt = corners[j];
310  double x = 0, y = 0;
311  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
312  double Z = computeZMethod1(plane_equation, x, y);
313  if (Z < 0) {
314  Z = -Z;
315  }
316  p.push_back(vpPoint(x * Z, y * Z, Z));
317 
318  pose_points[j].set_x(x);
319  pose_points[j].set_y(y);
320  }
321 
322  for (size_t i = 0; i < point3d.size(); i++) {
323  q.push_back(point3d[i]);
324  }
325 
326  cMo = compute3d3dTransformation(p, q);
327 
328  if (validPose(cMo)) {
329  vpPose pose;
330  pose.addPoints(pose_points);
331  if (pose.computePose(vpPose::VIRTUAL_VS, cMo)) {
332  if (confidence_index != NULL) {
333  *confidence_index = std::min(1.0, normalized_weights * static_cast<double>(nb_points_3d) / polygon.getArea());
334  }
335  return true;
336  }
337  }
338  }
339 
340  return false;
341 }
342 
376  const std::vector<std::vector<vpImagePoint> > &corners,
377  const vpCameraParameters &colorIntrinsics,
378  const std::vector<std::vector<vpPoint> > &point3d,
379  vpHomogeneousMatrix &cMo, double *confidence_index, bool coplanar_points)
380 {
381 
382  if (corners.size() != point3d.size()) {
384  "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
385  point3d.size(), corners.size()));
386  }
387  std::vector<vpPoint> pose_points;
388  if (confidence_index != NULL) {
389  *confidence_index = 0.0;
390  }
391 
392  for (size_t i = 0; i < point3d.size(); i++) {
393  std::vector<vpPoint> tagPoint3d = point3d[i];
394  for (size_t j = 0; j < tagPoint3d.size(); j++) {
395  pose_points.push_back(tagPoint3d[j]);
396  }
397  }
398 
399  // Total area of the polygon to estimate confidence
400  double totalArea = 0.0;
401 
402  // If coplanar is true, the tags_points_3d will be used to compute one plane
403  std::vector<double> tag_points_3d;
404 
405  // Otherwise the vector of planes will be used to compute each plane for each vector
406  std::vector<std::vector<double> > tag_points_3d_nonplanar;
407  size_t nb_points_3d_non_planar = 0;
408 
409  // Loop through each object, compute 3d point cloud of each
410  for (size_t i = 0; i < corners.size(); i++) {
411  std::vector<double> points_3d;
412  vpPolygon polygon(corners[i]);
413  vpRect bb = polygon.getBoundingBox();
414 
415  // The area to calculate final confidence index should be total area of the tags
416  totalArea += polygon.getArea();
417 
418  unsigned int top = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getTop())));
419  unsigned int bottom = static_cast<unsigned int>(
420  std::min(static_cast<int>(depthMap.getHeight()) - 1, static_cast<int>(bb.getBottom())));
421  unsigned int left = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getLeft())));
422  unsigned int right =
423  static_cast<unsigned int>(std::min(static_cast<int>(depthMap.getWidth()) - 1, static_cast<int>(bb.getRight())));
424 
425  points_3d.reserve((bottom - top) * (right - left));
426  for (unsigned int idx_i = top; idx_i < bottom; idx_i++) {
427  for (unsigned int idx_j = left; idx_j < right; idx_j++) {
428  vpImagePoint imPt(idx_i, idx_j);
429  if (depthMap[idx_i][idx_j] > 0 && polygon.isInside(imPt)) {
430  double x = 0, y = 0;
431  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
432  double Z = depthMap[idx_i][idx_j];
433  points_3d.push_back(x * Z);
434  points_3d.push_back(y * Z);
435  points_3d.push_back(Z);
436  }
437  }
438  }
439 
440  // If coplanar_points is true, feed all 3d points to single vector
441  // Otherwise, each vector will hold 3d points for seperate planes
442  if (coplanar_points) {
443  tag_points_3d.insert(tag_points_3d.end(), points_3d.begin(), points_3d.end());
444  } else {
445  tag_points_3d_nonplanar.push_back(points_3d);
446  nb_points_3d_non_planar += points_3d.size();
447  }
448  }
449 
450  size_t nb_points_3d = 0;
451 
452  if (coplanar_points) {
453  nb_points_3d = tag_points_3d.size() / 3;
454  } else {
455  nb_points_3d = nb_points_3d_non_planar / 3;
456  }
457 
458  if (nb_points_3d > 4) {
459  std::vector<vpPoint> p, q;
460 
461  // Plane equation
462  vpColVector plane_equation, centroid;
463  double normalized_weights = 0;
464 
465  if (coplanar_points) {
466  // If all objects are coplanar, use points insides tag_points_3d to estimate the plane
467  estimatePlaneEquationSVD(tag_points_3d, plane_equation, centroid, normalized_weights);
468  int count = 0;
469  for (size_t j = 0; j < corners.size(); j++) {
470  std::vector<vpImagePoint> tag_corner = corners[j];
471  for (size_t i = 0; i < tag_corner.size(); i++) {
472  const vpImagePoint &imPt = tag_corner[i];
473  double x = 0, y = 0;
474  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
475  double Z = computeZMethod1(plane_equation, x, y);
476  std::cout << Z;
477  if (Z < 0) {
478  Z = -Z;
479  }
480  p.push_back(vpPoint(x * Z, y * Z, Z));
481  pose_points[count].set_x(x);
482  pose_points[count].set_y(y);
483  count++;
484  }
485  }
486  } else {
487  // If the tags is not coplanar, estimate the plane for each tags
488  size_t count = 0;
489 
490  for (size_t k = 0; k < tag_points_3d_nonplanar.size(); k++) {
491  std::vector<double> rec_points_3d = tag_points_3d_nonplanar[k];
492  double tag_normalized_weights = 0;
493 
494  if (rec_points_3d.size() >= 9) {
495  // The array must has at least 3 points for the function estimatePlaneEquationSVD not to crash
496  estimatePlaneEquationSVD(rec_points_3d, plane_equation, centroid, tag_normalized_weights);
497  normalized_weights += tag_normalized_weights;
498 
499  // Get the 2d points of the tag the plane just recomputed
500  std::vector<vpImagePoint> tag_corner = corners[k];
501 
502  for (size_t i = 0; i < tag_corner.size(); i++) {
503  const vpImagePoint &imPt = tag_corner[i];
504  double x = 0, y = 0;
505  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
506  double Z = computeZMethod1(plane_equation, x, y);
507 
508  if (Z < 0) {
509  Z = -Z;
510  }
511  p.push_back(vpPoint(x * Z, y * Z, Z));
512  pose_points[count].set_x(x);
513  pose_points[count].set_y(y);
514  count++;
515  }
516  } else {
517  // Sometimes an object may do not have enough points registered due to small size or bad alignment btw depth
518  // and rgb. This behavior happens with Orbbec camera while Realsenses was fine. To prevent exception while
519  // computePose, skip recomputing the failed estimation tag's (4 point - corners)
520  count += corners[k].size();
521  }
522  }
523  normalized_weights = normalized_weights / tag_points_3d_nonplanar.size();
524  }
525 
526  for (size_t i = 0; i < point3d.size(); i++) {
527  std::vector<vpPoint> tagPoint3d = point3d[i];
528  // Sometimes an object may do not have enough points registered due to small size.
529  // The issue happens with Orbbec camera while Realsenses was fine.
530  // To prevent wrong estimation or exception (p and q sizes are differents),
531  // ignore the recomputer vector (tag_points_3d_nonplanar) when size = 0
532  if (coplanar_points) {
533  for (size_t j = 0; j < tagPoint3d.size(); j++) {
534  q.push_back(tagPoint3d[j]);
535  }
536  } else {
537  if (tag_points_3d_nonplanar[i].size() > 0) {
538  for (size_t j = 0; j < tagPoint3d.size(); j++) {
539  q.push_back(tagPoint3d[j]);
540  }
541  }
542  }
543  }
544 
545  // Due to the possibility of q's size might less than p's, check their size should be identical
546  if (p.size() == q.size()) {
547  cMo = compute3d3dTransformation(p, q);
548 
549  if (validPose(cMo)) {
550  vpPose pose;
551  pose.addPoints(pose_points);
552  if (pose.computePose(vpPose::VIRTUAL_VS, cMo)) {
553  if (confidence_index != NULL) {
554  *confidence_index = std::min(1.0, normalized_weights * static_cast<double>(nb_points_3d) / totalArea);
555  }
556  return true;
557  }
558  }
559  }
560  }
561  return false;
562 }
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2030
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
double getTop() const
Definition: vpRect.h:193
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6476
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
Implementation of an homogeneous matrix and operations on such kind of matrices.
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:164
double getArea() const
Definition: vpPolygon.h:161
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
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:254
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
double getRight() const
Definition: vpRect.h:180
vpRect getBoundingBox() const
Definition: vpPolygon.h:177
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
unsigned int getCols() const
Definition: vpArray2D.h:279
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
vpMatrix t() const
Definition: vpMatrix.cpp:464
double get_u() const
Definition: vpImagePoint.h:262
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:309
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:80
Generic class defining intrinsic camera parameters.
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5175
static bool isNaN(double value)
Definition: vpMath.cpp:85
double getLeft() const
Definition: vpRect.h:174
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
Definition: vpImage.h:1115
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Contains an M-estimator and various influence function.
Definition: vpRobust.h:88
Tukey influence function.
Definition: vpRobust.h:93
Defines a rectangle in the plane.
Definition: vpRect.h:79
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
unsigned int getWidth() const
Definition: vpImage.h:246
double getBottom() const
Definition: vpRect.h:98
double get_v() const
Definition: vpImagePoint.h:273