39 #include <visp3/vision/vpPlaneEstimation.h>
41 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
42 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
45 #ifdef VISP_HAVE_OPENMP
50 #include <visp3/core/vpMeterPixelConversion.h>
51 #include <visp3/core/vpPixelMeterConversion.h>
52 #include <visp3/core/vpRobust.h>
58 constexpr
auto PlaneSvdMaxError{1e-4};
59 constexpr
auto PlaneSvdMaxIter{10};
61 template <
class T> T &make_ref(T &&x) {
return x; }
73 #ifdef VISP_HAVE_OPENMP
74 auto num_procs = omp_get_num_procs();
75 num_procs = num_procs > 2 ? num_procs - 2 : num_procs;
76 omp_set_num_threads(num_procs);
79 auto compute_centroid = [=](
const std::vector<double> &point_cloud,
const vpColVector &weights) {
80 double cent_x{0.}, cent_y{0.}, cent_z{0.}, total_w{0.};
83 #ifdef VISP_HAVE_OPENMP
84 #pragma omp parallel for num_threads(num_procs) reduction(+ : total_w, cent_x, cent_y, cent_z)
86 for (i = 0; i < static_cast<int>(weights.size()); i++) {
87 const auto pt_cloud_start_idx = 3 * i;
89 cent_x += weights[i] * point_cloud[pt_cloud_start_idx + 0];
90 cent_y += weights[i] * point_cloud[pt_cloud_start_idx + 1];
91 cent_z += weights[i] * point_cloud[pt_cloud_start_idx + 2];
93 total_w += weights[i];
96 return std::make_tuple(
vpColVector{cent_x, cent_y, cent_z}, total_w);
100 auto prev_error = 1e3;
101 auto error = prev_error - 1;
102 const unsigned int nPoints =
static_cast<unsigned int>(point_cloud.size() / 3);
111 for (
auto iter = 0u; iter < PlaneSvdMaxIter && std::fabs(error - prev_error) > PlaneSvdMaxError; iter++) {
117 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_17)
118 auto [centroid, total_w] = compute_centroid(point_cloud, weights);
125 std::tie(centroid, total_w) = compute_centroid(point_cloud, weights);
132 #ifdef VISP_HAVE_OPENMP
133 #pragma omp parallel for num_threads(num_procs)
135 for (i = 0; i < static_cast<int>(nPoints); i++) {
136 const auto pt_cloud_start_idx = 3 * i;
138 M[i][0] = weights[i] * (point_cloud[pt_cloud_start_idx + 0] - centroid[0]);
139 M[i][1] = weights[i] * (point_cloud[pt_cloud_start_idx + 1] - centroid[1]);
140 M[i][2] = weights[i] * (point_cloud[pt_cloud_start_idx + 2] - centroid[2]);
148 auto smallestSv = W[0];
149 auto indexSmallestSv = 0u;
150 for (
auto i = 1u; i < W.size(); i++) {
151 if (W[i] < smallestSv) {
157 normal = V.getCol(indexSmallestSv);
160 const auto A = normal[0], B = normal[1], C = normal[2];
161 const auto D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
166 const auto smth = std::hypot(A, B, C);
168 #ifdef VISP_HAVE_OPENMP
169 #pragma omp parallel for num_threads(num_procs) reduction(+ : error)
171 for (i = 0; i < static_cast<int>(nPoints); i++) {
172 const auto pt_cloud_start_idx = 3 * i;
174 residues[i] = std::fabs(A * point_cloud[pt_cloud_start_idx + 0] + B * point_cloud[pt_cloud_start_idx + 1] +
175 C * point_cloud[pt_cloud_start_idx + 2] + D) /
178 error += weights[i] * residues[i];
188 auto [centroid, total_w] = compute_centroid(point_cloud, weights);
192 const auto A = normal[0], B = normal[1], C = normal[2];
193 const auto D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
212 std::optional<vpPlane>
215 const unsigned int avg_nb_of_pts_to_estimate,
218 #ifdef VISP_HAVE_OPENMP
219 auto num_procs = omp_get_num_procs();
220 num_procs = num_procs > 2 ? num_procs - 2 : num_procs;
221 omp_set_num_threads(num_procs);
232 static_cast<double>(I_depth_raw.
getHeight())};
233 for (
const auto &roi_corner : roi.
getCorners()) {
234 if (img_bound.isInside(roi_corner)) {
242 if ( ! roi.
isInside( img_bound.getTopLeft() ) ||
243 ! roi.
isInside( img_bound.getTopRight() ) ||
244 ! roi.
isInside( img_bound.getBottomLeft() ) ||
245 ! roi.
isInside( img_bound.getBottomRight() ) )
254 const int roi_top =
static_cast<int>(std::max(0., roi_bb.getTop()));
255 const int roi_bottom =
static_cast<int>(std::min(
static_cast<double>(I_depth_raw.
getHeight()), roi_bb.getBottom()));
256 const int roi_left =
static_cast<int>(std::max(0., roi_bb.getLeft()));
257 const int roi_right =
static_cast<int>(std::min(
static_cast<double>(I_depth_raw.
getWidth()), roi_bb.getRight()));
260 unsigned int subsample_factor =
261 static_cast<int>(sqrt(((roi_right - roi_left) * (roi_bottom - roi_top)) / avg_nb_of_pts_to_estimate));
262 subsample_factor =
vpMath::clamp(subsample_factor, 1u, MaxSubSampFactorToEstimatePlane);
265 std::vector<double> pt_cloud{};
267 #if defined(VISP_HAVE_OPENMP) && !(_WIN32)
270 #pragma omp declare reduction (merge : std::vector<double> : omp_out.insert( end( omp_out ), std::make_move_iterator( begin( omp_in ) ), std::make_move_iterator( end( omp_in ) ) ))
271 #pragma omp parallel for num_threads(num_procs) collapse(2) reduction(merge : pt_cloud)
273 for (
int i = roi_top; i < roi_bottom; i = i + subsample_factor) {
274 for (
int j = roi_left; j < roi_right; j = j + subsample_factor) {
275 const auto pixel =
vpImagePoint{
static_cast<double>(i),
static_cast<double>(j)};
276 if (I_depth_raw[i][j] != 0 && isInside(pixel)) {
279 const double Z = I_depth_raw[i][j] * depth_scale;
281 pt_cloud.push_back(x * Z);
282 pt_cloud.push_back(y * Z);
283 pt_cloud.push_back(Z);
288 if (pt_cloud.size() < MinPointNbToEstimatePlane) {
295 const auto plane = estimatePlaneEquationSVD(pt_cloud, weights);
299 for (
auto i = 0u; i < weights.size(); i++) {
300 const auto X{pt_cloud[3 * i + 0]}, Y{pt_cloud[3 * i + 1]}, Z{pt_cloud[3 * i + 2]};
305 const int b =
static_cast<int>(std::max(0., 255 * (1 - 2 * weights[i])));
306 const int r =
static_cast<int>(std::max(0., 255 * (2 * weights[i] - 1)));
307 const int g = 255 - b - r;
309 heat_map->get()[
static_cast<int>(ip.get_i())][
static_cast<int>(ip.get_j())] =
vpColor(r, g, b);
313 return estimatePlaneEquationSVD(pt_cloud);
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionnalities.
static const vpColor black
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
unsigned int getHeight() const
static T clamp(const T &v, const T &lower, const T &upper)
Implementation of a matrix and operations on matrices.
void svd(vpColVector &w, vpMatrix &V)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
static std::optional< vpPlane > estimatePlane(const vpImage< uint16_t > &I_depth_raw, double depth_scale, const vpCameraParameters &depth_intrinsics, const vpPolygon &roi, const unsigned int avg_nb_of_pts_to_estimate=500, std::optional< std::reference_wrapper< vpImage< vpRGBa > > > heat_map={})
This class defines the container for a plane geometrical structure.
Defines a generic 2D polygon.
const std::vector< vpImagePoint > & getCorners() const
vpRect getBoundingBox() const
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
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)