iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
TransformationEstimationLM.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 NRT_POINTCLOUD2_REGISTRATION_TRANSFORMATION_TRANSFORMATIONESTIMATIONLM_H_
37 #define NRT_POINTCLOUD2_REGISTRATION_TRANSFORMATION_TRANSFORMATIONESTIMATIONLM_H_
38 
39 #include <nrt/Core/Typing/Enum.H>
40 #include <nrt/PointCloud2/Registration/Transformation/TransformationEstimationBase.H>
41 
42 //! LM Distance Methods
43 /*! @class LMDistanceMethod
44  When doing an LM optimization:
45  -@c nrt::LMDistanceMethod::L1, use manhattan distance
46  -@c nrt::LMDistanceMethod::L2, use euclidean distance
47  -@c nrt::LMDistanceMethod::L2Square, use squared euclidean distance
48  -@c nrt::LMDistanceMethod::Mahalanobis, use mahalanobis distance, see extra documentation about this */
49 NRT_DEFINE_ENUM_CLASS( LMDistanceMethod, (L1) (L2) (L2Square) (Mahalanobis) );
50 
51 namespace nrt
52 {
53  //! Estimates a rigid transform using Levenberg Marquardt
54  /*! This transformation supports multiple distance metrics which are used
55  to score how well the currently estimated transform fits during the LM
56  optimization. See \ref LMDistanceMethod for the available methods.
57 
58  If using Mahalanobis distance, the covariance must be specified with each call to
59  estimateTransform. As such, using Mahalanobis distance is not supported within the
60  default nrt::Registration framework and needs to be done by hand. */
61  class TransformationEstimationLM: public TransformationEstimationBase
62  {
63  public:
64  typedef PointCloud2::AffineTransform AffineTransform;
65 
66  TransformationEstimationLM( LMDistanceMethod method = LMDistanceMethod::L2Square );
67 
68  //! Estimates a rigid transformation between a source and target cloud
69  /*! @param source The source point cloud
70  @param target The target point cloud
71  @return The 4x4 rotation matrix that performs the transform */
72  AffineTransform estimateTransform( PointCloud2 const source,
73  PointCloud2 const target );
74 
75  //! Estimates a rigid transformation between a source and target cloud
76  /*! @param source The source point cloud
77  @param target The target point cloud
78  @param correspondences The correspondences between two clouds
79  @return The 4x4 rotation matrix that performs the transform */
80  AffineTransform estimateTransform( PointCloud2 const source,
81  PointCloud2 const target,
82  Correspondences const correspondences );
83 
84  //! Estimates a rigid transformation between a source and target cloud with some
85  //! given covariance
86  /*! @param source The source point cloud
87  @param target The target point cloud
88  @param correspondences The correspondences between two clouds
89  @param covariance The covariance to use for Mahalanobis distance
90  @return The 4x4 rotation matrix that performs the transform */
91  AffineTransform estimateTransform( PointCloud2 const source,
92  PointCloud2 const target,
93  Correspondences const correspondences,
94  PointCloud2::Matrix4 const & covariance );
95 
96  //! Gets the minimum required number of correspondences to estimate a transform
97  inline size_t minimumNumCorrespondences() const { return 3; }
98 
99  protected:
100  //! Does the actual estimation
101  /*! The covariance matrix is only required for mahalanobis distance */
102  AffineTransform estimate( PointCloud2::ConstIterator<> && sourceBegin,
103  PointCloud2::ConstIterator<> && targetBegin,
104  const size_t srcSize,
105  PointCloud2::Matrix4 const & covariance = PointCloud2::Matrix4::Identity() );
106 
107  private:
108  //! Used by eigen during optimization
109  struct LMFunctor;
110 
111  //! Distance method used
112  LMDistanceMethod itsDistanceMethod;
113  };
114 } // namespace nrt
115 
116 #endif // NRT_POINTCLOUD2_REGISTRATION_TRANSFORMATION_TRANSFORMATIONESTIMATIONLM_H_
117 #endif // NRT_HAVE_CLOUD