Visual Servoing Platform  version 3.4.1 under development (2021-09-25)
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/vision/vpPose.h>
37 #include <visp3/core/vpPolygon.h>
38 #include <visp3/core/vpPixelMeterConversion.h>
39 #include <visp3/core/vpRobust.h>
40 
41 namespace {
42 vpHomogeneousMatrix compute3d3dTransformation(const std::vector<vpPoint>& p, const std::vector<vpPoint>& q) {
43  double N = static_cast<double>(p.size());
44 
45  vpColVector p_bar(3, 0.0);
46  vpColVector q_bar(3, 0.0);
47  for (size_t i = 0; i < p.size(); i++) {
48  for (unsigned int j = 0; j < 3; j++) {
49  p_bar[j] += p[i].oP[j];
50  q_bar[j] += q[i].oP[j];
51  }
52  }
53 
54  for (unsigned int j = 0; j < 3; j++) {
55  p_bar[j] /= N;
56  q_bar[j] /= N;
57  }
58 
59  vpMatrix pc(static_cast<unsigned int>(p.size()), 3);
60  vpMatrix qc(static_cast<unsigned int>(q.size()), 3);
61 
62  for (unsigned int i = 0; i < static_cast<unsigned int>(p.size()); i++) {
63  for (unsigned int j = 0; j < 3; j++) {
64  pc[i][j] = p[i].oP[j] - p_bar[j];
65  qc[i][j] = q[i].oP[j] - q_bar[j];
66  }
67  }
68 
69  vpMatrix pct_qc = pc.t()*qc;
70  vpMatrix U = pct_qc, V;
71  vpColVector W;
72  U.svd(W, V);
73 
74  vpMatrix Vt = V.t();
75  vpMatrix R = U*Vt;
76 
77  double det = R.det();
78  if (det < 0) {
79  Vt[2][0] *= -1;
80  Vt[2][1] *= -1;
81  Vt[2][2] *= -1;
82 
83  R = U*Vt;
84  }
85 
86  vpColVector t = p_bar - R*q_bar;
87 
89  for (unsigned int i = 0; i < 3; i++) {
90  for (unsigned int j = 0; j < 3; j++) {
91  cMo[i][j] = R[i][j];
92  }
93  cMo[i][3] = t[i];
94  }
95 
96  return cMo;
97 }
98 
99 void estimatePlaneEquationSVD(const std::vector<double> &point_cloud_face,
100  vpColVector &plane_equation_estimated, vpColVector &centroid,
101  double &normalized_weights)
102 {
103  unsigned int max_iter = 10;
104  double prev_error = 1e3;
105  double error = 1e3 - 1;
106  unsigned int nPoints = static_cast<unsigned int>(point_cloud_face.size() / 3);
107 
108  vpColVector weights(nPoints, 1.0);
109  vpColVector residues(nPoints);
110  vpMatrix M(nPoints, 3);
111  vpRobust tukey;
112  tukey.setMinMedianAbsoluteDeviation(1e-4);
113  vpColVector normal;
114 
115  plane_equation_estimated.resize(4, false);
116  for (unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
117  if (iter != 0) {
118  tukey.MEstimator(vpRobust::TUKEY, residues, weights);
119  }
120 
121  // Compute centroid
122  double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
123  double total_w = 0.0;
124 
125  for (unsigned int i = 0; i < nPoints; i++) {
126  centroid_x += weights[i] * point_cloud_face[3 * i + 0];
127  centroid_y += weights[i] * point_cloud_face[3 * i + 1];
128  centroid_z += weights[i] * point_cloud_face[3 * i + 2];
129  total_w += weights[i];
130  }
131 
132  centroid_x /= total_w;
133  centroid_y /= total_w;
134  centroid_z /= total_w;
135 
136  // Minimization
137  for (unsigned int i = 0; i < nPoints; i++) {
138  M[static_cast<unsigned int>(i)][0] = weights[i] * (point_cloud_face[3 * i + 0] - centroid_x);
139  M[static_cast<unsigned int>(i)][1] = weights[i] * (point_cloud_face[3 * i + 1] - centroid_y);
140  M[static_cast<unsigned int>(i)][2] = weights[i] * (point_cloud_face[3 * i + 2] - centroid_z);
141  }
142 
143  vpColVector W;
144  vpMatrix V;
145  vpMatrix J = M.t() * M;
146  J.svd(W, V);
147 
148  double smallestSv = W[0];
149  unsigned int indexSmallestSv = 0;
150  for (unsigned int i = 1; i < W.size(); i++) {
151  if (W[i] < smallestSv) {
152  smallestSv = W[i];
153  indexSmallestSv = i;
154  }
155  }
156 
157  normal = V.getCol(indexSmallestSv);
158 
159  // Compute plane equation
160  double A = normal[0], B = normal[1], C = normal[2];
161  double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
162 
163  // Update plane equation
164  plane_equation_estimated[0] = A;
165  plane_equation_estimated[1] = B;
166  plane_equation_estimated[2] = C;
167  plane_equation_estimated[3] = D;
168 
169  // Compute error points to estimated plane
170  prev_error = error;
171  error = 0.0;
172  for (unsigned int i = 0; i < nPoints; i++) {
173  residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
174  C * point_cloud_face[3 * i + 2] + D) / sqrt(A * A + B * B + C * C);
175  error += weights[i] * residues[i];
176  }
177  error /= total_w;
178  }
179 
180  // Update final weights
181  tukey.MEstimator(vpRobust::TUKEY, residues, weights);
182 
183  // Update final centroid
184  centroid.resize(3, false);
185  double total_w = 0.0;
186 
187  for (unsigned int i = 0; i < nPoints; i++) {
188  centroid[0] += weights[i] * point_cloud_face[3 * i];
189  centroid[1] += weights[i] * point_cloud_face[3 * i + 1];
190  centroid[2] += weights[i] * point_cloud_face[3 * i + 2];
191  total_w += weights[i];
192  }
193 
194  centroid[0] /= total_w;
195  centroid[1] /= total_w;
196  centroid[2] /= total_w;
197 
198  // Compute final plane equation
199  double A = normal[0], B = normal[1], C = normal[2];
200  double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
201 
202  // Update final plane equation
203  plane_equation_estimated[0] = A;
204  plane_equation_estimated[1] = B;
205  plane_equation_estimated[2] = C;
206  plane_equation_estimated[3] = D;
207 
208  normalized_weights = total_w / nPoints;
209 }
210 
211 double computeZMethod1(const vpColVector& plane_equation, double x, double y) {
212  return -plane_equation[3] / (plane_equation[0]*x + plane_equation[1]*y + plane_equation[2]);
213 }
214 
215 bool validPose(const vpHomogeneousMatrix& cMo) {
216  bool valid = true;
217 
218  for (unsigned int i = 0; i < cMo.getRows() && valid; i++) {
219  for (unsigned int j = 0; j < cMo.getCols() && valid; j++) {
220  if (vpMath::isNaN(cMo[i][j])) {
221  valid = false;
222  }
223  }
224  }
225 
226  return valid;
227 }
228 }
229 
250 bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
251  const vpCameraParameters &colorIntrinsics, const std::vector<vpPoint> &point3d,
252  vpHomogeneousMatrix &cMo, double *confidence_index)
253 {
254  if (corners.size() != point3d.size()) {
255  throw(vpException(vpException::fatalError, "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
256  point3d.size(), corners.size()));
257  }
258  std::vector<vpPoint> pose_points;
259  if (confidence_index != NULL) {
260  *confidence_index = 0.0;
261  }
262 
263  for (size_t i = 0; i < point3d.size(); i ++) {
264  pose_points.push_back(point3d[i]);
265  }
266 
267  vpPolygon polygon(corners);
268  vpRect bb = polygon.getBoundingBox();
269  unsigned int top = static_cast<unsigned int>(std::max( 0, static_cast<int>(bb.getTop()) ));
270  unsigned int bottom = static_cast<unsigned int>(std::min( static_cast<int>(depthMap.getHeight())-1, static_cast<int>(bb.getBottom()) ));
271  unsigned int left = static_cast<unsigned int>(std::max( 0, static_cast<int>(bb.getLeft()) ));
272  unsigned int right = static_cast<unsigned int>(std::min( static_cast<int>(depthMap.getWidth())-1, static_cast<int>(bb.getRight()) ));
273 
274  std::vector<double> points_3d;
275  points_3d.reserve( (bottom-top)*(right-left) );
276  for (unsigned int idx_i = top; idx_i < bottom; idx_i++) {
277  for (unsigned int idx_j = left; idx_j < right; idx_j++) {
278  vpImagePoint imPt(idx_i, idx_j);
279  if (depthMap[idx_i][idx_j] > 0 && polygon.isInside(imPt)) {
280  double x = 0, y = 0;
281  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
282  double Z = depthMap[idx_i][idx_j];
283  points_3d.push_back(x*Z);
284  points_3d.push_back(y*Z);
285  points_3d.push_back(Z);
286  }
287  }
288  }
289 
290  unsigned int nb_points_3d = static_cast<unsigned int>(points_3d.size() / 3);
291 
292  if (nb_points_3d > 4) {
293  std::vector<vpPoint> p, q;
294 
295  // Plane equation
296  vpColVector plane_equation, centroid;
297  double normalized_weights = 0;
298  estimatePlaneEquationSVD(points_3d, plane_equation, centroid, normalized_weights);
299 
300  for (size_t j = 0; j < corners.size(); j++) {
301  const vpImagePoint& imPt = corners[j];
302  double x = 0, y = 0;
303  vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
304  double Z = computeZMethod1(plane_equation, x, y);
305  if (Z < 0) {
306  Z = -Z;
307  }
308  p.push_back(vpPoint(x*Z, y*Z, Z));
309 
310  pose_points[j].set_x(x);
311  pose_points[j].set_y(y);
312  }
313 
314  for (size_t i = 0; i < point3d.size(); i ++) {
315  q.push_back(point3d[i]);
316  }
317 
318  cMo = compute3d3dTransformation(p, q);
319 
320  if (validPose(cMo)) {
321  vpPose pose;
322  pose.addPoints(pose_points);
323  if (pose.computePose(vpPose::VIRTUAL_VS, cMo)) {
324  if (confidence_index != NULL) {
325  *confidence_index = std::min(1.0, normalized_weights * static_cast<double>(nb_points_3d) / polygon.getArea());
326  }
327  return true;
328  }
329  }
330  }
331 
332  return false;
333 }
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:6477
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:250
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 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