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>
43void estimatePlaneEquationSVD(
const std::vector<double> &point_cloud_face,
vpPlane &plane_equation_estimated,
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);
58 for (
unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
64 double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
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];
74 centroid_x /= total_w;
75 centroid_y /= total_w;
76 centroid_z /= total_w;
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);
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) {
99 normal = V.
getCol(indexSmallestSv);
102 double A = normal[0], B = normal[1], C = normal[2];
103 double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
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];
121 centroid.
resize(3,
false);
122 double total_w = 0.0;
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];
131 centroid[0] /= total_w;
132 centroid[1] /= total_w;
133 centroid[2] /= total_w;
136 double A = normal[0], B = normal[1], C = normal[2];
137 double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
140 plane_equation_estimated.
setABCD(A, B, C, D);
142 normalized_weights = total_w / nPoints;
170 double *confidence_index)
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()));
177 std::vector<vpPoint> pose_points;
178 if (confidence_index != NULL) {
179 *confidence_index = 0.0;
182 for (
size_t i = 0; i < point3d.size(); i++) {
183 pose_points.push_back(point3d[i]);
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())));
193 static_cast<unsigned int>(std::min(
static_cast<int>(depthMap.
getWidth()) - 1,
static_cast<int>(bb.
getRight())));
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++) {
200 if (depthMap[idx_i][idx_j] > 0 && polygon.
isInside(imPt)) {
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);
211 unsigned int nb_points_3d =
static_cast<unsigned int>(points_3d.size() / 3);
213 if (nb_points_3d > 4) {
214 std::vector<vpPoint> p, q;
219 double normalized_weights = 0;
220 estimatePlaneEquationSVD(points_3d, plane_equation, centroid, normalized_weights);
222 for (
size_t j = 0; j < corners.size(); j++) {
226 double Z = plane_equation.
computeZ(x, y);
230 p.push_back(
vpPoint(x * Z, y * Z, Z));
232 pose_points[j].set_x(x);
233 pose_points[j].set_y(y);
236 for (
size_t i = 0; i < point3d.size(); i++) {
237 q.push_back(point3d[i]);
246 if (confidence_index != NULL) {
247 *confidence_index = std::min(1.0, normalized_weights *
static_cast<double>(nb_points_3d) / polygon.
getArea());
290 const std::vector<std::vector<vpImagePoint> > &corners,
292 const std::vector<std::vector<vpPoint> > &point3d,
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()));
301 std::vector<vpPoint> pose_points;
302 if (confidence_index != NULL) {
303 *confidence_index = 0.0;
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]);
314 double totalArea = 0.0;
317 std::vector<double> tag_points_3d;
320 std::vector<std::vector<double> > tag_points_3d_nonplanar;
321 size_t nb_points_3d_non_planar = 0;
324 for (
size_t i = 0; i < corners.size(); i++) {
325 std::vector<double> points_3d;
330 totalArea += polygon.
getArea();
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())));
337 static_cast<unsigned int>(std::min(
static_cast<int>(depthMap.
getWidth()) - 1,
static_cast<int>(bb.
getRight())));
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++) {
343 if (depthMap[idx_i][idx_j] > 0 && polygon.
isInside(imPt)) {
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);
356 if (coplanar_points) {
357 tag_points_3d.
insert(tag_points_3d.end(), points_3d.begin(), points_3d.end());
359 tag_points_3d_nonplanar.push_back(points_3d);
360 nb_points_3d_non_planar += points_3d.size();
364 size_t nb_points_3d = 0;
366 if (coplanar_points) {
367 nb_points_3d = tag_points_3d.size() / 3;
369 nb_points_3d = nb_points_3d_non_planar / 3;
372 if (nb_points_3d > 4) {
373 std::vector<vpPoint> p, q;
378 double normalized_weights = 0;
380 if (coplanar_points) {
382 estimatePlaneEquationSVD(tag_points_3d, plane_equation, centroid, normalized_weights);
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++) {
390 double Z = plane_equation.
computeZ(x, y);
395 p.push_back(
vpPoint(x * Z, y * Z, Z));
396 pose_points[count].set_x(x);
397 pose_points[count].set_y(y);
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;
409 if (rec_points_3d.size() >= 9) {
411 estimatePlaneEquationSVD(rec_points_3d, plane_equation, centroid, tag_normalized_weights);
412 normalized_weights += tag_normalized_weights;
415 std::vector<vpImagePoint> tag_corner = corners[k];
417 for (
size_t i = 0; i < tag_corner.size(); i++) {
421 double Z = plane_equation.
computeZ(x, y);
426 p.push_back(
vpPoint(x * Z, y * Z, Z));
427 pose_points[count].set_x(x);
428 pose_points[count].set_y(y);
435 count += corners[k].size();
438 normalized_weights = normalized_weights / tag_points_3d_nonplanar.size();
441 for (
size_t i = 0; i < point3d.size(); i++) {
442 std::vector<vpPoint> tagPoint3d = point3d[i];
447 if (coplanar_points) {
448 for (
size_t j = 0; j < tagPoint3d.size(); j++) {
449 q.push_back(tagPoint3d[j]);
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]);
461 if (p.size() == q.size()) {
468 if (confidence_index != NULL) {
469 *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 emitted 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 ...
Definition of the vpImage class member functions.
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)