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>
43 void 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 double fabs_error_m_prev_error = std::fabs(error - prev_error);
59 for (
unsigned int iter = 0; (iter < max_iter) && (fabs_error_m_prev_error > 1e-6); ++iter) {
65 double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
68 for (
unsigned int i = 0; i < nPoints; ++i) {
69 centroid_x += weights[i] * point_cloud_face[(3 * i) + 0];
70 centroid_y += weights[i] * point_cloud_face[(3 * i) + 1];
71 centroid_z += weights[i] * point_cloud_face[(3 * i) + 2];
72 total_w += weights[i];
75 centroid_x /= total_w;
76 centroid_y /= total_w;
77 centroid_z /= total_w;
80 for (
unsigned int i = 0; i < nPoints; ++i) {
81 M[
static_cast<unsigned int>(i)][0] = weights[i] * (point_cloud_face[(3 * i) + 0] - centroid_x);
82 M[
static_cast<unsigned int>(i)][1] = weights[i] * (point_cloud_face[(3 * i) + 1] - centroid_y);
83 M[
static_cast<unsigned int>(i)][2] = weights[i] * (point_cloud_face[(3 * i) + 2] - centroid_z);
91 double smallestSv = W[0];
92 unsigned int indexSmallestSv = 0;
93 unsigned int w_size = W.
size();
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];
118 fabs_error_m_prev_error = std::fabs(error - prev_error);
125 centroid.
resize(3,
false);
126 double total_w = 0.0;
128 for (
unsigned int i = 0; i < nPoints; ++i) {
129 centroid[0] += weights[i] * point_cloud_face[3 * i];
130 centroid[1] += weights[i] * point_cloud_face[(3 * i) + 1];
131 centroid[2] += weights[i] * point_cloud_face[(3 * i) + 2];
132 total_w += weights[i];
135 centroid[0] /= total_w;
136 centroid[1] /= total_w;
137 centroid[2] /= total_w;
140 double A = normal[0], B = normal[1], C = normal[2];
141 double D = -((A * centroid[0]) + (B * centroid[1]) + (C * centroid[2]));
144 plane_equation_estimated.
setABCD(A, B, C, D);
146 normalized_weights = total_w / nPoints;
154 double *confidence_index)
156 if (corners.size() != point3d.size()) {
158 "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
159 point3d.size(), corners.size()));
161 std::vector<vpPoint> pose_points;
162 if (confidence_index !=
nullptr) {
163 *confidence_index = 0.0;
166 size_t point3d_size = point3d.size();
167 for (
size_t i = 0; i < point3d_size; ++i) {
168 pose_points.push_back(point3d[i]);
173 unsigned int top =
static_cast<unsigned int>(std::max<int>(0,
static_cast<int>(bb.
getTop())));
174 unsigned int bottom =
175 static_cast<unsigned int>(std::min<int>(
static_cast<int>(depthMap.
getHeight()) - 1,
static_cast<int>(bb.
getBottom())));
176 unsigned int left =
static_cast<unsigned int>(std::max<int>(0,
static_cast<int>(bb.
getLeft())));
178 static_cast<unsigned int>(std::min<int>(
static_cast<int>(depthMap.
getWidth()) - 1,
static_cast<int>(bb.
getRight())));
180 std::vector<double> points_3d;
181 points_3d.reserve((bottom - top) * (right - left));
182 for (
unsigned int idx_i = top; idx_i < bottom; ++idx_i) {
183 for (
unsigned int idx_j = left; idx_j < right; ++idx_j) {
185 if ((depthMap[idx_i][idx_j] > 0) && (polygon.
isInside(imPt))) {
188 double Z = depthMap[idx_i][idx_j];
189 points_3d.push_back(x * Z);
190 points_3d.push_back(y * Z);
191 points_3d.push_back(Z);
196 unsigned int nb_points_3d =
static_cast<unsigned int>(points_3d.size() / 3);
198 if (nb_points_3d > 4) {
199 std::vector<vpPoint> p, q;
204 double normalized_weights = 0;
205 estimatePlaneEquationSVD(points_3d, plane_equation, centroid, normalized_weights);
207 size_t corners_size = corners.size();
208 for (
size_t j = 0; j < corners_size; ++j) {
212 double Z = plane_equation.
computeZ(x, y);
216 p.push_back(
vpPoint(x * Z, y * Z, Z));
218 pose_points[j].set_x(x);
219 pose_points[j].set_y(y);
222 size_t point3d_size = point3d.size();
223 for (
size_t i = 0; i < point3d_size; ++i) {
224 q.push_back(point3d[i]);
233 if (confidence_index !=
nullptr) {
234 *confidence_index = std::min<double>(1.0, (normalized_weights *
static_cast<double>(nb_points_3d)) / polygon.
getArea());
245 const std::vector<std::vector<vpImagePoint> > &corners,
247 const std::vector<std::vector<vpPoint> > &point3d,
251 if (corners.size() != point3d.size()) {
253 "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
254 point3d.size(), corners.size()));
256 std::vector<vpPoint> pose_points;
257 if (confidence_index !=
nullptr) {
258 *confidence_index = 0.0;
261 size_t point3d_size = point3d.size();
262 for (
size_t i = 0; i < point3d_size; ++i) {
263 std::vector<vpPoint> tagPoint3d = point3d[i];
264 size_t tagpoint3d_size = tagPoint3d.size();
265 for (
size_t j = 0; j < tagpoint3d_size; ++j) {
266 pose_points.push_back(tagPoint3d[j]);
271 double totalArea = 0.0;
274 std::vector<double> tag_points_3d;
277 std::vector<std::vector<double> > tag_points_3d_nonplanar;
278 size_t nb_points_3d_non_planar = 0;
281 size_t corners_size = corners.size();
282 for (
size_t i = 0; i < corners_size; ++i) {
283 std::vector<double> points_3d;
288 totalArea += polygon.
getArea();
290 unsigned int top =
static_cast<unsigned int>(std::max<int>(0,
static_cast<int>(bb.
getTop())));
291 unsigned int bottom =
static_cast<unsigned int>(
292 std::min<int>(
static_cast<int>(depthMap.
getHeight()) - 1,
static_cast<int>(bb.
getBottom())));
293 unsigned int left =
static_cast<unsigned int>(std::max<int>(0,
static_cast<int>(bb.
getLeft())));
295 static_cast<unsigned int>(std::min<int>(
static_cast<int>(depthMap.
getWidth()) - 1,
static_cast<int>(bb.
getRight())));
297 points_3d.reserve((bottom - top) * (right - left));
298 for (
unsigned int idx_i = top; idx_i < bottom; ++idx_i) {
299 for (
unsigned int idx_j = left; idx_j < right; ++idx_j) {
301 if ((depthMap[idx_i][idx_j] > 0) && (polygon.
isInside(imPt))) {
304 double Z = depthMap[idx_i][idx_j];
305 points_3d.push_back(x * Z);
306 points_3d.push_back(y * Z);
307 points_3d.push_back(Z);
314 if (coplanar_points) {
315 tag_points_3d.
insert(tag_points_3d.end(), points_3d.begin(), points_3d.end());
318 tag_points_3d_nonplanar.push_back(points_3d);
319 nb_points_3d_non_planar += points_3d.size();
323 size_t nb_points_3d = 0;
325 if (coplanar_points) {
326 nb_points_3d = tag_points_3d.size() / 3;
329 nb_points_3d = nb_points_3d_non_planar / 3;
332 if (nb_points_3d > 4) {
333 std::vector<vpPoint> p, q;
338 double normalized_weights = 0;
340 if (coplanar_points) {
342 estimatePlaneEquationSVD(tag_points_3d, plane_equation, centroid, normalized_weights);
344 size_t corners_size = corners.size();
345 for (
size_t j = 0; j < corners_size; ++j) {
346 std::vector<vpImagePoint> tag_corner = corners[j];
347 size_t tag_corner_size = tag_corner.size();
348 for (
size_t i = 0; i < tag_corner_size; ++i) {
352 double Z = plane_equation.
computeZ(x, y);
357 p.push_back(
vpPoint(x * Z, y * Z, Z));
358 pose_points[count].set_x(x);
359 pose_points[count].set_y(y);
368 size_t tag_points_3d_nonplanar_size = tag_points_3d_nonplanar.size();
369 for (
size_t k = 0; k < tag_points_3d_nonplanar_size; ++k) {
370 std::vector<double> rec_points_3d = tag_points_3d_nonplanar[k];
371 double tag_normalized_weights = 0;
373 if (rec_points_3d.size() >= 9) {
375 estimatePlaneEquationSVD(rec_points_3d, plane_equation, centroid, tag_normalized_weights);
376 normalized_weights += tag_normalized_weights;
379 std::vector<vpImagePoint> tag_corner = corners[k];
381 size_t tag_corner_size = tag_corner.size();
382 for (
size_t i = 0; i < tag_corner_size; ++i) {
386 double Z = plane_equation.
computeZ(x, y);
391 p.push_back(
vpPoint(x * Z, y * Z, Z));
392 pose_points[count].set_x(x);
393 pose_points[count].set_y(y);
401 count += corners[k].size();
404 normalized_weights = normalized_weights / tag_points_3d_nonplanar.size();
407 size_t point3d_size = point3d.size();
408 for (
size_t i = 0; i < point3d_size; ++i) {
409 std::vector<vpPoint> tagPoint3d = point3d[i];
414 if (coplanar_points) {
415 size_t tag_point3d_size = tagPoint3d.size();
416 for (
size_t j = 0; j < tag_point3d_size; ++j) {
417 q.push_back(tagPoint3d[j]);
421 if (tag_points_3d_nonplanar[i].size() > 0) {
422 size_t tag_point3d_size = tagPoint3d.size();
423 for (
size_t j = 0; j < tag_point3d_size; ++j) {
424 q.push_back(tagPoint3d[j]);
431 if (p.size() == q.size()) {
438 if (confidence_index !=
nullptr) {
439 *confidence_index = std::min<double>(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 ...
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...
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=nullptr)
void addPoints(const std::vector< vpPoint > &lP)
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
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)