34 #include <visp3/vision/vpPlaneEstimation.h>
36 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
37 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
40 #ifdef VISP_HAVE_OPENMP
45 #include <visp3/core/vpMeterPixelConversion.h>
46 #include <visp3/core/vpPixelMeterConversion.h>
47 #include <visp3/core/vpRobust.h>
53 constexpr
auto PlaneSvdMaxError{1e-4};
54 constexpr
auto PlaneSvdMaxIter{10};
56 template <
class T> T &make_ref(T &&x) {
return x; }
68 #ifdef VISP_HAVE_OPENMP
69 auto num_procs = omp_get_num_procs();
70 num_procs = num_procs > 2 ? num_procs - 2 : num_procs;
71 omp_set_num_threads(num_procs);
74 auto compute_centroid = [=](
const std::vector<double> &point_cloud,
const vpColVector &weights) {
75 double cent_x{0.}, cent_y{0.}, cent_z{0.}, total_w{0.};
78 #ifdef VISP_HAVE_OPENMP
79 #pragma omp parallel for num_threads(num_procs) reduction(+ : total_w, cent_x, cent_y, cent_z)
81 for (i = 0; i < static_cast<int>(weights.size()); i++) {
82 const auto pt_cloud_start_idx = 3 * i;
84 cent_x += weights[i] * point_cloud[pt_cloud_start_idx + 0];
85 cent_y += weights[i] * point_cloud[pt_cloud_start_idx + 1];
86 cent_z += weights[i] * point_cloud[pt_cloud_start_idx + 2];
88 total_w += weights[i];
91 return std::make_tuple(
vpColVector{cent_x, cent_y, cent_z}, total_w);
95 auto prev_error = 1e3;
96 auto error = prev_error - 1;
97 const unsigned int nPoints =
static_cast<unsigned int>(point_cloud.size() / 3);
106 for (
auto iter = 0u; iter < PlaneSvdMaxIter && std::fabs(error - prev_error) > PlaneSvdMaxError; iter++) {
112 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_17)
113 auto [centroid, total_w] = compute_centroid(point_cloud, weights);
120 std::tie(centroid, total_w) = compute_centroid(point_cloud, weights);
127 #ifdef VISP_HAVE_OPENMP
128 #pragma omp parallel for num_threads(num_procs)
130 for (i = 0; i < static_cast<int>(nPoints); i++) {
131 const auto pt_cloud_start_idx = 3 * i;
133 M[i][0] = weights[i] * (point_cloud[pt_cloud_start_idx + 0] - centroid[0]);
134 M[i][1] = weights[i] * (point_cloud[pt_cloud_start_idx + 1] - centroid[1]);
135 M[i][2] = weights[i] * (point_cloud[pt_cloud_start_idx + 2] - centroid[2]);
143 auto smallestSv = W[0];
144 auto indexSmallestSv = 0u;
145 for (
auto i = 1u; i < W.size(); i++) {
146 if (W[i] < smallestSv) {
152 normal = V.getCol(indexSmallestSv);
155 const auto A = normal[0], B = normal[1], C = normal[2];
156 const auto D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
161 const auto smth = std::hypot(A, B, C);
163 #ifdef VISP_HAVE_OPENMP
164 #pragma omp parallel for num_threads(num_procs) reduction(+ : error)
166 for (i = 0; i < static_cast<int>(nPoints); i++) {
167 const auto pt_cloud_start_idx = 3 * i;
169 residues[i] = std::fabs(A * point_cloud[pt_cloud_start_idx + 0] + B * point_cloud[pt_cloud_start_idx + 1] +
170 C * point_cloud[pt_cloud_start_idx + 2] + D) /
173 error += weights[i] * residues[i];
183 auto [centroid, total_w] = compute_centroid(point_cloud, weights);
187 const auto A = normal[0], B = normal[1], C = normal[2];
188 const auto D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
207 std::optional<vpPlane>
210 const unsigned int avg_nb_of_pts_to_estimate,
213 #ifdef VISP_HAVE_OPENMP
214 auto num_procs = omp_get_num_procs();
215 num_procs = num_procs > 2 ? num_procs - 2 : num_procs;
216 omp_set_num_threads(num_procs);
227 static_cast<double>(I_depth_raw.
getHeight())};
228 for (
const auto &roi_corner : roi.
getCorners()) {
229 if (img_bound.isInside(roi_corner)) {
237 if ( ! roi.
isInside( img_bound.getTopLeft() ) ||
238 ! roi.
isInside( img_bound.getTopRight() ) ||
239 ! roi.
isInside( img_bound.getBottomLeft() ) ||
240 ! roi.
isInside( img_bound.getBottomRight() ) )
249 const int roi_top =
static_cast<int>(std::max(0., roi_bb.getTop()));
250 const int roi_bottom =
static_cast<int>(std::min(
static_cast<double>(I_depth_raw.
getHeight()), roi_bb.getBottom()));
251 const int roi_left =
static_cast<int>(std::max(0., roi_bb.getLeft()));
252 const int roi_right =
static_cast<int>(std::min(
static_cast<double>(I_depth_raw.
getWidth()), roi_bb.getRight()));
255 unsigned int subsample_factor =
256 static_cast<int>(sqrt(((roi_right - roi_left) * (roi_bottom - roi_top)) / avg_nb_of_pts_to_estimate));
257 subsample_factor =
vpMath::clamp(subsample_factor, 1u, MaxSubSampFactorToEstimatePlane);
260 std::vector<double> pt_cloud{};
262 #if defined(VISP_HAVE_OPENMP) && !(_WIN32)
265 #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 ) ) ))
266 #pragma omp parallel for num_threads(num_procs) collapse(2) reduction(merge : pt_cloud)
268 for (
int i = roi_top; i < roi_bottom; i = i + subsample_factor) {
269 for (
int j = roi_left; j < roi_right; j = j + subsample_factor) {
270 const auto pixel =
vpImagePoint{
static_cast<double>(i),
static_cast<double>(j)};
271 if (I_depth_raw[i][j] != 0 && isInside(pixel)) {
274 const double Z = I_depth_raw[i][j] * depth_scale;
276 pt_cloud.push_back(x * Z);
277 pt_cloud.push_back(y * Z);
278 pt_cloud.push_back(Z);
283 if (pt_cloud.size() < MinPointNbToEstimatePlane) {
290 const auto plane = estimatePlaneEquationSVD(pt_cloud, weights);
294 for (
auto i = 0u; i < weights.size(); i++) {
295 const auto X{pt_cloud[3 * i + 0]}, Y{pt_cloud[3 * i + 1]}, Z{pt_cloud[3 * i + 2]};
300 const int b =
static_cast<int>(std::max(0., 255 * (1 - 2 * weights[i])));
301 const int r =
static_cast<int>(std::max(0., 255 * (2 * weights[i] - 1)));
302 const int g = 255 - b - r;
304 heat_map->get()[
static_cast<int>(ip.get_i())][
static_cast<int>(ip.get_j())] =
vpColor(r, g, b);
308 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 functionalities.
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)