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