Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
52namespace pcl {
53namespace registration {
54/** \brief @b ELCH (Explicit Loop Closing Heuristic) class
55 * \author Jochen Sprickerhof
56 * \ingroup registration
57 */
58template <typename PointT>
59class ELCH : public PCLBase<PointT> {
60public:
61 using Ptr = shared_ptr<ELCH<PointT>>;
62 using ConstPtr = shared_ptr<const ELCH<PointT>>;
63
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. */
89 : loop_graph_(new LoopGraph)
90 , loop_start_(0)
91 , loop_end_(0)
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
202protected:
204
205 /** \brief This method should get called before starting the actual computation. */
206 virtual bool
207 initCompute();
208
209private:
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
246public:
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
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition pcl_base.hpp:175
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< const Registration< PointT, PointT, float > > ConstPtr
shared_ptr< Registration< PointT, PointT, float > > Ptr
ELCH (Explicit Loop Closing Heuristic) class
Definition elch.h:59
typename PointCloud::Ptr PointCloudPtr
Definition elch.h:65
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
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
pcl::PointCloud< PointT > PointCloud
Definition elch.h:64
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 PointCloud::ConstPtr PointCloudConstPtr
Definition elch.h:66
boost::graph_traits< LoopGraph >::vertex_descriptor getLoopEnd()
Getter for the last scan of a loop.
Definition elch.h:147
typename Registration::Ptr RegistrationPtr
Definition elch.h:84
boost::graph_traits< LoopGraph >::vertex_descriptor getLoopStart()
Getter for the first scan of a loop.
Definition elch.h:130
~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