Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
40namespace
41{
42 // See also vpPlaneEstimation.cpp that implements the same functionaly in c++17
43void 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
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
167bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
168 const vpCameraParameters &colorIntrinsics,
169 const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
170 double *confidence_index)
171{
172 if (corners.size() != point3d.size()) {
174 "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
175 point3d.size(), corners.size()));
176 }
177 std::vector<vpPoint> pose_points;
178 if (confidence_index != NULL) {
179 *confidence_index = 0.0;
180 }
181
182 for (size_t i = 0; i < point3d.size(); i++) {
183 pose_points.push_back(point3d[i]);
184 }
185
186 vpPolygon polygon(corners);
187 vpRect bb = polygon.getBoundingBox();
188 unsigned int top = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getTop())));
189 unsigned int bottom =
190 static_cast<unsigned int>(std::min(static_cast<int>(depthMap.getHeight()) - 1, static_cast<int>(bb.getBottom())));
191 unsigned int left = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getLeft())));
192 unsigned int right =
193 static_cast<unsigned int>(std::min(static_cast<int>(depthMap.getWidth()) - 1, static_cast<int>(bb.getRight())));
194
195 std::vector<double> points_3d;
196 points_3d.reserve((bottom - top) * (right - left));
197 for (unsigned int idx_i = top; idx_i < bottom; idx_i++) {
198 for (unsigned int idx_j = left; idx_j < right; idx_j++) {
199 vpImagePoint imPt(idx_i, idx_j);
200 if (depthMap[idx_i][idx_j] > 0 && polygon.isInside(imPt)) {
201 double x = 0, y = 0;
202 vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
203 double Z = depthMap[idx_i][idx_j];
204 points_3d.push_back(x * Z);
205 points_3d.push_back(y * Z);
206 points_3d.push_back(Z);
207 }
208 }
209 }
210
211 unsigned int nb_points_3d = static_cast<unsigned int>(points_3d.size() / 3);
212
213 if (nb_points_3d > 4) {
214 std::vector<vpPoint> p, q;
215
216 // Plane equation
217 vpPlane plane_equation;
218 vpColVector centroid;
219 double normalized_weights = 0;
220 estimatePlaneEquationSVD(points_3d, plane_equation, centroid, normalized_weights);
221
222 for (size_t j = 0; j < corners.size(); j++) {
223 const vpImagePoint &imPt = corners[j];
224 double x = 0, y = 0;
225 vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
226 double Z = plane_equation.computeZ(x, y);
227 if (Z < 0) {
228 Z = -Z;
229 }
230 p.push_back(vpPoint(x * Z, y * Z, Z));
231
232 pose_points[j].set_x(x);
233 pose_points[j].set_y(y);
234 }
235
236 for (size_t i = 0; i < point3d.size(); i++) {
237 q.push_back(point3d[i]);
238 }
239
241
242 if (cMo.isValid()) {
243 vpPose pose;
244 pose.addPoints(pose_points);
245 if (pose.computePose(vpPose::VIRTUAL_VS, cMo)) {
246 if (confidence_index != NULL) {
247 *confidence_index = std::min(1.0, normalized_weights * static_cast<double>(nb_points_3d) / polygon.getArea());
248 }
249 return true;
250 }
251 }
252 }
253
254 return false;
255}
256
290 const std::vector<std::vector<vpImagePoint> > &corners,
291 const vpCameraParameters &colorIntrinsics,
292 const std::vector<std::vector<vpPoint> > &point3d,
293 vpHomogeneousMatrix &cMo, double *confidence_index, bool coplanar_points)
294{
295
296 if (corners.size() != point3d.size()) {
298 "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
299 point3d.size(), corners.size()));
300 }
301 std::vector<vpPoint> pose_points;
302 if (confidence_index != NULL) {
303 *confidence_index = 0.0;
304 }
305
306 for (size_t i = 0; i < point3d.size(); i++) {
307 std::vector<vpPoint> tagPoint3d = point3d[i];
308 for (size_t j = 0; j < tagPoint3d.size(); j++) {
309 pose_points.push_back(tagPoint3d[j]);
310 }
311 }
312
313 // Total area of the polygon to estimate confidence
314 double totalArea = 0.0;
315
316 // If coplanar is true, the tags_points_3d will be used to compute one plane
317 std::vector<double> tag_points_3d;
318
319 // Otherwise the vector of planes will be used to compute each plane for each vector
320 std::vector<std::vector<double> > tag_points_3d_nonplanar;
321 size_t nb_points_3d_non_planar = 0;
322
323 // Loop through each object, compute 3d point cloud of each
324 for (size_t i = 0; i < corners.size(); i++) {
325 std::vector<double> points_3d;
326 vpPolygon polygon(corners[i]);
327 vpRect bb = polygon.getBoundingBox();
328
329 // The area to calculate final confidence index should be total area of the tags
330 totalArea += polygon.getArea();
331
332 unsigned int top = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getTop())));
333 unsigned int bottom = static_cast<unsigned int>(
334 std::min(static_cast<int>(depthMap.getHeight()) - 1, static_cast<int>(bb.getBottom())));
335 unsigned int left = static_cast<unsigned int>(std::max(0, static_cast<int>(bb.getLeft())));
336 unsigned int right =
337 static_cast<unsigned int>(std::min(static_cast<int>(depthMap.getWidth()) - 1, static_cast<int>(bb.getRight())));
338
339 points_3d.reserve((bottom - top) * (right - left));
340 for (unsigned int idx_i = top; idx_i < bottom; idx_i++) {
341 for (unsigned int idx_j = left; idx_j < right; idx_j++) {
342 vpImagePoint imPt(idx_i, idx_j);
343 if (depthMap[idx_i][idx_j] > 0 && polygon.isInside(imPt)) {
344 double x = 0, y = 0;
345 vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
346 double Z = depthMap[idx_i][idx_j];
347 points_3d.push_back(x * Z);
348 points_3d.push_back(y * Z);
349 points_3d.push_back(Z);
350 }
351 }
352 }
353
354 // If coplanar_points is true, feed all 3d points to single vector
355 // Otherwise, each vector will hold 3d points for separate planes
356 if (coplanar_points) {
357 tag_points_3d.insert(tag_points_3d.end(), points_3d.begin(), points_3d.end());
358 } else {
359 tag_points_3d_nonplanar.push_back(points_3d);
360 nb_points_3d_non_planar += points_3d.size();
361 }
362 }
363
364 size_t nb_points_3d = 0;
365
366 if (coplanar_points) {
367 nb_points_3d = tag_points_3d.size() / 3;
368 } else {
369 nb_points_3d = nb_points_3d_non_planar / 3;
370 }
371
372 if (nb_points_3d > 4) {
373 std::vector<vpPoint> p, q;
374
375 // Plane equation
376 vpPlane plane_equation;
377 vpColVector centroid;
378 double normalized_weights = 0;
379
380 if (coplanar_points) {
381 // If all objects are coplanar, use points insides tag_points_3d to estimate the plane
382 estimatePlaneEquationSVD(tag_points_3d, plane_equation, centroid, normalized_weights);
383 int count = 0;
384 for (size_t j = 0; j < corners.size(); j++) {
385 std::vector<vpImagePoint> tag_corner = corners[j];
386 for (size_t i = 0; i < tag_corner.size(); i++) {
387 const vpImagePoint &imPt = tag_corner[i];
388 double x = 0, y = 0;
389 vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
390 double Z = plane_equation.computeZ(x, y);
391 std::cout << Z;
392 if (Z < 0) {
393 Z = -Z;
394 }
395 p.push_back(vpPoint(x * Z, y * Z, Z));
396 pose_points[count].set_x(x);
397 pose_points[count].set_y(y);
398 count++;
399 }
400 }
401 } else {
402 // If the tags is not coplanar, estimate the plane for each tags
403 size_t count = 0;
404
405 for (size_t k = 0; k < tag_points_3d_nonplanar.size(); k++) {
406 std::vector<double> rec_points_3d = tag_points_3d_nonplanar[k];
407 double tag_normalized_weights = 0;
408
409 if (rec_points_3d.size() >= 9) {
410 // The array must has at least 3 points for the function estimatePlaneEquationSVD not to crash
411 estimatePlaneEquationSVD(rec_points_3d, plane_equation, centroid, tag_normalized_weights);
412 normalized_weights += tag_normalized_weights;
413
414 // Get the 2d points of the tag the plane just recomputed
415 std::vector<vpImagePoint> tag_corner = corners[k];
416
417 for (size_t i = 0; i < tag_corner.size(); i++) {
418 const vpImagePoint &imPt = tag_corner[i];
419 double x = 0, y = 0;
420 vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y);
421 double Z = plane_equation.computeZ(x, y);
422
423 if (Z < 0) {
424 Z = -Z;
425 }
426 p.push_back(vpPoint(x * Z, y * Z, Z));
427 pose_points[count].set_x(x);
428 pose_points[count].set_y(y);
429 count++;
430 }
431 } else {
432 // Sometimes an object may do not have enough points registered due to small size or bad alignment btw depth
433 // and rgb. This behavior happens with Orbbec camera while Realsenses was fine. To prevent exception while
434 // computePose, skip recomputing the failed estimation tag's (4 point - corners)
435 count += corners[k].size();
436 }
437 }
438 normalized_weights = normalized_weights / tag_points_3d_nonplanar.size();
439 }
440
441 for (size_t i = 0; i < point3d.size(); i++) {
442 std::vector<vpPoint> tagPoint3d = point3d[i];
443 // Sometimes an object may do not have enough points registered due to small size.
444 // The issue happens with Orbbec camera while Realsenses was fine.
445 // To prevent wrong estimation or exception (p and q sizes are differents),
446 // ignore the recomputer vector (tag_points_3d_nonplanar) when size = 0
447 if (coplanar_points) {
448 for (size_t j = 0; j < tagPoint3d.size(); j++) {
449 q.push_back(tagPoint3d[j]);
450 }
451 } else {
452 if (tag_points_3d_nonplanar[i].size() > 0) {
453 for (size_t j = 0; j < tagPoint3d.size(); j++) {
454 q.push_back(tagPoint3d[j]);
455 }
456 }
457 }
458 }
459
460 // Due to the possibility of q's size might less than p's, check their size should be identical
461 if (p.size() == q.size()) {
463
464 if (cMo.isValid()) {
465 vpPose pose;
466 pose.addPoints(pose_points);
467 if (pose.computePose(vpPose::VIRTUAL_VS, cMo)) {
468 if (confidence_index != NULL) {
469 *confidence_index = std::min(1.0, normalized_weights * static_cast<double>(nb_points_3d) / totalArea);
470 }
471 return true;
472 }
473 }
474 }
475 }
476 return false;
477}
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
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 ...
double get_u() const
double get_v() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
Definition vpImage.h:1361
unsigned int getHeight() const
Definition vpImage.h:184
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void svd(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition vpMatrix.cpp:461
vpColVector getCol(unsigned int j) const
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
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:96
void addPoints(const std::vector< vpPoint > &lP)
Definition vpPose.cpp:155
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
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)
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:87
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition vpRobust.h:155