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