Point Cloud Library (PCL) 1.12.1
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
49namespace pcl {
50namespace 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 */
84template <typename GraphT>
86public:
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
207protected:
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
222private:
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:194
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.