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