Point Cloud Library (PCL)  1.11.0
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 #pragma once
42 
43 #include <pcl/memory.h>
44 #include <pcl/pcl_macros.h>
45 #include <pcl/pcl_base.h>
46 #include <pcl/point_types.h>
47 #include <pcl/point_cloud.h>
48 #include <pcl/registration/registration.h>
49 #include <pcl/registration/boost.h>
50 #include <pcl/registration/eigen.h>
51 #include <pcl/registration/icp.h>
52 #include <pcl/registration/boost_graph.h>
53 
54 namespace pcl
55 {
56  namespace registration
57  {
58  /** \brief @b ELCH (Explicit Loop Closing Heuristic) class
59  * \author Jochen Sprickerhof
60  * \ingroup registration
61  */
62  template <typename PointT>
63  class ELCH : public PCLBase<PointT>
64  {
65  public:
66  using Ptr = shared_ptr<ELCH<PointT> >;
67  using ConstPtr = shared_ptr<const ELCH<PointT> >;
68 
70  using PointCloudPtr = typename PointCloud::Ptr;
72 
73  struct Vertex
74  {
75  Vertex () : cloud () {}
77  Eigen::Affine3f transform;
78  };
79 
80  /** \brief graph structure to hold the SLAM graph */
81  using LoopGraph = boost::adjacency_list<
82  boost::listS, boost::eigen_vecS, boost::undirectedS,
83  Vertex,
84  boost::no_property>;
85 
86  using LoopGraphPtr = shared_ptr<LoopGraph>;
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  compute_loop_ (true),
99  vd_ ()
100  {};
101 
102  /** \brief Empty destructor */
103  ~ELCH () {}
104 
105  /** \brief Add a new point cloud to the internal graph.
106  * \param[in] cloud the new point cloud
107  */
108  inline void
110  {
111  typename boost::graph_traits<LoopGraph>::vertex_descriptor vd = add_vertex (*loop_graph_);
112  (*loop_graph_)[vd].cloud = cloud;
113  if (num_vertices (*loop_graph_) > 1)
114  add_edge (vd_, vd, *loop_graph_);
115  vd_ = vd;
116  }
117 
118  /** \brief Getter for the internal graph. */
119  inline LoopGraphPtr
121  {
122  return (loop_graph_);
123  }
124 
125  /** \brief Setter for a new internal graph.
126  * \param[in] loop_graph the new graph
127  */
128  inline void
130  {
131  loop_graph_ = loop_graph;
132  }
133 
134  /** \brief Getter for the first scan of a loop. */
135  inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
137  {
138  return (loop_start_);
139  }
140 
141  /** \brief Setter for the first scan of a loop.
142  * \param[in] loop_start the scan that starts the loop
143  */
144  inline void
145  setLoopStart (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_start)
146  {
147  loop_start_ = loop_start;
148  }
149 
150  /** \brief Getter for the last scan of a loop. */
151  inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
153  {
154  return (loop_end_);
155  }
156 
157  /** \brief Setter for the last scan of a loop.
158  * \param[in] loop_end the scan that ends the loop
159  */
160  inline void
161  setLoopEnd (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_end)
162  {
163  loop_end_ = loop_end;
164  }
165 
166  /** \brief Getter for the registration algorithm. */
167  inline RegistrationPtr
169  {
170  return (reg_);
171  }
172 
173  /** \brief Setter for the registration algorithm.
174  * \param[in] reg the registration algorithm used to compute the transformation between the start and the end of the loop
175  */
176  inline void
178  {
179  reg_ = reg;
180  }
181 
182  /** \brief Getter for the transformation between the first and the last scan. */
183  inline Eigen::Matrix4f
185  {
186  return (loop_transform_);
187  }
188 
189  /** \brief Setter for the transformation between the first and the last scan.
190  * \param[in] loop_transform the transformation between the first and the last scan
191  */
192  inline void
193  setLoopTransform (const Eigen::Matrix4f &loop_transform)
194  {
195  loop_transform_ = loop_transform;
196  compute_loop_ = false;
197  }
198 
199  /** \brief Computes new poses for all point clouds by closing the loop
200  * between start and end point cloud. This will transform all given point
201  * clouds for now!
202  */
203  void
204  compute ();
205 
206  protected:
208 
209  /** \brief This method should get called before starting the actual computation. */
210  virtual bool
211  initCompute ();
212 
213  private:
214  /** \brief graph structure for the internal optimization graph */
215  using LOAGraph = boost::adjacency_list<
216  boost::listS, boost::vecS, boost::undirectedS,
217  boost::no_property,
218  boost::property< boost::edge_weight_t, double > >;
219 
220  /**
221  * graph balancer algorithm computes the weights
222  * @param[in] g the graph
223  * @param[in] f index of the first node
224  * @param[in] l index of the last node
225  * @param[out] weights array for the weights
226  */
227  void
228  loopOptimizerAlgorithm (LOAGraph &g, double *weights);
229 
230  /** \brief The internal loop graph. */
231  LoopGraphPtr loop_graph_;
232 
233  /** \brief The first scan of the loop. */
234  typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_start_;
235 
236  /** \brief The last scan of the loop. */
237  typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_end_;
238 
239  /** \brief The registration object used to close the loop. */
240  RegistrationPtr reg_;
241 
242  /** \brief The transformation between that start and end of the loop. */
243  Eigen::Matrix4f loop_transform_;
244  bool compute_loop_;
245 
246  /** \brief previously added node in the loop_graph_. */
247  typename boost::graph_traits<LoopGraph>::vertex_descriptor vd_;
248 
249  public:
251  };
252  }
253 }
254 
255 #include <pcl/registration/impl/elch.hpp>
pcl::registration::ELCH::addPointCloud
void addPointCloud(PointCloudPtr cloud)
Add a new point cloud to the internal graph.
Definition: elch.h:109
pcl::registration::ELCH::setLoopStart
void setLoopStart(const typename boost::graph_traits< LoopGraph >::vertex_descriptor &loop_start)
Setter for the first scan of a loop.
Definition: elch.h:145
pcl::Registration< PointT, PointT >::Ptr
shared_ptr< Registration< PointT, PointT, float > > Ptr
Definition: registration.h:71
boost::eigen_vecS
Definition: boost_graph.h:50
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl::registration::ELCH::RegistrationPtr
typename Registration::Ptr RegistrationPtr
Definition: elch.h:89
pcl
Definition: convolution.h:46
point_types.h
pcl::registration::ELCH::ConstPtr
shared_ptr< const ELCH< PointT > > ConstPtr
Definition: elch.h:67
pcl::Registration< PointT, PointT >
pcl::PCLBase::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
pcl::PCLBase::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
pcl::registration::ELCH::getLoopStart
boost::graph_traits< LoopGraph >::vertex_descriptor getLoopStart()
Getter for the first scan of a loop.
Definition: elch.h:136
pcl::registration::ELCH::RegistrationConstPtr
typename Registration::ConstPtr RegistrationConstPtr
Definition: elch.h:90
pcl::registration::ELCH::compute
void compute()
Computes new poses for all point clouds by closing the loop between start and end point cloud.
Definition: elch.hpp:217
pcl::registration::ELCH::setReg
void setReg(RegistrationPtr reg)
Setter for the registration algorithm.
Definition: elch.h:177
pcl::registration::ELCH::setLoopEnd
void setLoopEnd(const typename boost::graph_traits< LoopGraph >::vertex_descriptor &loop_end)
Setter for the last scan of a loop.
Definition: elch.h:161
pcl::registration::ELCH::getLoopGraph
LoopGraphPtr getLoopGraph()
Getter for the internal graph.
Definition: elch.h:120
pcl::registration::ELCH::setLoopTransform
void setLoopTransform(const Eigen::Matrix4f &loop_transform)
Setter for the transformation between the first and the last scan.
Definition: elch.h:193
pcl::IterativeClosestPoint
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
Definition: icp.h:97
pcl::registration::ELCH::Vertex
Definition: elch.h:74
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:70
pcl::registration::ELCH::Ptr
shared_ptr< ELCH< PointT > > Ptr
Definition: elch.h:66
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::registration::ELCH::Vertex::cloud
PointCloudPtr cloud
Definition: elch.h:76
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:629
pcl::registration::ELCH::Vertex::Vertex
Vertex()
Definition: elch.h:75
pcl::registration::ELCH::setLoopGraph
void setLoopGraph(LoopGraphPtr loop_graph)
Setter for a new internal graph.
Definition: elch.h:129
pcl::registration::ELCH
ELCH (Explicit Loop Closing Heuristic) class
Definition: elch.h:64
pcl::registration::ELCH::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: elch.hpp:157
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
pcl::registration::ELCH::LoopGraphPtr
shared_ptr< LoopGraph > LoopGraphPtr
Definition: elch.h:86
pcl::registration::ELCH::getLoopTransform
Eigen::Matrix4f getLoopTransform()
Getter for the transformation between the first and the last scan.
Definition: elch.h:184
pcl::registration::ELCH::Vertex::transform
Eigen::Affine3f transform
Definition: elch.h:77
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
pcl::registration::ELCH::LoopGraph
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
pcl::registration::ELCH::ELCH
ELCH()
Empty constructor.
Definition: elch.h:93
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
pcl::registration::ELCH::getReg
RegistrationPtr getReg()
Getter for the registration algorithm.
Definition: elch.h:168
pcl::Registration< PointT, PointT >::ConstPtr
shared_ptr< const Registration< PointT, PointT, float > > ConstPtr
Definition: registration.h:72
pcl::registration::ELCH::~ELCH
~ELCH()
Empty destructor.
Definition: elch.h:103
pcl::registration::ELCH::getLoopEnd
boost::graph_traits< LoopGraph >::vertex_descriptor getLoopEnd()
Getter for the last scan of a loop.
Definition: elch.h:152
memory.h
Defines functions, macros and traits for allocating and using memory.