Point Cloud Library (PCL)  1.9.1-dev
moment_of_inertia_estimation.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  *
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author : Sergey Ushakov
36  * Email : sergey.s.ushakov@mail.ru
37  *
38  */
39 
40 #pragma once
41 
42 #include <vector>
43 #include <cmath>
44 #include <pcl/features/feature.h>
45 #include <pcl/pcl_macros.h>
46 #include <pcl/PointIndices.h>
47 
48 namespace pcl
49 {
50  /** \brief
51  * Implements the method for extracting features based on moment of inertia. It also
52  * calculates AABB, OBB and eccentricity of the projected cloud.
53  */
54  template <typename PointT>
56  {
57  public:
58 
65 
68 
69  public:
70 
71  /** \brief Provide a pointer to the input dataset
72  * \param[in] cloud the const boost shared pointer to a PointCloud message
73  */
74  void
75  setInputCloud (const PointCloudConstPtr& cloud) override;
76 
77  /** \brief Provide a pointer to the vector of indices that represents the input data.
78  * \param[in] indices a pointer to the vector of indices that represents the input data.
79  */
80  void
81  setIndices (const IndicesPtr& indices) override;
82 
83  /** \brief Provide a pointer to the vector of indices that represents the input data.
84  * \param[in] indices a pointer to the vector of indices that represents the input data.
85  */
86  void
87  setIndices (const IndicesConstPtr& indices) override;
88 
89  /** \brief Provide a pointer to the vector of indices that represents the input data.
90  * \param[in] indices a pointer to the vector of indices that represents the input data.
91  */
92  void
93  setIndices (const PointIndicesConstPtr& indices) override;
94 
95  /** \brief Set the indices for the points laying within an interest region of
96  * the point cloud.
97  * \note you shouldn't call this method on unorganized point clouds!
98  * \param[in] row_start the offset on rows
99  * \param[in] col_start the offset on columns
100  * \param[in] nb_rows the number of rows to be considered row_start included
101  * \param[in] nb_cols the number of columns to be considered col_start included
102  */
103  void
104  setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols) override;
105 
106  /** \brief Constructor that sets default values for member variables. */
108 
109  /** \brief Virtual destructor which frees the memory. */
110 
112 
113  /** \brief This method allows to set the angle step. It is used for the rotation
114  * of the axis which is used for moment of inertia/eccentricity calculation.
115  * \param[in] step angle step
116  */
117  void
118  setAngleStep (const float step);
119 
120  /** \brief Returns the angle step. */
121  float
122  getAngleStep () const;
123 
124  /** \brief This method allows to set the normalize_ flag. If set to false, then
125  * point_mass_ will be used to scale the moment of inertia values. Otherwise,
126  * point_mass_ will be set to 1 / number_of_points. Default value is true.
127  * \param[in] need_to_normalize desired value
128  */
129  void
130  setNormalizePointMassFlag (bool need_to_normalize);
131 
132  /** \brief Returns the normalize_ flag. */
133  bool
134  getNormalizePointMassFlag () const;
135 
136  /** \brief This method allows to set point mass that will be used for
137  * moment of inertia calculation. It is needed to scale moment of inertia values.
138  * default value is 0.0001.
139  * \param[in] point_mass point mass
140  */
141  void
142  setPointMass (const float point_mass);
143 
144  /** \brief Returns the mass of point. */
145  float
146  getPointMass () const;
147 
148  /** \brief This method launches the computation of all features. After execution
149  * it sets is_valid_ flag to true and each feature can be accessed with the
150  * corresponding get method.
151  */
152  void
153  compute ();
154 
155  /** \brief This method gives access to the computed axis aligned bounding box. It returns true
156  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
157  * \param[out] min_point min point of the AABB
158  * \param[out] max_point max point of the AABB
159  */
160  bool
161  getAABB (PointT& min_point, PointT& max_point) const;
162 
163  /** \brief This method gives access to the computed oriented bounding box. It returns true
164  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
165  * Note that in order to get the OBB, each vertex of the given AABB (specified with min_point and max_point)
166  * must be rotated with the given rotational matrix (rotation transform) and then positioned.
167  * Also pay attention to the fact that this is not the minimal possible bounding box. This is the bounding box
168  * which is oriented in accordance with the eigen vectors.
169  * \param[out] min_point min point of the OBB
170  * \param[out] max_point max point of the OBB
171  * \param[out] position position of the OBB
172  * \param[out] rotational_matrix this matrix represents the rotation transform
173  */
174  bool
175  getOBB (PointT& min_point, PointT& max_point, PointT& position, Eigen::Matrix3f& rotational_matrix) const;
176 
177  /** \brief This method gives access to the computed eigen values. It returns true
178  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
179  * \param[out] major major eigen value
180  * \param[out] middle middle eigen value
181  * \param[out] minor minor eigen value
182  */
183  bool
184  getEigenValues (float& major, float& middle, float& minor) const;
185 
186  /** \brief This method gives access to the computed eigen vectors. It returns true
187  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
188  * \param[out] major axis which corresponds to the eigen vector with the major eigen value
189  * \param[out] middle axis which corresponds to the eigen vector with the middle eigen value
190  * \param[out] minor axis which corresponds to the eigen vector with the minor eigen value
191  */
192  bool
193  getEigenVectors (Eigen::Vector3f& major, Eigen::Vector3f& middle, Eigen::Vector3f& minor) const;
194 
195  /** \brief This method gives access to the computed moments of inertia. It returns true
196  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
197  * \param[out] moment_of_inertia computed moments of inertia
198  */
199  bool
200  getMomentOfInertia (std::vector <float>& moment_of_inertia) const;
201 
202  /** \brief This method gives access to the computed ecentricities. It returns true
203  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
204  * \param[out] eccentricity computed eccentricities
205  */
206  bool
207  getEccentricity (std::vector <float>& eccentricity) const;
208 
209  /** \brief This method gives access to the computed mass center. It returns true
210  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
211  * Note that when mass center of a cloud is computed, mass point is always considered equal 1.
212  * \param[out] mass_center computed mass center
213  */
214  bool
215  getMassCenter (Eigen::Vector3f& mass_center) const;
216 
217  private:
218 
219  /** \brief This method rotates the given vector around the given axis.
220  * \param[in] vector vector that must be rotated
221  * \param[in] axis axis around which vector must be rotated
222  * \param[in] angle angle in degrees
223  * \param[out] rotated_vector resultant vector
224  */
225  void
226  rotateVector (const Eigen::Vector3f& vector, const Eigen::Vector3f& axis, const float angle, Eigen::Vector3f& rotated_vector) const;
227 
228  /** \brief This method computes center of mass and axis aligned bounding box. */
229  void
230  computeMeanValue ();
231 
232  /** \brief This method computes the oriented bounding box. */
233  void
234  computeOBB ();
235 
236  /** \brief This method computes the covariance matrix for the input_ cloud.
237  * \param[out] covariance_matrix stores the computed covariance matrix
238  */
239  void
240  computeCovarianceMatrix (Eigen::Matrix <float, 3, 3>& covariance_matrix) const;
241 
242  /** \brief This method computes the covariance matrix for the given cloud.
243  * It uses all points in the cloud, unlike the previous method that uses indices.
244  * \param[in] cloud cloud for which covariance matrix will be computed
245  * \param[out] covariance_matrix stores the computed covariance matrix
246  */
247  void
248  computeCovarianceMatrix (PointCloudConstPtr cloud, Eigen::Matrix <float, 3, 3>& covariance_matrix) const;
249 
250  /** \brief This method calculates the eigen values and eigen vectors
251  * for the given covariance matrix. Note that it returns normalized eigen
252  * vectors that always form the right-handed coordinate system.
253  * \param[in] covariance_matrix covariance matrix
254  * \param[out] major_axis eigen vector which corresponds to a major eigen value
255  * \param[out] middle_axis eigen vector which corresponds to a middle eigen value
256  * \param[out] minor_axis eigen vector which corresponds to a minor eigen value
257  * \param[out] major_value major eigen value
258  * \param[out] middle_value middle eigen value
259  * \param[out] minor_value minor eigen value
260  */
261  void
262  computeEigenVectors (const Eigen::Matrix <float, 3, 3>& covariance_matrix, Eigen::Vector3f& major_axis,
263  Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis, float& major_value, float& middle_value,
264  float& minor_value);
265 
266  /** \brief This method returns the moment of inertia of a given input_ cloud.
267  * Note that when moment of inertia is computed it is multiplied by the point mass.
268  * Point mass can be accessed with the corresponding get/set methods.
269  * \param[in] current_axis axis that will be used in moment of inertia computation
270  * \param[in] mean_value mean value(center of mass) of the cloud
271  */
272  float
273  calculateMomentOfInertia (const Eigen::Vector3f& current_axis, const Eigen::Vector3f& mean_value) const;
274 
275  /** \brief This method simply projects the given input_ cloud on the plane specified with
276  * the normal vector.
277  * \param[in] normal_vector nrmal vector of the plane
278  * \param[in] point point belonging to the plane
279  * \param[out] projected_cloud projected cloud
280  */
281  void
282  getProjectedCloud (const Eigen::Vector3f& normal_vector, const Eigen::Vector3f& point, typename pcl::PointCloud <PointT>::Ptr projected_cloud) const;
283 
284  /** \brief This method returns the eccentricity of the projected cloud.
285  * \param[in] covariance_matrix covariance matrix of the projected cloud
286  * \param[in] normal_vector normal vector of the plane, it is used to discard the
287  * third eigen vector and eigen value*/
288  float
289  computeEccentricity (const Eigen::Matrix <float, 3, 3>& covariance_matrix, const Eigen::Vector3f& normal_vector);
290 
291  private:
292 
293  /** \brief Indicates if the stored values (eccentricity, moment of inertia, AABB etc.)
294  * are valid when accessed with the get methods. */
295  bool is_valid_;
296 
297  /** \brief Stores the angle step */
298  float step_;
299 
300  /** \brief Stores the mass of point in the cloud */
301  float point_mass_;
302 
303  /** \brief Stores the flag for mass normalization */
304  bool normalize_;
305 
306  /** \brief Stores the mean value (center of mass) of the cloud */
307  Eigen::Vector3f mean_value_;
308 
309  /** \brief Major eigen vector */
310  Eigen::Vector3f major_axis_;
311 
312  /** \brief Middle eigen vector */
313  Eigen::Vector3f middle_axis_;
314 
315  /** \brief Minor eigen vector */
316  Eigen::Vector3f minor_axis_;
317 
318  /** \brief Major eigen value */
319  float major_value_;
320 
321  /** \brief Middle eigen value */
322  float middle_value_;
323 
324  /** \brief Minor eigen value */
325  float minor_value_;
326 
327  /** \brief Stores calculated moments of inertia */
328  std::vector <float> moment_of_inertia_;
329 
330  /** \brief Stores calculated eccentricities */
331  std::vector <float> eccentricity_;
332 
333  /** \brief Min point of the axis aligned bounding box */
334  PointT aabb_min_point_;
335 
336  /** \brief Max point of the axis aligned bounding box */
337  PointT aabb_max_point_;
338 
339  /** \brief Min point of the oriented bounding box */
340  PointT obb_min_point_;
341 
342  /** \brief Max point of the oriented bounding box */
343  PointT obb_max_point_;
344 
345  /** \brief Stores position of the oriented bounding box */
346  Eigen::Vector3f obb_position_;
347 
348  /** \brief Stores the rotational matrix of the oriented bounding box */
349  Eigen::Matrix3f obb_rotational_matrix_;
350 
351  public:
353  };
354 }
355 
356 #define PCL_INSTANTIATE_MomentOfInertiaEstimation(T) template class pcl::MomentOfInertiaEstimation<T>;
357 
358 #ifdef PCL_NO_PRECOMPILE
359 #include <pcl/features/impl/moment_of_inertia_estimation.hpp>
360 #endif
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:359
boost::shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61
Implements the method for extracting features based on moment of inertia.
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
PCL base class.
Definition: pcl_base.h:69
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:412
boost::shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:62
A point structure representing Euclidean xyz coordinates, and the RGB color.
#define PCL_EXPORTS
Definition: pcl_macros.h:241
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_base.h:77
Defines all the PCL and non-PCL macros used.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74