Visual Servoing Platform  version 3.6.1 under development (2024-11-07)
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 // Check if std:c++17 or higher
37 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
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 BEGIN_VISP_NAMESPACE
50 
51 #ifndef DOXYGEN_SHOULD_SKIP_THIS
52 // Local helpers
53 namespace
54 {
55 
56 constexpr auto PlaneSvdMaxError { 1e-4 };
57 constexpr auto PlaneSvdMaxIter { 10 };
58 
59 template <class T> T &make_ref(T &&x) { return x; }
60 
68 vpPlane estimatePlaneEquationSVD(const std::vector<double> &point_cloud, vpColVector &weights = make_ref(vpColVector {}))
69 {
70  // Local helpers
71 #ifdef VISP_HAVE_OPENMP
72  auto num_procs = omp_get_num_procs();
73  num_procs = num_procs > 2 ? num_procs - 2 : num_procs;
74  omp_set_num_threads(num_procs);
75 #endif
76 
77  auto compute_centroid = [=](const std::vector<double> &point_cloud, const vpColVector &weights) {
78  double cent_x { 0. }, cent_y { 0. }, cent_z { 0. }, total_w { 0. };
79 
80  int i = 0;
81 #ifdef VISP_HAVE_OPENMP
82 #pragma omp parallel for num_threads(num_procs) reduction(+ : total_w, cent_x, cent_y, cent_z)
83 #endif
84  for (i = 0; i < static_cast<int>(weights.size()); i++) {
85  const auto pt_cloud_start_idx = 3 * i;
86 
87  cent_x += weights[i] * point_cloud[pt_cloud_start_idx + 0];
88  cent_y += weights[i] * point_cloud[pt_cloud_start_idx + 1];
89  cent_z += weights[i] * point_cloud[pt_cloud_start_idx + 2];
90 
91  total_w += weights[i];
92  }
93 
94  return std::make_tuple(vpColVector { cent_x, cent_y, cent_z }, total_w);
95  };
96 
97  //
98  auto prev_error = 1e3;
99  auto error = prev_error - 1;
100  const unsigned int nPoints = static_cast<unsigned int>(point_cloud.size() / 3);
101 
102  vpColVector residues(nPoints);
103  weights = vpColVector(nPoints, 1.0);
104  vpColVector normal;
105  vpMatrix M(nPoints, 3);
106  vpRobust tukey;
107  tukey.setMinMedianAbsoluteDeviation(1e-4);
108 
109  for (auto iter = 0u; iter < PlaneSvdMaxIter && std::fabs(error - prev_error) > PlaneSvdMaxError; iter++) {
110  if (iter != 0) {
111  tukey.MEstimator(vpRobust::TUKEY, residues, weights);
112  }
113 
114  // Compute centroid
115 #if ((__cplusplus > 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG > 201703L)))
116  auto [centroid, total_w] = compute_centroid(point_cloud, weights);
117 #else
118  // C++17 structured binding are not fully supported by clang 13.0 on macOS
119  // See
120  // https://stackoverflow.com/questions/46114214/lambda-implicit-capture-fails-with-variable-declared-from-structured-binding
121  vpColVector centroid;
122  double total_w;
123  std::tie(centroid, total_w) = compute_centroid(point_cloud, weights);
124 #endif
125 
126  centroid /= total_w;
127 
128  // Minimization
129  int i = 0;
130 #ifdef VISP_HAVE_OPENMP
131 #pragma omp parallel for num_threads(num_procs)
132 #endif
133  for (i = 0; i < static_cast<int>(nPoints); i++) {
134  const auto pt_cloud_start_idx = 3 * i;
135 
136  M[i][0] = weights[i] * (point_cloud[pt_cloud_start_idx + 0] - centroid[0]);
137  M[i][1] = weights[i] * (point_cloud[pt_cloud_start_idx + 1] - centroid[1]);
138  M[i][2] = weights[i] * (point_cloud[pt_cloud_start_idx + 2] - centroid[2]);
139  }
140 
141  vpColVector W {};
142  vpMatrix V {};
143  auto J = M.t() * M;
144  J.svd(W, V);
145 
146  auto smallestSv = W[0];
147  auto indexSmallestSv = 0u;
148  for (auto i = 1u; i < W.size(); i++) {
149  if (W[i] < smallestSv) {
150  smallestSv = W[i];
151  indexSmallestSv = i;
152  }
153  }
154 
155  normal = V.getCol(indexSmallestSv);
156 
157  // Compute plane equation
158  const auto A = normal[0], B = normal[1], C = normal[2];
159  const auto D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
160 
161  // Compute error points to estimated plane
162  prev_error = error;
163  error = 0.;
164  const auto smth = std::hypot(A, B, C);
165 
166 #ifdef VISP_HAVE_OPENMP
167 #pragma omp parallel for num_threads(num_procs) reduction(+ : error)
168 #endif
169  for (i = 0; i < static_cast<int>(nPoints); i++) {
170  const auto pt_cloud_start_idx = 3 * i;
171 
172  residues[i] = std::fabs(A * point_cloud[pt_cloud_start_idx + 0] + B * point_cloud[pt_cloud_start_idx + 1] +
173  C * point_cloud[pt_cloud_start_idx + 2] + D) /
174  smth;
175 
176  error += weights[i] * residues[i];
177  }
178 
179  error /= total_w;
180  }
181 
182  // Update final weights
183  tukey.MEstimator(vpRobust::TUKEY, residues, weights);
184 
185  // Update final centroid
186  auto [centroid, total_w] = compute_centroid(point_cloud, weights);
187  centroid /= total_w;
188 
189  // Compute final plane equation
190  const auto A = normal[0], B = normal[1], C = normal[2];
191  const auto D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
192 
193  // Return final plane equation
194  return { A, B, C, D };
195 }
196 
197 } // namespace
198 
199 #endif // DOXYGEN_SHOULD_SKIP_THIS
200 std::optional<vpPlane>
201 vpPlaneEstimation::estimatePlane(const vpImage<uint16_t> &I_depth_raw, double depth_scale,
202  const vpCameraParameters &depth_intrinsics, const vpPolygon &roi,
203  const unsigned int avg_nb_of_pts_to_estimate,
204  std::optional<std::reference_wrapper<vpImage<vpRGBa> > > heat_map)
205 {
206 #ifdef VISP_HAVE_OPENMP
207  auto num_procs = omp_get_num_procs();
208  num_procs = num_procs > 2 ? num_procs - 2 : num_procs;
209  omp_set_num_threads(num_procs);
210 #endif
211 
212  // Local helper: Reduce computation (roi.isInside)
213  // Default: the img is totally included in the ROI
214  std::function<bool(const vpImagePoint &)> isInside = [](const vpImagePoint &) { return true; };
215 
216  // If the img is crossed by the ROI, vpPolygon::isInside has to be used
217  {
218  // If at least one ROI corner is inside the img bound
219  const vpRect img_bound { vpImagePoint(0, 0), static_cast<double>(I_depth_raw.getWidth()),
220  static_cast<double>(I_depth_raw.getHeight()) };
221  for (const auto &roi_corner : roi.getCorners()) {
222  if (img_bound.isInside(roi_corner)) {
223  isInside = [&roi](const vpImagePoint &ip) { return roi.isInside(ip); };
224  break;
225  }
226  }
227 
228  // If at least one img corner is outside the ROI
229  // clang-format off
230  if (!roi.isInside(img_bound.getTopLeft()) ||
231  !roi.isInside(img_bound.getTopRight()) ||
232  !roi.isInside(img_bound.getBottomLeft()) ||
233  !roi.isInside(img_bound.getBottomRight()))
234  // clang-format on
235  {
236  isInside = [&roi](const vpImagePoint &ip) { return roi.isInside(ip); };
237  }
238  }
239 
240  // Limit research area
241  const auto roi_bb = roi.getBoundingBox();
242  const int roi_top = static_cast<int>(std::max<double>(0., roi_bb.getTop()));
243  const int roi_bottom = static_cast<int>(std::min<double>(static_cast<double>(I_depth_raw.getHeight()), roi_bb.getBottom()));
244  const int roi_left = static_cast<int>(std::max<double>(0., roi_bb.getLeft()));
245  const int roi_right = static_cast<int>(std::min<double>(static_cast<double>(I_depth_raw.getWidth()), roi_bb.getRight()));
246 
247  // Reduce computation time by using subsample factor
248  unsigned int subsample_factor =
249  static_cast<int>(sqrt(((roi_right - roi_left) * (roi_bottom - roi_top)) / avg_nb_of_pts_to_estimate));
250  subsample_factor = vpMath::clamp(subsample_factor, 1u, MaxSubSampFactorToEstimatePlane);
251 
252  // Create the point cloud which will be used for plane estimation
253  std::vector<double> pt_cloud {};
254 
255 #if defined(VISP_HAVE_OPENMP) && !(_WIN32)
256 // The following OpenMP 4.0 directive is not supported by Visual C++ compiler that allows only OpenMP 2.0 support
257 // https://docs.microsoft.com/en-us/cpp/parallel/openmp/openmp-in-visual-cpp?redirectedfrom=MSDN&view=msvc-170
258 #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 ) ) ))
259 #pragma omp parallel for num_threads(num_procs) collapse(2) reduction(merge : pt_cloud)
260 #endif
261  for (int i = roi_top; i < roi_bottom; i = i + subsample_factor) {
262  for (int j = roi_left; j < roi_right; j = j + subsample_factor) {
263  const auto pixel = vpImagePoint { static_cast<double>(i), static_cast<double>(j) };
264  if (I_depth_raw[i][j] != 0 && isInside(pixel)) {
265  double x { 0. }, y { 0. };
266  vpPixelMeterConversion::convertPoint(depth_intrinsics, pixel, x, y);
267  const double Z = I_depth_raw[i][j] * depth_scale;
268 
269  pt_cloud.push_back(x * Z);
270  pt_cloud.push_back(y * Z);
271  pt_cloud.push_back(Z);
272  }
273  }
274  }
275 
276  if (pt_cloud.size() < MinPointNbToEstimatePlane) {
277  return std::nullopt;
278  }
279 
280  // Display heatmap
281  if (heat_map) {
282  vpColVector weights {};
283  const auto plane = estimatePlaneEquationSVD(pt_cloud, weights);
284 
285  heat_map->get() = vpImage<vpRGBa> { I_depth_raw.getHeight(), I_depth_raw.getWidth(), vpColor::black };
286 
287  for (auto i = 0u; i < weights.size(); i++) {
288  const auto X { pt_cloud[3 * i + 0] }, Y { pt_cloud[3 * i + 1] }, Z { pt_cloud[3 * i + 2] };
289 
290  vpImagePoint ip {};
291  vpMeterPixelConversion::convertPoint(depth_intrinsics, X / Z, Y / Z, ip);
292 
293  const int b = static_cast<int>(std::max<double>(0., 255 * (1 - 2 * weights[i])));
294  const int r = static_cast<int>(std::max<double>(0., 255 * (2 * weights[i] - 1)));
295  const int g = 255 - b - r;
296 
297  heat_map->get()[static_cast<int>(ip.get_i())][static_cast<int>(ip.get_j())] = vpColor(r, g, b);
298  }
299  return plane;
300  }
301  else {
302  return estimatePlaneEquationSVD(pt_cloud);
303  }
304 }
305 END_VISP_NAMESPACE
306 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static const vpColor black
Definition: vpColor.h:211
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
Definition of the vpImage class member functions.
Definition: vpImage.h:131
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static T clamp(const T &v, const T &lower, const T &upper)
Definition: vpMath.h:218
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
void svd(vpColVector &w, vpMatrix &V)
vpMatrix t() const
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.
Definition: vpPlane.h:57
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
const std::vector< vpImagePoint > & getCorners() const
Definition: vpPolygon.h:140
vpRect getBoundingBox() const
Definition: vpPolygon.h:164
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:400
Defines a rectangle in the plane.
Definition: vpRect.h:79
Contains an M-estimator and various influence function.
Definition: vpRobust.h:84
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:89
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:130
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:136