Point Cloud Library (PCL)  1.14.0-dev
graph_handler.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/edge_measurements.h>
44 #include <pcl/registration/vertex_estimates.h>
45 #include <pcl/exceptions.h>
46 
47 #include "boost/graph/graph_traits.hpp"
48 
49 namespace pcl {
50 namespace registration {
51 /** \brief @b GraphHandler class is a wrapper for a general SLAM graph
52  * The actual graph class must fulfill the following boost::graph concepts:
53  * - BidirectionalGraph
54  * - AdjacencyGraph
55  * - VertexAndEdgeListGraph
56  * - MutableGraph
57  *
58  * Other valid expressions:
59  * - add_edge (m,g) add a new edge according to the given measurement. Return
60  * type: std::pair<edge_descriptor, bool>
61  * - add_vertex (e,g) add a new vertex according to the given estimate. Return
62  * type: vertex_descriptor
63  * - get_pose (v,g) retrieve the pose estimate for v, if any. Return type:
64  * Eigen::Matrix4f
65  * - get_cloud (v,g) retrieve the cloud pointer associated to v, if any. Return
66  * type: pcl::PointCloud<PointT>::ConstPtr
67  * - set_estimate (v,e,g) set the estimate for an existing vertex. Return type: void.
68  * - set_measurement (d,m,g) set the measurement for an existing edge. Return type:
69  * void. Notation:
70  * - m an edge measurement
71  * - e a vertex estimate
72  * - v a vertex
73  * - d an edge
74  * - g a graph
75  * A valid graph implementation should accept at least the PoseEstimate estimate and the
76  * PoseMeasurement measurement
77  *
78  * If a specific graph implementation needs initialization and/or finalization,
79  * specialize the protected methods init() and deinit() for your graph type
80  *
81  * \author Nicola Fioraio
82  * \ingroup registration
83  */
84 template <typename GraphT>
85 class GraphHandler {
86 public:
87  using Ptr = shared_ptr<GraphHandler<GraphT>>;
88  using ConstPtr = shared_ptr<const GraphHandler<GraphT>>;
89  using GraphPtr = shared_ptr<GraphT>;
90  using GraphConstPtr = shared_ptr<const GraphT>;
91 
92  using Vertex = typename boost::graph_traits<GraphT>::vertex_descriptor;
93  using Edge = typename boost::graph_traits<GraphT>::edge_descriptor;
94 
95  /** \brief Empty constructor. */
96  GraphHandler() : graph_impl_(new GraphT())
97  {
98  if (!init())
99  throw InitFailedException("Graph Initialization Failed",
100  __FILE__,
101  "pcl::registration::GraphHandler::GraphHandler ()",
102  __LINE__);
103  }
104 
105  /** \brief Destructor. */
107 
108  /** \brief Clear the graph */
109  inline void
111  {
112  deinit();
113  graph_impl_.reset(new GraphT());
114  if (!init())
115  throw InitFailedException("Graph Initialization Failed",
116  __FILE__,
117  "pcl::registration::GraphHandler::clear ()",
118  __LINE__);
119  }
120 
121  /** \brief Get a pointer to the BGL graph */
122  inline GraphConstPtr
123  getGraph() const
124  {
125  return graph_impl_;
126  }
127 
128  /** \brief Get a pointer to the BGL graph */
129  inline GraphPtr
131  {
132  return graph_impl_;
133  }
134 
135  /** \brief Add a new point cloud to the graph and return the new vertex
136  * \param cloud the new point cloud
137  * \param pose the pose estimate
138  * \return a reference to the new vertex
139  */
140  template <class PointT>
141  inline Vertex
143  const Eigen::Matrix4f& pose)
144  {
145  return add_vertex(PoseEstimate<PointT>(cloud, pose), *graph_impl_);
146  }
147 
148  /** \brief Add a new generic vertex created according to the given estimate
149  * \param estimate the parameters' estimate
150  * \return a reference to the new vertex
151  */
152  template <class EstimateT>
153  inline Vertex
154  addGenericVertex(const EstimateT& estimate)
155  {
156  return add_vertex(estimate, *graph_impl_);
157  }
158 
159  /** \brief Add a new constraint between two poses
160  * \param v_start the first pose
161  * \param v_end the second pose
162  * \param relative_transformation the transformation from v_start to v_end
163  * \param information_matrix the uncertainty
164  * \return a reference to the new edge
165  */
166  template <class InformationT>
167  inline Edge
168  addPoseConstraint(const Vertex& v_start,
169  const Vertex& v_end,
170  const Eigen::Matrix4f& relative_transformation,
171  const InformationT& information_matrix)
172  {
174  v_start, v_end, relative_transformation, information_matrix),
175  *graph_impl_);
176  }
177 
178  /** \brief Add a generic constraint created according to the given measurement
179  * \param measurement the measurement
180  * \return a reference to the new edge
181  */
182  template <class MeasurementT>
183  inline Edge
184  addGenericConstraint(const MeasurementT& measurement)
185  {
186  return add_edge(measurement, *graph_impl_);
187  }
188 
189  /** \brief Remove a vertex from the graph
190  * \param v the vertex
191  */
192  inline void
194  {
195  remove_vertex(v.v_, *graph_impl_);
196  }
197 
198  /** \brief Remove a constraint from the graph
199  * \param e the edge
200  */
201  inline void
203  {
204  remove_edge(e.e_, *graph_impl_);
205  }
206 
207 protected:
208  /** \brief This method is called right after the creation of graph_impl_ */
209  inline bool
211  {
212  return true;
213  }
214 
215  /** \brief This method is called when graph_impl_ is going to be destroyed */
216  inline bool
218  {
219  return true;
220  }
221 
222 private:
223  GraphPtr graph_impl_;
224 };
225 } // namespace registration
226 } // namespace pcl
Definition: edge.h:49
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:196
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
GraphHandler class is a wrapper for a general SLAM graph The actual graph class must fulfill the foll...
Definition: graph_handler.h:85
Vertex addGenericVertex(const EstimateT &estimate)
Add a new generic vertex created according to the given estimate.
GraphConstPtr getGraph() const
Get a pointer to the BGL graph.
bool init()
This method is called right after the creation of graph_impl_.
bool deinit()
This method is called when graph_impl_ is going to be destroyed.
Vertex addPointCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const Eigen::Matrix4f &pose)
Add a new point cloud to the graph and return the new vertex.
shared_ptr< const GraphT > GraphConstPtr
Definition: graph_handler.h:90
shared_ptr< const GraphHandler< GraphT > > ConstPtr
Definition: graph_handler.h:88
GraphPtr getGraph()
Get a pointer to the BGL graph.
void removeVertex(const Vertex &v)
Remove a vertex from the graph.
shared_ptr< GraphT > GraphPtr
Definition: graph_handler.h:89
shared_ptr< GraphHandler< GraphT > > Ptr
Definition: graph_handler.h:87
typename boost::graph_traits< GraphT >::edge_descriptor Edge
Definition: graph_handler.h:93
GraphHandler()
Empty constructor.
Definition: graph_handler.h:96
Edge addPoseConstraint(const Vertex &v_start, const Vertex &v_end, const Eigen::Matrix4f &relative_transformation, const InformationT &information_matrix)
Add a new constraint between two poses.
Edge addGenericConstraint(const MeasurementT &measurement)
Add a generic constraint created according to the given measurement.
void removeConstraint(const Edge &e)
Remove a constraint from the graph.
typename boost::graph_traits< GraphT >::vertex_descriptor Vertex
Definition: graph_handler.h:92
void clear()
Clear the graph.