Point Cloud Library (PCL)  1.8.1
elch.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_ELCH_H_
42 #define PCL_ELCH_H_
43 
44 #include <pcl/pcl_base.h>
45 #include <pcl/point_types.h>
46 #include <pcl/point_cloud.h>
47 #include <pcl/registration/registration.h>
48 #include <pcl/registration/boost.h>
49 #include <pcl/registration/eigen.h>
50 #include <pcl/registration/icp.h>
51 #include <pcl/registration/boost_graph.h>
52 
53 namespace pcl
54 {
55  namespace registration
56  {
57  /** \brief @b ELCH (Explicit Loop Closing Heuristic) class
58  * \author Jochen Sprickerhof
59  * \ingroup registration
60  */
61  template <typename PointT>
62  class ELCH : public PCLBase<PointT>
63  {
64  public:
65  typedef boost::shared_ptr< ELCH<PointT> > Ptr;
66  typedef boost::shared_ptr< const ELCH<PointT> > ConstPtr;
67 
69  typedef typename PointCloud::Ptr PointCloudPtr;
71 
72  struct Vertex
73  {
74  Vertex () : cloud () {}
75  PointCloudPtr cloud;
76  Eigen::Affine3f transform;
77  };
78 
79  /** \brief graph structure to hold the SLAM graph */
80  typedef boost::adjacency_list<
81  boost::listS, boost::eigen_vecS, boost::undirectedS,
82  Vertex,
83  boost::no_property>
85 
86  typedef boost::shared_ptr< LoopGraph > LoopGraphPtr;
87 
91 
92  /** \brief Empty constructor. */
93  ELCH () :
94  loop_graph_ (new LoopGraph),
95  loop_start_ (0),
96  loop_end_ (0),
97  reg_ (new pcl::IterativeClosestPoint<PointT, PointT>),
98  loop_transform_ (),
99  compute_loop_ (true),
100  vd_ ()
101  {};
102 
103  /** \brief Empty destructor */
104  virtual ~ELCH () {}
105 
106  /** \brief Add a new point cloud to the internal graph.
107  * \param[in] cloud the new point cloud
108  */
109  inline void
110  addPointCloud (PointCloudPtr cloud)
111  {
112  typename boost::graph_traits<LoopGraph>::vertex_descriptor vd = add_vertex (*loop_graph_);
113  (*loop_graph_)[vd].cloud = cloud;
114  if (num_vertices (*loop_graph_) > 1)
115  add_edge (vd_, vd, *loop_graph_);
116  vd_ = vd;
117  }
118 
119  /** \brief Getter for the internal graph. */
120  inline LoopGraphPtr
122  {
123  return (loop_graph_);
124  }
125 
126  /** \brief Setter for a new internal graph.
127  * \param[in] loop_graph the new graph
128  */
129  inline void
130  setLoopGraph (LoopGraphPtr loop_graph)
131  {
132  loop_graph_ = loop_graph;
133  }
134 
135  /** \brief Getter for the first scan of a loop. */
136  inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
138  {
139  return (loop_start_);
140  }
141 
142  /** \brief Setter for the first scan of a loop.
143  * \param[in] loop_start the scan that starts the loop
144  */
145  inline void
146  setLoopStart (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_start)
147  {
148  loop_start_ = loop_start;
149  }
150 
151  /** \brief Getter for the last scan of a loop. */
152  inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
154  {
155  return (loop_end_);
156  }
157 
158  /** \brief Setter for the last scan of a loop.
159  * \param[in] loop_end the scan that ends the loop
160  */
161  inline void
162  setLoopEnd (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_end)
163  {
164  loop_end_ = loop_end;
165  }
166 
167  /** \brief Getter for the registration algorithm. */
168  inline RegistrationPtr
170  {
171  return (reg_);
172  }
173 
174  /** \brief Setter for the registration algorithm.
175  * \param[in] reg the registration algorithm used to compute the transformation between the start and the end of the loop
176  */
177  inline void
178  setReg (RegistrationPtr reg)
179  {
180  reg_ = reg;
181  }
182 
183  /** \brief Getter for the transformation between the first and the last scan. */
184  inline Eigen::Matrix4f
186  {
187  return (loop_transform_);
188  }
189 
190  /** \brief Setter for the transformation between the first and the last scan.
191  * \param[in] loop_transform the transformation between the first and the last scan
192  */
193  inline void
194  setLoopTransform (const Eigen::Matrix4f &loop_transform)
195  {
196  loop_transform_ = loop_transform;
197  compute_loop_ = false;
198  }
199 
200  /** \brief Computes new poses for all point clouds by closing the loop
201  * between start and end point cloud. This will transform all given point
202  * clouds for now!
203  */
204  void
205  compute ();
206 
207  protected:
209 
210  /** \brief This method should get called before starting the actual computation. */
211  virtual bool
212  initCompute ();
213 
214  private:
215  /** \brief graph structure for the internal optimization graph */
216  typedef boost::adjacency_list<
217  boost::listS, boost::vecS, boost::undirectedS,
218  boost::no_property,
219  boost::property< boost::edge_weight_t, double > >
220  LOAGraph;
221 
222  /**
223  * graph balancer algorithm computes the weights
224  * @param[in] g the graph
225  * @param[in] f index of the first node
226  * @param[in] l index of the last node
227  * @param[out] weights array for the weights
228  */
229  void
230  loopOptimizerAlgorithm (LOAGraph &g, double *weights);
231 
232  /** \brief The internal loop graph. */
233  LoopGraphPtr loop_graph_;
234 
235  /** \brief The first scan of the loop. */
236  typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_start_;
237 
238  /** \brief The last scan of the loop. */
239  typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_end_;
240 
241  /** \brief The registration object used to close the loop. */
242  RegistrationPtr reg_;
243 
244  /** \brief The transformation between that start and end of the loop. */
245  Eigen::Matrix4f loop_transform_;
246  bool compute_loop_;
247 
248  /** \brief previously added node in the loop_graph_. */
249  typename boost::graph_traits<LoopGraph>::vertex_descriptor vd_;
250 
251  public:
252  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
253  };
254  }
255 }
256 
257 #include <pcl/registration/impl/elch.hpp>
258 
259 #endif // PCL_ELCH_H_
boost::adjacency_list< boost::listS, boost::eigen_vecS, boost::undirectedS, Vertex, boost::no_property > LoopGraph
graph structure to hold the SLAM graph
Definition: elch.h:84
void setLoopEnd(const typename boost::graph_traits< LoopGraph >::vertex_descriptor &loop_end)
Setter for the last scan of a loop.
Definition: elch.h:162
virtual ~ELCH()
Empty destructor.
Definition: elch.h:104
boost::shared_ptr< const Registration< PointT, PointT, float > > ConstPtr
Definition: registration.h:73
pcl::PointCloud< PointT > PointCloud
Definition: elch.h:68
Eigen::Matrix4f getLoopTransform()
Getter for the transformation between the first and the last scan.
Definition: elch.h:185
PointCloud::ConstPtr PointCloudConstPtr
Definition: elch.h:70
void compute()
Computes new poses for all point clouds by closing the loop between start and end point cloud...
Definition: elch.hpp:218
void setLoopGraph(LoopGraphPtr loop_graph)
Setter for a new internal graph.
Definition: elch.h:130
boost::shared_ptr< Registration< PointSource, PointTarget, Scalar > > Ptr
Definition: registration.h:72
RegistrationPtr getReg()
Getter for the registration algorithm.
Definition: elch.h:169
void setLoopStart(const typename boost::graph_traits< LoopGraph >::vertex_descriptor &loop_start)
Setter for the first scan of a loop.
Definition: elch.h:146
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
ELCH (Explicit Loop Closing Heuristic) class
Definition: elch.h:62
Defines all the PCL implemented PointT point type structures.
void setReg(RegistrationPtr reg)
Setter for the registration algorithm.
Definition: elch.h:178
PCL base class.
Definition: pcl_base.h:68
boost::shared_ptr< LoopGraph > LoopGraphPtr
Definition: elch.h:86
pcl::Registration< PointT, PointT > Registration
Definition: elch.h:88
boost::graph_traits< LoopGraph >::vertex_descriptor getLoopEnd()
Getter for the last scan of a loop.
Definition: elch.h:153
boost::graph_traits< LoopGraph >::vertex_descriptor getLoopStart()
Getter for the first scan of a loop.
Definition: elch.h:137
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
Definition: icp.h:94
boost::shared_ptr< const ELCH< PointT > > ConstPtr
Definition: elch.h:66
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: elch.hpp:158
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Registration::ConstPtr RegistrationConstPtr
Definition: elch.h:90
PointCloud::Ptr PointCloudPtr
Definition: elch.h:69
boost::shared_ptr< ELCH< PointT > > Ptr
Definition: elch.h:65
Eigen::Affine3f transform
Definition: elch.h:76
void setLoopTransform(const Eigen::Matrix4f &loop_transform)
Setter for the transformation between the first and the last scan.
Definition: elch.h:194
A point structure representing Euclidean xyz coordinates, and the RGB color.
ELCH()
Empty constructor.
Definition: elch.h:93
void addPointCloud(PointCloudPtr cloud)
Add a new point cloud to the internal graph.
Definition: elch.h:110
LoopGraphPtr getLoopGraph()
Getter for the internal graph.
Definition: elch.h:121
Registration::Ptr RegistrationPtr
Definition: elch.h:89