36 #include <visp3/core/vpPixelMeterConversion.h>
37 #include <visp3/core/vpPlane.h>
38 #include <visp3/core/vpPolygon.h>
39 #include <visp3/core/vpRobust.h>
40 #include <visp3/vision/vpPose.h>
45 void estimatePlaneEquationSVD(
const std::vector<double> &point_cloud_face,
vpPlane &plane_equation_estimated,
48 unsigned int max_iter = 10;
49 double prev_error = 1e3;
50 double error = 1e3 - 1;
51 unsigned int nPoints =
static_cast<unsigned int>(point_cloud_face.size() / 3);
60 for (
unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
66 double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
69 for (
unsigned int i = 0; i < nPoints; i++) {
70 centroid_x += weights[i] * point_cloud_face[3 * i + 0];
71 centroid_y += weights[i] * point_cloud_face[3 * i + 1];
72 centroid_z += weights[i] * point_cloud_face[3 * i + 2];
73 total_w += weights[i];
76 centroid_x /= total_w;
77 centroid_y /= total_w;
78 centroid_z /= total_w;
81 for (
unsigned int i = 0; i < nPoints; i++) {
82 M[
static_cast<unsigned int>(i)][0] = weights[i] * (point_cloud_face[3 * i + 0] - centroid_x);
83 M[
static_cast<unsigned int>(i)][1] = weights[i] * (point_cloud_face[3 * i + 1] - centroid_y);
84 M[
static_cast<unsigned int>(i)][2] = weights[i] * (point_cloud_face[3 * i + 2] - centroid_z);
92 double smallestSv = W[0];
93 unsigned int indexSmallestSv = 0;
94 for (
unsigned int i = 1; i < W.
size(); i++) {
95 if (W[i] < smallestSv) {
101 normal = V.
getCol(indexSmallestSv);
104 double A = normal[0], B = normal[1], C = normal[2];
105 double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
110 for (
unsigned int i = 0; i < nPoints; i++) {
111 residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
112 C * point_cloud_face[3 * i + 2] + D) /
113 sqrt(A * A + B * B + C * C);
114 error += weights[i] * residues[i];
123 centroid.
resize(3,
false);
124 double total_w = 0.0;
126 for (
unsigned int i = 0; i < nPoints; i++) {
127 centroid[0] += weights[i] * point_cloud_face[3 * i];
128 centroid[1] += weights[i] * point_cloud_face[3 * i + 1];
129 centroid[2] += weights[i] * point_cloud_face[3 * i + 2];
130 total_w += weights[i];
133 centroid[0] /= total_w;
134 centroid[1] /= total_w;
135 centroid[2] /= total_w;
138 double A = normal[0], B = normal[1], C = normal[2];
139 double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
142 plane_equation_estimated.
setABCD(A, B, C, D);
144 normalized_weights = total_w / nPoints;
172 double *confidence_index)
174 if (corners.size() != point3d.size()) {
176 "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
177 point3d.size(), corners.size()));
179 std::vector<vpPoint> pose_points;
180 if (confidence_index != NULL) {
181 *confidence_index = 0.0;
184 for (
size_t i = 0; i < point3d.size(); i++) {
185 pose_points.push_back(point3d[i]);
190 unsigned int top =
static_cast<unsigned int>(std::max(0,
static_cast<int>(bb.
getTop())));
191 unsigned int bottom =
192 static_cast<unsigned int>(std::min(
static_cast<int>(depthMap.
getHeight()) - 1,
static_cast<int>(bb.
getBottom())));
193 unsigned int left =
static_cast<unsigned int>(std::max(0,
static_cast<int>(bb.
getLeft())));
195 static_cast<unsigned int>(std::min(
static_cast<int>(depthMap.
getWidth()) - 1,
static_cast<int>(bb.
getRight())));
197 std::vector<double> points_3d;
198 points_3d.reserve((bottom - top) * (right - left));
199 for (
unsigned int idx_i = top; idx_i < bottom; idx_i++) {
200 for (
unsigned int idx_j = left; idx_j < right; idx_j++) {
202 if (depthMap[idx_i][idx_j] > 0 && polygon.
isInside(imPt)) {
205 double Z = depthMap[idx_i][idx_j];
206 points_3d.push_back(x * Z);
207 points_3d.push_back(y * Z);
208 points_3d.push_back(Z);
213 unsigned int nb_points_3d =
static_cast<unsigned int>(points_3d.size() / 3);
215 if (nb_points_3d > 4) {
216 std::vector<vpPoint> p, q;
221 double normalized_weights = 0;
222 estimatePlaneEquationSVD(points_3d, plane_equation, centroid, normalized_weights);
224 for (
size_t j = 0; j < corners.size(); j++) {
228 double Z = plane_equation.
computeZ(x, y);
232 p.push_back(
vpPoint(x * Z, y * Z, Z));
234 pose_points[j].set_x(x);
235 pose_points[j].set_y(y);
238 for (
size_t i = 0; i < point3d.size(); i++) {
239 q.push_back(point3d[i]);
248 if (confidence_index != NULL) {
249 *confidence_index = std::min(1.0, normalized_weights *
static_cast<double>(nb_points_3d) / polygon.
getArea());
292 const std::vector<std::vector<vpImagePoint> > &corners,
294 const std::vector<std::vector<vpPoint> > &point3d,
298 if (corners.size() != point3d.size()) {
300 "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
301 point3d.size(), corners.size()));
303 std::vector<vpPoint> pose_points;
304 if (confidence_index != NULL) {
305 *confidence_index = 0.0;
308 for (
size_t i = 0; i < point3d.size(); i++) {
309 std::vector<vpPoint> tagPoint3d = point3d[i];
310 for (
size_t j = 0; j < tagPoint3d.size(); j++) {
311 pose_points.push_back(tagPoint3d[j]);
316 double totalArea = 0.0;
319 std::vector<double> tag_points_3d;
322 std::vector<std::vector<double> > tag_points_3d_nonplanar;
323 size_t nb_points_3d_non_planar = 0;
326 for (
size_t i = 0; i < corners.size(); i++) {
327 std::vector<double> points_3d;
332 totalArea += polygon.
getArea();
334 unsigned int top =
static_cast<unsigned int>(std::max(0,
static_cast<int>(bb.
getTop())));
335 unsigned int bottom =
static_cast<unsigned int>(
336 std::min(
static_cast<int>(depthMap.
getHeight()) - 1,
static_cast<int>(bb.
getBottom())));
337 unsigned int left =
static_cast<unsigned int>(std::max(0,
static_cast<int>(bb.
getLeft())));
339 static_cast<unsigned int>(std::min(
static_cast<int>(depthMap.
getWidth()) - 1,
static_cast<int>(bb.
getRight())));
341 points_3d.reserve((bottom - top) * (right - left));
342 for (
unsigned int idx_i = top; idx_i < bottom; idx_i++) {
343 for (
unsigned int idx_j = left; idx_j < right; idx_j++) {
345 if (depthMap[idx_i][idx_j] > 0 && polygon.
isInside(imPt)) {
348 double Z = depthMap[idx_i][idx_j];
349 points_3d.push_back(x * Z);
350 points_3d.push_back(y * Z);
351 points_3d.push_back(Z);
358 if (coplanar_points) {
359 tag_points_3d.
insert(tag_points_3d.end(), points_3d.begin(), points_3d.end());
361 tag_points_3d_nonplanar.push_back(points_3d);
362 nb_points_3d_non_planar += points_3d.size();
366 size_t nb_points_3d = 0;
368 if (coplanar_points) {
369 nb_points_3d = tag_points_3d.size() / 3;
371 nb_points_3d = nb_points_3d_non_planar / 3;
374 if (nb_points_3d > 4) {
375 std::vector<vpPoint> p, q;
380 double normalized_weights = 0;
382 if (coplanar_points) {
384 estimatePlaneEquationSVD(tag_points_3d, plane_equation, centroid, normalized_weights);
386 for (
size_t j = 0; j < corners.size(); j++) {
387 std::vector<vpImagePoint> tag_corner = corners[j];
388 for (
size_t i = 0; i < tag_corner.size(); i++) {
392 double Z = plane_equation.
computeZ(x, y);
397 p.push_back(
vpPoint(x * Z, y * Z, Z));
398 pose_points[count].set_x(x);
399 pose_points[count].set_y(y);
407 for (
size_t k = 0; k < tag_points_3d_nonplanar.size(); k++) {
408 std::vector<double> rec_points_3d = tag_points_3d_nonplanar[k];
409 double tag_normalized_weights = 0;
411 if (rec_points_3d.size() >= 9) {
413 estimatePlaneEquationSVD(rec_points_3d, plane_equation, centroid, tag_normalized_weights);
414 normalized_weights += tag_normalized_weights;
417 std::vector<vpImagePoint> tag_corner = corners[k];
419 for (
size_t i = 0; i < tag_corner.size(); i++) {
423 double Z = plane_equation.
computeZ(x, y);
428 p.push_back(
vpPoint(x * Z, y * Z, Z));
429 pose_points[count].set_x(x);
430 pose_points[count].set_y(y);
437 count += corners[k].size();
440 normalized_weights = normalized_weights / tag_points_3d_nonplanar.size();
443 for (
size_t i = 0; i < point3d.size(); i++) {
444 std::vector<vpPoint> tagPoint3d = point3d[i];
449 if (coplanar_points) {
450 for (
size_t j = 0; j < tagPoint3d.size(); j++) {
451 q.push_back(tagPoint3d[j]);
454 if (tag_points_3d_nonplanar[i].size() > 0) {
455 for (
size_t j = 0; j < tagPoint3d.size(); j++) {
456 q.push_back(tagPoint3d[j]);
463 if (p.size() == q.size()) {
470 if (confidence_index != NULL) {
471 *confidence_index = std::min(1.0, normalized_weights *
static_cast<double>(nb_points_3d) / totalArea);
unsigned int size() const
Return the number of elements of the 2D array.
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.
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 ...
unsigned int getWidth() const
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
unsigned int getHeight() const
Implementation of a matrix and operations on matrices.
void svd(vpColVector &w, vpMatrix &V)
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.
double computeZ(double x, double y) const
void setABCD(double a, double b, double c, double d)
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)