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