Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPlaneEstimation.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Class for Plane Estimation.
32 */
33
34#include <visp3/vision/vpPlaneEstimation.h>
35
36#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
37 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
38
39// OpenMP
40#ifdef VISP_HAVE_OPENMP
41#include <omp.h>
42#endif
43
44// Core
45#include <visp3/core/vpMeterPixelConversion.h>
46#include <visp3/core/vpPixelMeterConversion.h>
47#include <visp3/core/vpRobust.h>
48
49// Local helpers
50namespace
51{
52
53constexpr auto PlaneSvdMaxError{1e-4};
54constexpr auto PlaneSvdMaxIter{10};
55
56template <class T> T &make_ref(T &&x) { return x; }
57
65vpPlane estimatePlaneEquationSVD(const std::vector<double> &point_cloud, vpColVector &weights = make_ref(vpColVector{}))
66{
67 // Local helpers
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);
72#endif
73
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.};
76
77 int i = 0;
78#ifdef VISP_HAVE_OPENMP
79#pragma omp parallel for num_threads(num_procs) reduction(+ : total_w, cent_x, cent_y, cent_z)
80#endif
81 for (i = 0; i < static_cast<int>(weights.size()); i++) {
82 const auto pt_cloud_start_idx = 3 * i;
83
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];
87
88 total_w += weights[i];
89 }
90
91 return std::make_tuple(vpColVector{cent_x, cent_y, cent_z}, total_w);
92 };
93
94 //
95 auto prev_error = 1e3;
96 auto error = prev_error - 1;
97 const unsigned int nPoints = static_cast<unsigned int>(point_cloud.size() / 3);
98
99 vpColVector residues(nPoints);
100 weights = vpColVector(nPoints, 1.0);
101 vpColVector normal;
102 vpMatrix M(nPoints, 3);
103 vpRobust tukey;
105
106 for (auto iter = 0u; iter < PlaneSvdMaxIter && std::fabs(error - prev_error) > PlaneSvdMaxError; iter++) {
107 if (iter != 0) {
108 tukey.MEstimator(vpRobust::TUKEY, residues, weights);
109 }
110
111 // Compute centroid
112#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_17)
113 auto [centroid, total_w] = compute_centroid(point_cloud, weights);
114#else
115 // C++17 structured binding are not fully supported by clang 13.0 on macOS
116 // See
117 // https://stackoverflow.com/questions/46114214/lambda-implicit-capture-fails-with-variable-declared-from-structured-binding
118 vpColVector centroid;
119 double total_w;
120 std::tie(centroid, total_w) = compute_centroid(point_cloud, weights);
121#endif
122
123 centroid /= total_w;
124
125 // Minimization
126 int i = 0;
127#ifdef VISP_HAVE_OPENMP
128#pragma omp parallel for num_threads(num_procs)
129#endif
130 for (i = 0; i < static_cast<int>(nPoints); i++) {
131 const auto pt_cloud_start_idx = 3 * i;
132
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]);
136 }
137
138 vpColVector W{};
139 vpMatrix V{};
140 auto J = M.t() * M;
141 J.svd(W, V);
142
143 auto smallestSv = W[0];
144 auto indexSmallestSv = 0u;
145 for (auto i = 1u; i < W.size(); i++) {
146 if (W[i] < smallestSv) {
147 smallestSv = W[i];
148 indexSmallestSv = i;
149 }
150 }
151
152 normal = V.getCol(indexSmallestSv);
153
154 // Compute plane equation
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]);
157
158 // Compute error points to estimated plane
159 prev_error = error;
160 error = 0.;
161 const auto smth = std::hypot(A, B, C);
162
163#ifdef VISP_HAVE_OPENMP
164#pragma omp parallel for num_threads(num_procs) reduction(+ : error)
165#endif
166 for (i = 0; i < static_cast<int>(nPoints); i++) {
167 const auto pt_cloud_start_idx = 3 * i;
168
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) /
171 smth;
172
173 error += weights[i] * residues[i];
174 }
175
176 error /= total_w;
177 }
178
179 // Update final weights
180 tukey.MEstimator(vpRobust::TUKEY, residues, weights);
181
182 // Update final centroid
183 auto [centroid, total_w] = compute_centroid(point_cloud, weights);
184 centroid /= total_w;
185
186 // Compute final plane equation
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]);
189
190 // Return final plane equation
191 return {A, B, C, D};
192}
193
194} // namespace
195
207std::optional<vpPlane>
208vpPlaneEstimation::estimatePlane(const vpImage<uint16_t> &I_depth_raw, double depth_scale,
209 const vpCameraParameters &depth_intrinsics, const vpPolygon &roi,
210 const unsigned int avg_nb_of_pts_to_estimate,
211 std::optional<std::reference_wrapper<vpImage<vpRGBa> > > heat_map)
212{
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);
217#endif
218
219 // Local helper: Reduce computation (roi.isInside)
220 // Default: the img is totally included in the ROI
221 std::function<bool(const vpImagePoint &)> isInside = [](const vpImagePoint &) { return true; };
222
223 // If the img is crossed by the ROI, vpPolygon::isInside has to be used
224 {
225 // If at least one ROI corner is inside the img bound
226 const vpRect img_bound{vpImagePoint(0, 0), static_cast<double>(I_depth_raw.getWidth()),
227 static_cast<double>(I_depth_raw.getHeight())};
228 for (const auto &roi_corner : roi.getCorners()) {
229 if (img_bound.isInside(roi_corner)) {
230 isInside = [&roi](const vpImagePoint &ip) { return roi.isInside(ip); };
231 break;
232 }
233 }
234
235 // If at least one img corner is outside the ROI
236 // clang-format off
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() ) )
241 // clang-format on
242 {
243 isInside = [&roi](const vpImagePoint &ip) { return roi.isInside(ip); };
244 }
245 }
246
247 // Limit research area
248 const auto roi_bb = roi.getBoundingBox();
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()));
253
254 // Reduce computation time by using subsample factor
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);
258
259 // Create the point cloud which will be used for plane estimation
260 std::vector<double> pt_cloud{};
261
262#if defined(VISP_HAVE_OPENMP) && !(_WIN32)
263// The following OpenMP 4.0 directive is not supported by Visual C++ compiler that allows only OpenMP 2.0 support
264// https://docs.microsoft.com/en-us/cpp/parallel/openmp/openmp-in-visual-cpp?redirectedfrom=MSDN&view=msvc-170
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)
267#endif
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)) {
272 double x{0.}, y{0.};
273 vpPixelMeterConversion::convertPoint(depth_intrinsics, pixel, x, y);
274 const double Z = I_depth_raw[i][j] * depth_scale;
275
276 pt_cloud.push_back(x * Z);
277 pt_cloud.push_back(y * Z);
278 pt_cloud.push_back(Z);
279 }
280 }
281 }
282
283 if (pt_cloud.size() < MinPointNbToEstimatePlane) {
284 return std::nullopt;
285 }
286
287 // Display heatmap
288 if (heat_map) {
289 vpColVector weights{};
290 const auto plane = estimatePlaneEquationSVD(pt_cloud, weights);
291
292 heat_map->get() = vpImage<vpRGBa>{I_depth_raw.getHeight(), I_depth_raw.getWidth(), vpColor::black};
293
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]};
296
297 vpImagePoint ip{};
298 vpMeterPixelConversion::convertPoint(depth_intrinsics, X / Z, Y / Z, ip);
299
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;
303
304 heat_map->get()[static_cast<int>(ip.get_i())][static_cast<int>(ip.get_j())] = vpColor(r, g, b);
305 }
306 return plane;
307 } else {
308 return estimatePlaneEquationSVD(pt_cloud);
309 }
310}
311
312#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static const vpColor black
Definition vpColor.h:205
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.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static T clamp(const T &v, const T &lower, const T &upper)
Definition vpMath.h:139
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void svd(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition vpMatrix.cpp:461
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.
Definition vpPlane.h:54
Defines a generic 2D polygon.
Definition vpPolygon.h:97
vpRect getBoundingBox() const
Definition vpPolygon.h:171
const std::vector< vpImagePoint > & getCorners() const
Definition vpPolygon.h:147
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Defines a rectangle in the plane.
Definition vpRect.h:76
Contains an M-estimator and various influence function.
Definition vpRobust.h:83
@ TUKEY
Tukey influence function.
Definition vpRobust.h:87
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition vpRobust.h:155