Point Cloud Library (PCL)  1.10.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/memory.h>
46 #include <pcl/pcl_macros.h>
47 #include <pcl/PointIndices.h>
48 
49 namespace pcl
50 {
51  /** \brief
52  * Implements the method for extracting features based on moment of inertia. It also
53  * calculates AABB, OBB and eccentricity of the projected cloud.
54  */
55  template <typename PointT>
57  {
58  public:
59 
66 
69 
70  public:
71 
72  /** \brief Provide a pointer to the input dataset
73  * \param[in] cloud the const boost shared pointer to a PointCloud message
74  */
75  void
76  setInputCloud (const PointCloudConstPtr& cloud) override;
77 
78  /** \brief Provide a pointer to the vector of indices that represents the input data.
79  * \param[in] indices a pointer to the vector of indices that represents the input data.
80  */
81  void
82  setIndices (const IndicesPtr& indices) override;
83 
84  /** \brief Provide a pointer to the vector of indices that represents the input data.
85  * \param[in] indices a pointer to the vector of indices that represents the input data.
86  */
87  void
88  setIndices (const IndicesConstPtr& indices) override;
89 
90  /** \brief Provide a pointer to the vector of indices that represents the input data.
91  * \param[in] indices a pointer to the vector of indices that represents the input data.
92  */
93  void
94  setIndices (const PointIndicesConstPtr& indices) override;
95 
96  /** \brief Set the indices for the points laying within an interest region of
97  * the point cloud.
98  * \note you shouldn't call this method on unorganized point clouds!
99  * \param[in] row_start the offset on rows
100  * \param[in] col_start the offset on columns
101  * \param[in] nb_rows the number of rows to be considered row_start included
102  * \param[in] nb_cols the number of columns to be considered col_start included
103  */
104  void
105  setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols) override;
106 
107  /** \brief Constructor that sets default values for member variables. */
109 
110  /** \brief Virtual destructor which frees the memory. */
111 
113 
114  /** \brief This method allows to set the angle step. It is used for the rotation
115  * of the axis which is used for moment of inertia/eccentricity calculation.
116  * \param[in] step angle step
117  */
118  void
119  setAngleStep (const float step);
120 
121  /** \brief Returns the angle step. */
122  float
123  getAngleStep () const;
124 
125  /** \brief This method allows to set the normalize_ flag. If set to false, then
126  * point_mass_ will be used to scale the moment of inertia values. Otherwise,
127  * point_mass_ will be set to 1 / number_of_points. Default value is true.
128  * \param[in] need_to_normalize desired value
129  */
130  void
131  setNormalizePointMassFlag (bool need_to_normalize);
132 
133  /** \brief Returns the normalize_ flag. */
134  bool
135  getNormalizePointMassFlag () const;
136 
137  /** \brief This method allows to set point mass that will be used for
138  * moment of inertia calculation. It is needed to scale moment of inertia values.
139  * default value is 0.0001.
140  * \param[in] point_mass point mass
141  */
142  void
143  setPointMass (const float point_mass);
144 
145  /** \brief Returns the mass of point. */
146  float
147  getPointMass () const;
148 
149  /** \brief This method launches the computation of all features. After execution
150  * it sets is_valid_ flag to true and each feature can be accessed with the
151  * corresponding get method.
152  */
153  void
154  compute ();
155 
156  /** \brief This method gives access to the computed axis aligned bounding box. It returns true
157  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
158  * \param[out] min_point min point of the AABB
159  * \param[out] max_point max point of the AABB
160  */
161  bool
162  getAABB (PointT& min_point, PointT& max_point) const;
163 
164  /** \brief This method gives access to the computed oriented bounding box. It returns true
165  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
166  * Note that in order to get the OBB, each vertex of the given AABB (specified with min_point and max_point)
167  * must be rotated with the given rotational matrix (rotation transform) and then positioned.
168  * Also pay attention to the fact that this is not the minimal possible bounding box. This is the bounding box
169  * which is oriented in accordance with the eigen vectors.
170  * \param[out] min_point min point of the OBB
171  * \param[out] max_point max point of the OBB
172  * \param[out] position position of the OBB
173  * \param[out] rotational_matrix this matrix represents the rotation transform
174  */
175  bool
176  getOBB (PointT& min_point, PointT& max_point, PointT& position, Eigen::Matrix3f& rotational_matrix) const;
177 
178  /** \brief This method gives access to the computed eigen values. It returns true
179  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
180  * \param[out] major major eigen value
181  * \param[out] middle middle eigen value
182  * \param[out] minor minor eigen value
183  */
184  bool
185  getEigenValues (float& major, float& middle, float& minor) const;
186 
187  /** \brief This method gives access to the computed eigen vectors. It returns true
188  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
189  * \param[out] major axis which corresponds to the eigen vector with the major eigen value
190  * \param[out] middle axis which corresponds to the eigen vector with the middle eigen value
191  * \param[out] minor axis which corresponds to the eigen vector with the minor eigen value
192  */
193  bool
194  getEigenVectors (Eigen::Vector3f& major, Eigen::Vector3f& middle, Eigen::Vector3f& minor) const;
195 
196  /** \brief This method gives access to the computed moments of inertia. It returns true
197  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
198  * \param[out] moment_of_inertia computed moments of inertia
199  */
200  bool
201  getMomentOfInertia (std::vector <float>& moment_of_inertia) const;
202 
203  /** \brief This method gives access to the computed ecentricities. It returns true
204  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
205  * \param[out] eccentricity computed eccentricities
206  */
207  bool
208  getEccentricity (std::vector <float>& eccentricity) const;
209 
210  /** \brief This method gives access to the computed mass center. It returns true
211  * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise.
212  * Note that when mass center of a cloud is computed, mass point is always considered equal 1.
213  * \param[out] mass_center computed mass center
214  */
215  bool
216  getMassCenter (Eigen::Vector3f& mass_center) const;
217 
218  private:
219 
220  /** \brief This method rotates the given vector around the given axis.
221  * \param[in] vector vector that must be rotated
222  * \param[in] axis axis around which vector must be rotated
223  * \param[in] angle angle in degrees
224  * \param[out] rotated_vector resultant vector
225  */
226  void
227  rotateVector (const Eigen::Vector3f& vector, const Eigen::Vector3f& axis, const float angle, Eigen::Vector3f& rotated_vector) const;
228 
229  /** \brief This method computes center of mass and axis aligned bounding box. */
230  void
231  computeMeanValue ();
232 
233  /** \brief This method computes the oriented bounding box. */
234  void
235  computeOBB ();
236 
237  /** \brief This method computes the covariance matrix for the input_ cloud.
238  * \param[out] covariance_matrix stores the computed covariance matrix
239  */
240  void
241  computeCovarianceMatrix (Eigen::Matrix <float, 3, 3>& covariance_matrix) const;
242 
243  /** \brief This method computes the covariance matrix for the given cloud.
244  * It uses all points in the cloud, unlike the previous method that uses indices.
245  * \param[in] cloud cloud for which covariance matrix will be computed
246  * \param[out] covariance_matrix stores the computed covariance matrix
247  */
248  void
249  computeCovarianceMatrix (PointCloudConstPtr cloud, Eigen::Matrix <float, 3, 3>& covariance_matrix) const;
250 
251  /** \brief This method calculates the eigen values and eigen vectors
252  * for the given covariance matrix. Note that it returns normalized eigen
253  * vectors that always form the right-handed coordinate system.
254  * \param[in] covariance_matrix covariance matrix
255  * \param[out] major_axis eigen vector which corresponds to a major eigen value
256  * \param[out] middle_axis eigen vector which corresponds to a middle eigen value
257  * \param[out] minor_axis eigen vector which corresponds to a minor eigen value
258  * \param[out] major_value major eigen value
259  * \param[out] middle_value middle eigen value
260  * \param[out] minor_value minor eigen value
261  */
262  void
263  computeEigenVectors (const Eigen::Matrix <float, 3, 3>& covariance_matrix, Eigen::Vector3f& major_axis,
264  Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis, float& major_value, float& middle_value,
265  float& minor_value);
266 
267  /** \brief This method returns the moment of inertia of a given input_ cloud.
268  * Note that when moment of inertia is computed it is multiplied by the point mass.
269  * Point mass can be accessed with the corresponding get/set methods.
270  * \param[in] current_axis axis that will be used in moment of inertia computation
271  * \param[in] mean_value mean value(center of mass) of the cloud
272  */
273  float
274  calculateMomentOfInertia (const Eigen::Vector3f& current_axis, const Eigen::Vector3f& mean_value) const;
275 
276  /** \brief This method simply projects the given input_ cloud on the plane specified with
277  * the normal vector.
278  * \param[in] normal_vector nrmal vector of the plane
279  * \param[in] point point belonging to the plane
280  * \param[out] projected_cloud projected cloud
281  */
282  void
283  getProjectedCloud (const Eigen::Vector3f& normal_vector, const Eigen::Vector3f& point, typename pcl::PointCloud <PointT>::Ptr projected_cloud) const;
284 
285  /** \brief This method returns the eccentricity of the projected cloud.
286  * \param[in] covariance_matrix covariance matrix of the projected cloud
287  * \param[in] normal_vector normal vector of the plane, it is used to discard the
288  * third eigen vector and eigen value*/
289  float
290  computeEccentricity (const Eigen::Matrix <float, 3, 3>& covariance_matrix, const Eigen::Vector3f& normal_vector);
291 
292  private:
293 
294  /** \brief Indicates if the stored values (eccentricity, moment of inertia, AABB etc.)
295  * are valid when accessed with the get methods. */
296  bool is_valid_;
297 
298  /** \brief Stores the angle step */
299  float step_;
300 
301  /** \brief Stores the mass of point in the cloud */
302  float point_mass_;
303 
304  /** \brief Stores the flag for mass normalization */
305  bool normalize_;
306 
307  /** \brief Stores the mean value (center of mass) of the cloud */
308  Eigen::Vector3f mean_value_;
309 
310  /** \brief Major eigen vector */
311  Eigen::Vector3f major_axis_;
312 
313  /** \brief Middle eigen vector */
314  Eigen::Vector3f middle_axis_;
315 
316  /** \brief Minor eigen vector */
317  Eigen::Vector3f minor_axis_;
318 
319  /** \brief Major eigen value */
320  float major_value_;
321 
322  /** \brief Middle eigen value */
323  float middle_value_;
324 
325  /** \brief Minor eigen value */
326  float minor_value_;
327 
328  /** \brief Stores calculated moments of inertia */
329  std::vector <float> moment_of_inertia_;
330 
331  /** \brief Stores calculated eccentricities */
332  std::vector <float> eccentricity_;
333 
334  /** \brief Min point of the axis aligned bounding box */
335  PointT aabb_min_point_;
336 
337  /** \brief Max point of the axis aligned bounding box */
338  PointT aabb_max_point_;
339 
340  /** \brief Min point of the oriented bounding box */
341  PointT obb_min_point_;
342 
343  /** \brief Max point of the oriented bounding box */
344  PointT obb_max_point_;
345 
346  /** \brief Stores position of the oriented bounding box */
347  Eigen::Vector3f obb_position_;
348 
349  /** \brief Stores the rotational matrix of the oriented bounding box */
350  Eigen::Matrix3f obb_rotational_matrix_;
351 
352  public:
354  };
355 }
356 
357 #define PCL_INSTANTIATE_MomentOfInertiaEstimation(T) template class pcl::MomentOfInertiaEstimation<T>;
358 
359 #ifdef PCL_NO_PRECOMPILE
360 #include <pcl/features/impl/moment_of_inertia_estimation.hpp>
361 #endif
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:414
Defines functions, macros and traits for allocating and using memory.
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:62
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:65
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:63
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:70
A point structure representing Euclidean xyz coordinates, and the RGB color.
#define PCL_EXPORTS
Definition: pcl_macros.h:276
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_base.h:78
Defines all the PCL and non-PCL macros used.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:75