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