Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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:
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
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
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
Iterator class for point clouds with or without given indices.
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
GraphHandler class is a wrapper for a general SLAM graph The actual graph class must fulfill the foll...
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
GraphPtr getGraph()
Get a pointer to the BGL graph.
void removeVertex(const Vertex &v)
Remove a vertex from the graph.
shared_ptr< GraphT > GraphPtr
typename boost::graph_traits< GraphT >::edge_descriptor Edge
GraphHandler()
Empty constructor.
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
void clear()
Clear the graph.