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 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;
150 double *confidence_index)
152 if (corners.size() != point3d.size()) {
154 "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
155 point3d.size(), corners.size()));
157 std::vector<vpPoint> pose_points;
158 if (confidence_index !=
nullptr) {
159 *confidence_index = 0.0;
162 for (
size_t i = 0; i < point3d.size(); i++) {
163 pose_points.push_back(point3d[i]);
168 unsigned int top =
static_cast<unsigned int>(std::max(0,
static_cast<int>(bb.
getTop())));
169 unsigned int bottom =
170 static_cast<unsigned int>(std::min(
static_cast<int>(depthMap.
getHeight()) - 1,
static_cast<int>(bb.
getBottom())));
171 unsigned int left =
static_cast<unsigned int>(std::max(0,
static_cast<int>(bb.
getLeft())));
173 static_cast<unsigned int>(std::min(
static_cast<int>(depthMap.
getWidth()) - 1,
static_cast<int>(bb.
getRight())));
175 std::vector<double> points_3d;
176 points_3d.reserve((bottom - top) * (right - left));
177 for (
unsigned int idx_i = top; idx_i < bottom; idx_i++) {
178 for (
unsigned int idx_j = left; idx_j < right; idx_j++) {
180 if (depthMap[idx_i][idx_j] > 0 && polygon.
isInside(imPt)) {
183 double Z = depthMap[idx_i][idx_j];
184 points_3d.push_back(x * Z);
185 points_3d.push_back(y * Z);
186 points_3d.push_back(Z);
191 unsigned int nb_points_3d =
static_cast<unsigned int>(points_3d.size() / 3);
193 if (nb_points_3d > 4) {
194 std::vector<vpPoint> p, q;
199 double normalized_weights = 0;
200 estimatePlaneEquationSVD(points_3d, plane_equation, centroid, normalized_weights);
202 for (
size_t j = 0; j < corners.size(); j++) {
206 double Z = plane_equation.
computeZ(x, y);
210 p.push_back(
vpPoint(x * Z, y * Z, Z));
212 pose_points[j].set_x(x);
213 pose_points[j].set_y(y);
216 for (
size_t i = 0; i < point3d.size(); i++) {
217 q.push_back(point3d[i]);
226 if (confidence_index !=
nullptr) {
227 *confidence_index = std::min(1.0, normalized_weights *
static_cast<double>(nb_points_3d) / polygon.
getArea());
238 const std::vector<std::vector<vpImagePoint> > &corners,
240 const std::vector<std::vector<vpPoint> > &point3d,
244 if (corners.size() != point3d.size()) {
246 "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size",
247 point3d.size(), corners.size()));
249 std::vector<vpPoint> pose_points;
250 if (confidence_index !=
nullptr) {
251 *confidence_index = 0.0;
254 for (
size_t i = 0; i < point3d.size(); i++) {
255 std::vector<vpPoint> tagPoint3d = point3d[i];
256 for (
size_t j = 0; j < tagPoint3d.size(); j++) {
257 pose_points.push_back(tagPoint3d[j]);
262 double totalArea = 0.0;
265 std::vector<double> tag_points_3d;
268 std::vector<std::vector<double> > tag_points_3d_nonplanar;
269 size_t nb_points_3d_non_planar = 0;
272 for (
size_t i = 0; i < corners.size(); i++) {
273 std::vector<double> points_3d;
278 totalArea += polygon.
getArea();
280 unsigned int top =
static_cast<unsigned int>(std::max(0,
static_cast<int>(bb.
getTop())));
281 unsigned int bottom =
static_cast<unsigned int>(
282 std::min(
static_cast<int>(depthMap.
getHeight()) - 1,
static_cast<int>(bb.
getBottom())));
283 unsigned int left =
static_cast<unsigned int>(std::max(0,
static_cast<int>(bb.
getLeft())));
285 static_cast<unsigned int>(std::min(
static_cast<int>(depthMap.
getWidth()) - 1,
static_cast<int>(bb.
getRight())));
287 points_3d.reserve((bottom - top) * (right - left));
288 for (
unsigned int idx_i = top; idx_i < bottom; idx_i++) {
289 for (
unsigned int idx_j = left; idx_j < right; idx_j++) {
291 if (depthMap[idx_i][idx_j] > 0 && polygon.
isInside(imPt)) {
294 double Z = depthMap[idx_i][idx_j];
295 points_3d.push_back(x * Z);
296 points_3d.push_back(y * Z);
297 points_3d.push_back(Z);
304 if (coplanar_points) {
305 tag_points_3d.
insert(tag_points_3d.end(), points_3d.begin(), points_3d.end());
308 tag_points_3d_nonplanar.push_back(points_3d);
309 nb_points_3d_non_planar += points_3d.size();
313 size_t nb_points_3d = 0;
315 if (coplanar_points) {
316 nb_points_3d = tag_points_3d.size() / 3;
319 nb_points_3d = nb_points_3d_non_planar / 3;
322 if (nb_points_3d > 4) {
323 std::vector<vpPoint> p, q;
328 double normalized_weights = 0;
330 if (coplanar_points) {
332 estimatePlaneEquationSVD(tag_points_3d, plane_equation, centroid, normalized_weights);
334 for (
size_t j = 0; j < corners.size(); j++) {
335 std::vector<vpImagePoint> tag_corner = corners[j];
336 for (
size_t i = 0; i < tag_corner.size(); i++) {
340 double Z = plane_equation.
computeZ(x, y);
345 p.push_back(
vpPoint(x * Z, y * Z, Z));
346 pose_points[count].set_x(x);
347 pose_points[count].set_y(y);
356 for (
size_t k = 0; k < tag_points_3d_nonplanar.size(); k++) {
357 std::vector<double> rec_points_3d = tag_points_3d_nonplanar[k];
358 double tag_normalized_weights = 0;
360 if (rec_points_3d.size() >= 9) {
362 estimatePlaneEquationSVD(rec_points_3d, plane_equation, centroid, tag_normalized_weights);
363 normalized_weights += tag_normalized_weights;
366 std::vector<vpImagePoint> tag_corner = corners[k];
368 for (
size_t i = 0; i < tag_corner.size(); i++) {
372 double Z = plane_equation.
computeZ(x, y);
377 p.push_back(
vpPoint(x * Z, y * Z, Z));
378 pose_points[count].set_x(x);
379 pose_points[count].set_y(y);
387 count += corners[k].size();
390 normalized_weights = normalized_weights / tag_points_3d_nonplanar.size();
393 for (
size_t i = 0; i < point3d.size(); i++) {
394 std::vector<vpPoint> tagPoint3d = point3d[i];
399 if (coplanar_points) {
400 for (
size_t j = 0; j < tagPoint3d.size(); j++) {
401 q.push_back(tagPoint3d[j]);
405 if (tag_points_3d_nonplanar[i].size() > 0) {
406 for (
size_t j = 0; j < tagPoint3d.size(); j++) {
407 q.push_back(tagPoint3d[j]);
414 if (p.size() == q.size()) {
421 if (confidence_index !=
nullptr) {
422 *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 ...
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)