iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ModelPlane.H
Go to the documentation of this file.
1 /*! @file
2  @author Shane Grant
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 #ifdef NRT_HAVE_CLOUD
36 #ifndef INCLUDE_NRT_POINTCLOUD2_SAMPLECONCENSUS_MODELS_MODELPLANE_H_
37 #define INCLUDE_NRT_POINTCLOUD2_SAMPLECONCENSUS_MODELS_MODELPLANE_H_
38 
39 #include <memory>
40 #include <unordered_map>
41 #include <random>
42 
46 
47 namespace nrt
48 {
49  //! A model for performing plane fitting
50  /*! This model estimates a plane for a given set of points, optionally using
51  point normals present as nrt::PointNormal to place additional constraints
52  on the optimization */
53  class ModelPlane : public ModelBase
54  {
55  typedef PointCloud2::Vector4 Vector4;
56  typedef PointCloud2::VectorX VectorX;
57  typedef Eigen::Array<PointCloud2::BaseType, 4, 1> Array4_1;
58 
59  public:
60  //! A parameterized plane
61  typedef Eigen::Hyperplane<PointCloud2::BaseType, 3> Plane;
62 
63  //! A validation function that returns true if the points are not collinear
65 
66  //! Constructor
67  /*! @param source The source cloud
68  @param func The optional evaluation function. Should return true if a plane can be created
69  from the set of points provided.
70  @param inlierThreshold Distance threshold to use for select and countWithinDistance()
71  @param normalWeight The weighting to apply to normals. If non-empty, the input point cloud
72  must have normals in the form of nrt::PointNormal. This will place
73  an additional constraint on the optimization such that the inliers
74  have some maximum angular deviation between the plane's normal
75  and the inlier point normals. In the range [0 1].
76  @param sampleMaxIterations Maximum number of iterations to use during sample selection
77  @param seedRandom If true, the random number generator will be seeded with
78  the current time, otherwise it will always
79  receive the same default seed */
80  ModelPlane( PointCloud2 const source,
81  Optional<ModelBase::SampleValidFunction> func = OptionalEmpty,
82  double inlierThreshold = std::numeric_limits<double>::max(),
83  Optional<double> normalWeight = OptionalEmpty,
84  size_t sampleMaxIterations = 1000,
85  bool seedRandom = false );
86 
87  //! Constructor with subsets
88  /*! @param source The source cloud
89  @param sourceIndices The subset of the input to consider
90  @param func The optional evaluation function. Should return true if a plane can be created
91  from the set of points provided.
92  @param normalWeight The weighting to apply to normals. If non-zero, the input point cloud
93  must have normals in the form of nrt::PointNormal. This will place
94  an additional constraint on the optimization such that the inliers
95  have some maximum angular deviation between the plane's normal
96  and the inlier point normals.
97  @param inlierThreshold Distance threshold to use for select and countWithinDistance()
98  @param sampleMaxIterations Maximum number of iterations to use during sample selection
99  @param seedRandom If true, the random number generator will be seeded with
100  the current time, otherwise it will always
101  receive the same default seed */
102  ModelPlane( PointCloud2 const source, Indices const sourceIndices,
103  Optional<ModelBase::SampleValidFunction> func = OptionalEmpty,
104  double inlierThreshold = -std::numeric_limits<double>::max(),
105  Optional<double> normalWeight = OptionalEmpty,
106  size_t sampleMaxIterations = 1000,
107  bool seedRandom = false );
108 
109  //! Virtual destructor
110  virtual ~ModelPlane();
111 
112  //! Returns the minimum valid sample size for the model
113  virtual size_t getSampleSize() const { return 3; }
114 
115  //! Attempts to form a valid model from the points at the selected indices
116  /*! @param sampleIndices The valid indices (check with isSampleValid() ) to use
117  @return The coefficients for the model */
118  virtual VectorX computeModelCoefficients( Indices const sampleIndices ) const;
119 
120  //! Recomputes the model coefficients using the provided inlier indices
121  /*! This can be called after inliers have been computed to refine a solution
122  @param inlierIndices The inlier indices
123  @param modelCoefficients The coefficients found via computeModelCoefficients()
124  @return The optimized set of model coefficients */
125  virtual VectorX optimizeModelCoefficients( Indices const inlierIndices,
126  VectorX const & modelCoefficients ) const;
127 
128  //! Computes distances from transformed points to their correspondences
129  virtual std::vector<double> getDistancesToModel( VectorX const & modelCoefficients ) const;
130 
131  //! Selects all points from the input data set that are inliers under the model according
132  //! to some threshold
133  virtual Indices selectWithinDistance( VectorX const & modelCoefficients ) const;
134 
135  //! Counts all of the points which respect the model coefficients that are inliers
136  virtual int countWithinDistance( VectorX const & modelCoefficients ) const;
137 
138  protected:
139  //! Check if some set of coefficients is valid to use
140  bool isValid( VectorX const & coefs ) const;
141 
142  private:
145  Optional<double> itsNormalWeight; //!< Weighting given to point normals in optimization
146  };
147 } //namespace nrt
148 
149 #endif // INCLUDE_NRT_POINTCLOUD2_SAMPLECONCENSUS_MODELS_MODELPLANE_H_
150 #endif // NRT_HAVE_CLOUD