36 #include <visp3/vision/vpPose.h> 37 #include <visp3/core/vpPolygon.h> 38 #include <visp3/core/vpPixelMeterConversion.h> 39 #include <visp3/core/vpRobust.h> 42 vpHomogeneousMatrix compute3d3dTransformation(
const std::vector<vpPoint>& p,
const std::vector<vpPoint>& q) {
43 double N =
static_cast<double>(p.size());
47 for (
size_t i = 0; i < p.size(); i++) {
48 for (
unsigned int j = 0; j < 3; j++) {
49 p_bar[j] += p[i].oP[j];
50 q_bar[j] += q[i].oP[j];
54 for (
unsigned int j = 0; j < 3; j++) {
59 vpMatrix pc(static_cast<unsigned int>(p.size()), 3);
60 vpMatrix qc(static_cast<unsigned int>(q.size()), 3);
62 for (
unsigned int i = 0; i < static_cast<unsigned int>(p.size()); i++) {
63 for (
unsigned int j = 0; j < 3; j++) {
64 pc[i][j] = p[i].oP[j] - p_bar[j];
65 qc[i][j] = q[i].oP[j] - q_bar[j];
89 for (
unsigned int i = 0; i < 3; i++) {
90 for (
unsigned int j = 0; j < 3; j++) {
99 void estimatePlaneEquationSVD(
const std::vector<double> &point_cloud_face,
101 double &normalized_weights)
103 unsigned int max_iter = 10;
104 double prev_error = 1e3;
105 double error = 1e3 - 1;
106 unsigned int nPoints =
static_cast<unsigned int>(point_cloud_face.size() / 3);
115 plane_equation_estimated.
resize(4,
false);
116 for (
unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
122 double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
123 double total_w = 0.0;
125 for (
unsigned int i = 0; i < nPoints; i++) {
126 centroid_x += weights[i] * point_cloud_face[3 * i + 0];
127 centroid_y += weights[i] * point_cloud_face[3 * i + 1];
128 centroid_z += weights[i] * point_cloud_face[3 * i + 2];
129 total_w += weights[i];
132 centroid_x /= total_w;
133 centroid_y /= total_w;
134 centroid_z /= total_w;
137 for (
unsigned int i = 0; i < nPoints; i++) {
138 M[
static_cast<unsigned int>(i)][0] = weights[i] * (point_cloud_face[3 * i + 0] - centroid_x);
139 M[
static_cast<unsigned int>(i)][1] = weights[i] * (point_cloud_face[3 * i + 1] - centroid_y);
140 M[
static_cast<unsigned int>(i)][2] = weights[i] * (point_cloud_face[3 * i + 2] - centroid_z);
148 double smallestSv = W[0];
149 unsigned int indexSmallestSv = 0;
150 for (
unsigned int i = 1; i < W.
size(); i++) {
151 if (W[i] < smallestSv) {
157 normal = V.
getCol(indexSmallestSv);
160 double A = normal[0], B = normal[1], C = normal[2];
161 double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
164 plane_equation_estimated[0] = A;
165 plane_equation_estimated[1] = B;
166 plane_equation_estimated[2] = C;
167 plane_equation_estimated[3] = D;
172 for (
unsigned int i = 0; i < nPoints; i++) {
173 residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
174 C * point_cloud_face[3 * i + 2] + D) / sqrt(A * A + B * B + C * C);
175 error += weights[i] * residues[i];
184 centroid.
resize(3,
false);
185 double total_w = 0.0;
187 for (
unsigned int i = 0; i < nPoints; i++) {
188 centroid[0] += weights[i] * point_cloud_face[3 * i];
189 centroid[1] += weights[i] * point_cloud_face[3 * i + 1];
190 centroid[2] += weights[i] * point_cloud_face[3 * i + 2];
191 total_w += weights[i];
194 centroid[0] /= total_w;
195 centroid[1] /= total_w;
196 centroid[2] /= total_w;
199 double A = normal[0], B = normal[1], C = normal[2];
200 double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
203 plane_equation_estimated[0] = A;
204 plane_equation_estimated[1] = B;
205 plane_equation_estimated[2] = C;
206 plane_equation_estimated[3] = D;
208 normalized_weights = total_w / nPoints;
211 double computeZMethod1(
const vpColVector& plane_equation,
double x,
double y) {
212 return -plane_equation[3] / (plane_equation[0]*x + plane_equation[1]*y + plane_equation[2]);
218 for (
unsigned int i = 0; i < cMo.
getRows() && valid; i++) {
219 for (
unsigned int j = 0; j < cMo.
getCols() && valid; j++) {
254 if (corners.size() != point3d.size()) {
256 point3d.size(), corners.size()));
258 std::vector<vpPoint> pose_points;
259 if (confidence_index != NULL) {
260 *confidence_index = 0.0;
263 for (
size_t i = 0; i < point3d.size(); i ++) {
264 pose_points.push_back(point3d[i]);
269 unsigned int top =
static_cast<unsigned int>(std::max( 0, static_cast<int>(bb.
getTop()) ));
270 unsigned int bottom =
static_cast<unsigned int>(std::min( static_cast<int>(depthMap.
getHeight())-1, static_cast<int>(bb.
getBottom()) ));
271 unsigned int left =
static_cast<unsigned int>(std::max( 0, static_cast<int>(bb.
getLeft()) ));
272 unsigned int right =
static_cast<unsigned int>(std::min( static_cast<int>(depthMap.
getWidth())-1, static_cast<int>(bb.
getRight()) ));
274 std::vector<double> points_3d;
275 points_3d.reserve( (bottom-top)*(right-left) );
276 for (
unsigned int idx_i = top; idx_i < bottom; idx_i++) {
277 for (
unsigned int idx_j = left; idx_j < right; idx_j++) {
279 if (depthMap[idx_i][idx_j] > 0 && polygon.
isInside(imPt)) {
282 double Z = depthMap[idx_i][idx_j];
283 points_3d.push_back(x*Z);
284 points_3d.push_back(y*Z);
285 points_3d.push_back(Z);
290 unsigned int nb_points_3d =
static_cast<unsigned int>(points_3d.size() / 3);
292 if (nb_points_3d > 4) {
293 std::vector<vpPoint> p, q;
297 double normalized_weights = 0;
298 estimatePlaneEquationSVD(points_3d, plane_equation, centroid, normalized_weights);
300 for (
size_t j = 0; j < corners.size(); j++) {
304 double Z = computeZMethod1(plane_equation, x, y);
308 p.push_back(
vpPoint(x*Z, y*Z, Z));
310 pose_points[j].set_x(x);
311 pose_points[j].set_y(y);
314 for (
size_t i = 0; i < point3d.size(); i ++) {
315 q.push_back(point3d[i]);
318 cMo = compute3d3dTransformation(p, q);
320 if (validPose(cMo)) {
324 if (confidence_index != NULL) {
325 *confidence_index = std::min(1.0, normalized_weights * static_cast<double>(nb_points_3d) / polygon.
getArea());
void svd(vpColVector &w, vpMatrix &V)
Implementation of a matrix and operations on matrices.
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
double det(vpDetMethod method=LU_DECOMPOSITION) const
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void addPoints(const std::vector< vpPoint > &lP)
error that can be emited by ViSP classes.
unsigned int getRows() const
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)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
unsigned int size() const
Return the number of elements of the 2D array.
vpRect getBoundingBox() const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
unsigned int getCols() const
Defines a generic 2D polygon.
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...
Generic class defining intrinsic camera parameters.
vpColVector getCol(unsigned int j) const
static bool isNaN(double value)
void resize(unsigned int i, bool flagNullify=true)
unsigned int getHeight() const
Implementation of column vector and the associated operations.
Contains an M-estimator and various influence function.
Tukey influence function.
Defines a rectangle in the plane.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void setMinMedianAbsoluteDeviation(double mad_min)
unsigned int getWidth() const