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