36#include <visp3/core/vpPixelMeterConversion.h>
37#include <visp3/core/vpPolygon.h>
38#include <visp3/core/vpRobust.h>
39#include <visp3/vision/vpPose.h>
43vpHomogeneousMatrix compute3d3dTransformation(
const std::vector<vpPoint> &p,
const std::vector<vpPoint> &q)
45 double N =
static_cast<double>(p.size());
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];
56 for (
unsigned int j = 0; j < 3; j++) {
61 vpMatrix pc(
static_cast<unsigned int>(p.size()), 3);
62 vpMatrix qc(
static_cast<unsigned int>(q.size()), 3);
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];
91 for (
unsigned int i = 0; i < 3; i++) {
92 for (
unsigned int j = 0; j < 3; j++) {
101void estimatePlaneEquationSVD(
const std::vector<double> &point_cloud_face,
vpColVector &plane_equation_estimated,
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);
116 plane_equation_estimated.
resize(4,
false);
117 for (
unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
123 double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
124 double total_w = 0.0;
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];
133 centroid_x /= total_w;
134 centroid_y /= total_w;
135 centroid_z /= total_w;
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);
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) {
158 normal = V.
getCol(indexSmallestSv);
161 double A = normal[0], B = normal[1], C = normal[2];
162 double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
165 plane_equation_estimated[0] = A;
166 plane_equation_estimated[1] = B;
167 plane_equation_estimated[2] = C;
168 plane_equation_estimated[3] = D;
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];
186 centroid.
resize(3,
false);
187 double total_w = 0.0;
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];
196 centroid[0] /= total_w;
197 centroid[1] /= total_w;
198 centroid[2] /= total_w;
201 double A = normal[0], B = normal[1], C = normal[2];
202 double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
205 plane_equation_estimated[0] = A;
206 plane_equation_estimated[1] = B;
207 plane_equation_estimated[2] = C;
208 plane_equation_estimated[3] = D;
210 normalized_weights = total_w / nPoints;
213double computeZMethod1(
const vpColVector &plane_equation,
double x,
double y)
215 return -plane_equation[3] / (plane_equation[0] * x + plane_equation[1] * y + plane_equation[2]);
222 for (
unsigned int i = 0; i < cMo.
getRows() && valid; i++) {
223 for (
unsigned int j = 0; j < cMo.
getCols() && valid; j++) {
257 double *confidence_index)
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()));
264 std::vector<vpPoint> pose_points;
265 if (confidence_index != NULL) {
266 *confidence_index = 0.0;
269 for (
size_t i = 0; i < point3d.size(); i++) {
270 pose_points.push_back(point3d[i]);
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())));
280 static_cast<unsigned int>(std::min(
static_cast<int>(depthMap.
getWidth()) - 1,
static_cast<int>(bb.
getRight())));
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++) {
287 if (depthMap[idx_i][idx_j] > 0 && polygon.
isInside(imPt)) {
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);
298 unsigned int nb_points_3d =
static_cast<unsigned int>(points_3d.size() / 3);
300 if (nb_points_3d > 4) {
301 std::vector<vpPoint> p, q;
305 double normalized_weights = 0;
306 estimatePlaneEquationSVD(points_3d, plane_equation, centroid, normalized_weights);
308 for (
size_t j = 0; j < corners.size(); j++) {
312 double Z = computeZMethod1(plane_equation, x, y);
316 p.push_back(
vpPoint(x * Z, y * Z, Z));
318 pose_points[j].set_x(x);
319 pose_points[j].set_y(y);
322 for (
size_t i = 0; i < point3d.size(); i++) {
323 q.push_back(point3d[i]);
326 cMo = compute3d3dTransformation(p, q);
328 if (validPose(cMo)) {
332 if (confidence_index != NULL) {
333 *confidence_index = std::min(1.0, normalized_weights *
static_cast<double>(nb_points_3d) / polygon.
getArea());
376 const std::vector<std::vector<vpImagePoint> > &corners,
378 const std::vector<std::vector<vpPoint> > &point3d,
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()));
387 std::vector<vpPoint> pose_points;
388 if (confidence_index != NULL) {
389 *confidence_index = 0.0;
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]);
400 double totalArea = 0.0;
403 std::vector<double> tag_points_3d;
406 std::vector<std::vector<double> > tag_points_3d_nonplanar;
407 size_t nb_points_3d_non_planar = 0;
410 for (
size_t i = 0; i < corners.size(); i++) {
411 std::vector<double> points_3d;
416 totalArea += polygon.
getArea();
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())));
423 static_cast<unsigned int>(std::min(
static_cast<int>(depthMap.
getWidth()) - 1,
static_cast<int>(bb.
getRight())));
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++) {
429 if (depthMap[idx_i][idx_j] > 0 && polygon.
isInside(imPt)) {
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);
442 if (coplanar_points) {
443 tag_points_3d.
insert(tag_points_3d.end(), points_3d.begin(), points_3d.end());
445 tag_points_3d_nonplanar.push_back(points_3d);
446 nb_points_3d_non_planar += points_3d.size();
450 size_t nb_points_3d = 0;
452 if (coplanar_points) {
453 nb_points_3d = tag_points_3d.size() / 3;
455 nb_points_3d = nb_points_3d_non_planar / 3;
458 if (nb_points_3d > 4) {
459 std::vector<vpPoint> p, q;
463 double normalized_weights = 0;
465 if (coplanar_points) {
467 estimatePlaneEquationSVD(tag_points_3d, plane_equation, centroid, normalized_weights);
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++) {
475 double Z = computeZMethod1(plane_equation, x, y);
480 p.push_back(
vpPoint(x * Z, y * Z, Z));
481 pose_points[count].set_x(x);
482 pose_points[count].set_y(y);
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;
494 if (rec_points_3d.size() >= 9) {
496 estimatePlaneEquationSVD(rec_points_3d, plane_equation, centroid, tag_normalized_weights);
497 normalized_weights += tag_normalized_weights;
500 std::vector<vpImagePoint> tag_corner = corners[k];
502 for (
size_t i = 0; i < tag_corner.size(); i++) {
506 double Z = computeZMethod1(plane_equation, x, y);
511 p.push_back(
vpPoint(x * Z, y * Z, Z));
512 pose_points[count].set_x(x);
513 pose_points[count].set_y(y);
520 count += corners[k].size();
523 normalized_weights = normalized_weights / tag_points_3d_nonplanar.size();
526 for (
size_t i = 0; i < point3d.size(); i++) {
527 std::vector<vpPoint> tagPoint3d = point3d[i];
532 if (coplanar_points) {
533 for (
size_t j = 0; j < tagPoint3d.size(); j++) {
534 q.push_back(tagPoint3d[j]);
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]);
546 if (p.size() == q.size()) {
547 cMo = compute3d3dTransformation(p, q);
549 if (validPose(cMo)) {
553 if (confidence_index != NULL) {
554 *confidence_index = std::min(1.0, normalized_weights *
static_cast<double>(nb_points_3d) / totalArea);
unsigned int getCols() const
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getRows() const
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 emited by ViSP classes.
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 ...
unsigned int getWidth() const
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
unsigned int getHeight() const
static bool isNaN(double value)
Implementation of a matrix and operations on matrices.
void svd(vpColVector &w, vpMatrix &V)
vpColVector getCol(unsigned int j) const
double det(vpDetMethod method=LU_DECOMPOSITION) const
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 ...
Defines a generic 2D polygon.
vpRect getBoundingBox() const
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...
void addPoints(const std::vector< vpPoint > &lP)
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
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.
Contains an M-estimator and various influence function.
@ TUKEY
Tukey influence function.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
void setMinMedianAbsoluteDeviation(double mad_min)