Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_GRAPH_HANDLER_H_ 00042 #define PCL_GRAPH_HANDLER_H_ 00043 00044 #include <pcl/registration/vertex_estimates.h> 00045 #include <pcl/registration/edge_measurements.h> 00046 #include <pcl/exceptions.h> 00047 #include "boost/graph/graph_traits.hpp" 00048 00049 namespace pcl 00050 { 00051 namespace registration 00052 { 00053 /** \brief @b GraphHandler class is a wrapper for a general SLAM graph 00054 * The actual graph class must fulfil the following boost::graph concepts: 00055 * - BidirectionalGraph 00056 * - AdjacencyGraph 00057 * - VertexAndEdgeListGraph 00058 * - MutableGraph 00059 * 00060 * Other valid expressions: 00061 * - add_edge (m,g) add a new edge according to the given measurement. Return type: std::pair<edge_descriptor, bool> 00062 * - add_vertex (e,g) add a new vertex according to the given estimate. Return type: vertex_descriptor 00063 * - get_pose (v,g) retrieve the pose estimate for v, if any. Return type: Eigen::Matrix4f 00064 * - get_cloud (v,g) retrieve the cloud pointer associated to v, if any. Return type: pcl::PointCloud<PointT>::ConstPtr 00065 * - set_estimate (v,e,g) set the estimate for an existing vertex. Return type: void. 00066 * - set_measurement (d,m,g) set the measurement for an existing edge. Return type: void. 00067 * Notation: 00068 * - m an edge measurement 00069 * - e a vertex estimate 00070 * - v a vertex 00071 * - d an edge 00072 * - g a graph 00073 * A valid graph implementation should accept at least the PoseEstimate estimate and the PoseMeasurement measurement 00074 * 00075 * If a specific graph implementation needs initialization and/or finalization, 00076 * specialize the protected methods init() and deinit() for your graph type 00077 * 00078 * \author Nicola Fioraio 00079 * \ingroup registration 00080 */ 00081 template <typename GraphT> 00082 class GraphHandler 00083 { 00084 public: 00085 typedef boost::shared_ptr<GraphHandler<GraphT> > Ptr; 00086 typedef boost::shared_ptr<const GraphHandler<GraphT> > ConstPtr; 00087 typedef boost::shared_ptr<GraphT> GraphPtr; 00088 typedef boost::shared_ptr<const GraphT> GraphConstPtr; 00089 00090 typedef typename boost::graph_traits<GraphT>::vertex_descriptor Vertex; 00091 typedef typename boost::graph_traits<GraphT>::edge_descriptor Edge; 00092 00093 /** \brief Empty constructor. */ 00094 GraphHandler () : graph_impl_ (new GraphT ()) 00095 { 00096 if (!init ()) 00097 throw InitFailedException ("Graph Initialization Failed", __FILE__, "pcl::registration::GraphHandler::GraphHandler ()", __LINE__); 00098 } 00099 00100 /** \brief Destructor. */ 00101 ~GraphHandler () 00102 { 00103 deinit (); 00104 } 00105 00106 /** \brief Clear the graph */ 00107 inline void 00108 clear () 00109 { 00110 deinit (); 00111 graph_impl_.reset (new GraphT ()); 00112 if (!init ()) 00113 throw InitFailedException ("Graph Initialization Failed", __FILE__, "pcl::registration::GraphHandler::clear ()", __LINE__); 00114 } 00115 00116 /** \brief Get a pointer to the BGL graph */ 00117 inline GraphConstPtr 00118 getGraph () const 00119 { 00120 return graph_impl_; 00121 } 00122 00123 /** \brief Get a pointer to the BGL graph */ 00124 inline GraphPtr 00125 getGraph () 00126 { 00127 return graph_impl_; 00128 } 00129 00130 /** \brief Add a new point cloud to the graph and return the new vertex 00131 * \param cloud the new point cloud 00132 * \param pose the pose estimate 00133 * \return a reference to the new vertex 00134 */ 00135 template <class PointT> inline Vertex 00136 addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud, const Eigen::Matrix4f& pose) 00137 { 00138 return add_vertex (PoseEstimate<PointT> (cloud, pose), *graph_impl_); 00139 } 00140 00141 /** \brief Add a new generic vertex created according to the given estimate 00142 * \param estimate the parameters' estimate 00143 * \return a reference to the new vertex 00144 */ 00145 template <class EstimateT> inline Vertex 00146 addGenericVertex (const EstimateT& estimate) 00147 { 00148 return add_vertex (estimate, *graph_impl_); 00149 } 00150 00151 /** \brief Add a new constraint between two poses 00152 * \param v_start the first pose 00153 * \param v_end the second pose 00154 * \param relative_transformation the transformation from v_start to v_end 00155 * \param information_matrix the uncertainty 00156 * \return a reference to the new edge 00157 */ 00158 template <class InformationT> inline Edge 00159 addPoseConstraint ( const Vertex& v_start, const Vertex& v_end, 00160 const Eigen::Matrix4f& relative_transformation, 00161 const InformationT& information_matrix) 00162 { 00163 return add_edge ( PoseMeasurement<Vertex, InformationT> (v_start, v_end, relative_transformation, information_matrix), 00164 *graph_impl_); 00165 } 00166 00167 /** \brief Add a generic constraint created according to the given measurement 00168 * \param measurement the measurement 00169 * \return a reference to the new edge 00170 */ 00171 template <class MeasurementT> inline Edge 00172 addGenericConstraint (const MeasurementT& measurement) 00173 { 00174 return add_edge (measurement, *graph_impl_); 00175 } 00176 00177 /** \brief Remove a vertex from the graph 00178 * \param v the vertex 00179 */ 00180 inline void 00181 removeVertex (const Vertex& v) 00182 { 00183 remove_vertex (v.v_, *graph_impl_); 00184 } 00185 00186 /** \brief Remove a constraint from the graph 00187 * \param e the edge 00188 */ 00189 inline void 00190 removeConstraint (const Edge& e) 00191 { 00192 remove_edge(e.e_, *graph_impl_); 00193 } 00194 00195 protected: 00196 /** \brief This method is called right after the creation of graph_impl_ */ 00197 inline bool 00198 init () 00199 { 00200 return true; 00201 } 00202 00203 /** \brief This method is called when graph_impl_ is going to be destroyed */ 00204 inline bool 00205 deinit () 00206 { 00207 return true; 00208 } 00209 00210 private: 00211 GraphPtr graph_impl_; 00212 }; 00213 } 00214 } 00215 00216 #endif // PCL_GRAPH_HANDLER_H_