iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ModelRegistration.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_MODELREGISTRATION_H_
37 #define INCLUDE_NRT_POINTCLOUD2_SAMPLECONCENSUS_MODELS_MODELREGISTRATION_H_
38 
39 #include <memory>
40 #include <unordered_map>
41 #include <random>
42 
45 
46 namespace nrt
47 {
48  //! A model for performing point cloud registration
49  /*! This model estimates a rigid transform between two point clouds using
50  a minimum number of points */
51  class ModelRegistration : public ModelBase, public TargetCloud
52  {
53  typedef PointCloud2::VectorX VectorX;
54 
55  public:
56  //! A validation function that returns true if all points chosen are
57  //! sufficiently far away from each other (see computeSampleDistanceThreshold())
59 
60  //! Constructor
61  /*! @param source The source cloud
62  @param target The target cloud
63  @param func The evaluation optional function. Should return true if all points
64  passed in allow for a valid registration.
65  @param inlierThreshold Distance threshold to use for select and countWithinDistance()
66  @param sampleMaxIterations Maximum number of iterations to use during sample selection
67  @param seedRandom If true, the random number generator will be seeded with
68  the current time, otherwise it will always
69  receive the same default seed */
70  ModelRegistration( PointCloud2 const source, PointCloud2 const target,
71  Optional<ModelBase::SampleValidFunction> func = OptionalEmpty,
72  double inlierThreshold = std::numeric_limits<double>::max(),
73  size_t sampleMaxIterations = 1000,
74  bool seedRandom = false );
75 
76  //! Constructor with subsets
77  /*! @param source The source cloud
78  @param sourceIndices Subset of the source to use
79  @param target The target cloud
80  @param targetIndices Subset of the target to use
81  @param func The evaluation optional function. Should return true if all points
82  passed in allow for a valid registration.
83  @param inlierThreshold Distance threshold to use for select and countWithinDistance()
84  @param sampleMaxIterations Maximum number of iterations to use during sample selection
85  @param seedRandom If true, the random number generator will be seeded with
86  the current time, otherwise it will always
87  receive the same default seed */
88  ModelRegistration( PointCloud2 const source, Indices const sourceIndices,
89  PointCloud2 const target, Indices const targetIndices,
90  Optional<ModelBase::SampleValidFunction> func = OptionalEmpty,
91  double inlierThreshold = std::numeric_limits<double>::max(),
92  size_t sampleMaxIterations = 1000,
93  bool seedRandom = false );
94 
95  //! Virtual destructor
96  virtual ~ModelRegistration();
97 
98  //! Returns the minimum valid sample size for the model
99  virtual size_t getSampleSize() const { return 3; }
100 
101  //! Attempts to form a valid model from the points at the selected indices
102  /*! @param sampleIndices The valid indices (check with isSampleValid() ) to use
103  @return The coefficients for the model */
104  virtual VectorX computeModelCoefficients( Indices const sampleIndices ) const;
105 
106  //! Recomputes the model coefficients using the provided inlier indices
107  /*! This can be called after inliers have been computed to refine a solution
108  @param inlierIndices The inlier indices
109  @param modelCoefficients The coefficients found via computeModelCoefficients()
110  @return The optimized set of model coefficients */
111  virtual VectorX optimizeModelCoefficients( Indices const inlierIndices,
112  VectorX const & modelCoefficients ) const;
113 
114  //! Computes distances from transformed points to their correspondences
115  virtual std::vector<double> getDistancesToModel( VectorX const & modelCoefficients ) const;
116 
117  //! Selects all points from the input data set that are inliers under the model according
118  //! to some threshold
119  virtual Indices selectWithinDistance( VectorX const & modelCoefficients ) const;
120 
121  //! Counts all of the points which respect the model coefficients that are inliers
122  virtual int countWithinDistance( VectorX const & modelCoefficients ) const;
123 
124  protected:
125  //! Computes an optimal distance threshold for the current cloud
126  double computeSampleDistanceThreshold() const;
127 
128  //! Computes a correspondence mapping used internally
129  std::unordered_map<size_t, size_t> computeCorrespondenceMap() const;
130 
131  //! Check if some set of coefficients is valid to use
132  bool isValid( VectorX const & coefs ) const;
133 
134  private:
137  double itsSampleDistanceThreshold; //!< How far points have to be apart to be valid samples
138 
139  std::unordered_map<size_t, size_t> itsCorrespondenceMap; //!< Interanlly used correspondence mapping
140  };
141 } //namespace nrt
142 
143 #endif // INCLUDE_NRT_POINTCLOUD2_SAMPLECONCENSUS_MODELS_MODELREGISTRATION_H_
144 #endif // NRT_HAVE_CLOUD