34 #include <visp3/vision/vpPlaneEstimation.h>
37 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
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 ((__cplusplus > 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG > 201703L)))
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]);
191 return { A, B, C, D };
196 std::optional<vpPlane>
197 vpPlaneEstimation::estimatePlane(
const vpImage<uint16_t> &I_depth_raw,
double depth_scale,
199 const unsigned int avg_nb_of_pts_to_estimate,
202 #ifdef VISP_HAVE_OPENMP
203 auto num_procs = omp_get_num_procs();
204 num_procs = num_procs > 2 ? num_procs - 2 : num_procs;
205 omp_set_num_threads(num_procs);
216 static_cast<double>(I_depth_raw.
getHeight()) };
217 for (
const auto &roi_corner : roi.
getCorners()) {
218 if (img_bound.isInside(roi_corner)) {
226 if (!roi.
isInside(img_bound.getTopLeft()) ||
227 !roi.
isInside(img_bound.getTopRight()) ||
228 !roi.
isInside(img_bound.getBottomLeft()) ||
229 !roi.
isInside(img_bound.getBottomRight()))
238 const int roi_top =
static_cast<int>(std::max(0., roi_bb.getTop()));
239 const int roi_bottom =
static_cast<int>(std::min(
static_cast<double>(I_depth_raw.
getHeight()), roi_bb.getBottom()));
240 const int roi_left =
static_cast<int>(std::max(0., roi_bb.getLeft()));
241 const int roi_right =
static_cast<int>(std::min(
static_cast<double>(I_depth_raw.
getWidth()), roi_bb.getRight()));
244 unsigned int subsample_factor =
245 static_cast<int>(sqrt(((roi_right - roi_left) * (roi_bottom - roi_top)) / avg_nb_of_pts_to_estimate));
246 subsample_factor =
vpMath::clamp(subsample_factor, 1u, MaxSubSampFactorToEstimatePlane);
249 std::vector<double> pt_cloud {};
251 #if defined(VISP_HAVE_OPENMP) && !(_WIN32)
254 #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 ) ) ))
255 #pragma omp parallel for num_threads(num_procs) collapse(2) reduction(merge : pt_cloud)
257 for (
int i = roi_top; i < roi_bottom; i = i + subsample_factor) {
258 for (
int j = roi_left; j < roi_right; j = j + subsample_factor) {
259 const auto pixel =
vpImagePoint {
static_cast<double>(i),
static_cast<double>(j) };
260 if (I_depth_raw[i][j] != 0 && isInside(pixel)) {
261 double x { 0. }, y { 0. };
263 const double Z = I_depth_raw[i][j] * depth_scale;
265 pt_cloud.push_back(x * Z);
266 pt_cloud.push_back(y * Z);
267 pt_cloud.push_back(Z);
272 if (pt_cloud.size() < MinPointNbToEstimatePlane) {
279 const auto plane = estimatePlaneEquationSVD(pt_cloud, weights);
283 for (
auto i = 0u; i < weights.size(); i++) {
284 const auto X { pt_cloud[3 * i + 0] }, Y { pt_cloud[3 * i + 1] }, Z { pt_cloud[3 * i + 2] };
289 const int b =
static_cast<int>(std::max(0., 255 * (1 - 2 * weights[i])));
290 const int r =
static_cast<int>(std::max(0., 255 * (2 * weights[i] - 1)));
291 const int g = 255 - b - r;
293 heat_map->get()[
static_cast<int>(ip.get_i())][
static_cast<int>(ip.get_j())] =
vpColor(r, g, b);
298 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)
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)