Visual Servoing Platform version 3.5.0
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
41namespace
42{
43vpHomogeneousMatrix 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;
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
101void 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;
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
213double 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
218bool 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
254bool 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}
unsigned int getCols() const
Definition: vpArray2D.h:279
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
unsigned int getRows() const
Definition: vpArray2D.h:289
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
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.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double get_u() const
Definition: vpImagePoint.h:262
double get_v() const
Definition: vpImagePoint.h:273
unsigned int getWidth() const
Definition: vpImage.h:246
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
Definition: vpImage.h:1115
unsigned int getHeight() const
Definition: vpImage.h:188
static bool isNaN(double value)
Definition: vpMath.cpp:85
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2030
vpMatrix t() const
Definition: vpMatrix.cpp:464
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5175
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6476
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
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:104
vpRect getBoundingBox() const
Definition: vpPolygon.h:177
double getArea() const
Definition: vpPolygon.h:161
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:81
@ VIRTUAL_VS
Definition: vpPose.h:95
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:374
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
Defines a rectangle in the plane.
Definition: vpRect.h:80
double getLeft() const
Definition: vpRect.h:174
double getRight() const
Definition: vpRect.h:180
double getBottom() const
Definition: vpRect.h:98
double getTop() const
Definition: vpRect.h:193
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