graph_handler.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#pragma once
#include <pcl/registration/vertex_estimates.h>
#include <pcl/registration/edge_measurements.h>
#include <pcl/exceptions.h>
#include "boost/graph/graph_traits.hpp"
namespace pcl
{
namespace registration
{
/** \brief @b GraphHandler class is a wrapper for a general SLAM graph
* The actual graph class must fulfill the following boost::graph concepts:
* - BidirectionalGraph
* - AdjacencyGraph
* - VertexAndEdgeListGraph
* - MutableGraph
*
* Other valid expressions:
* - add_edge (m,g) add a new edge according to the given measurement. Return type: std::pair<edge_descriptor, bool>
* - add_vertex (e,g) add a new vertex according to the given estimate. Return type: vertex_descriptor
* - get_pose (v,g) retrieve the pose estimate for v, if any. Return type: Eigen::Matrix4f
* - get_cloud (v,g) retrieve the cloud pointer associated to v, if any. Return type: pcl::PointCloud<PointT>::ConstPtr
* - set_estimate (v,e,g) set the estimate for an existing vertex. Return type: void.
* - set_measurement (d,m,g) set the measurement for an existing edge. Return type: void.
* Notation:
* - m an edge measurement
* - e a vertex estimate
* - v a vertex
* - d an edge
* - g a graph
* A valid graph implementation should accept at least the PoseEstimate estimate and the PoseMeasurement measurement
*
* If a specific graph implementation needs initialization and/or finalization,
* specialize the protected methods init() and deinit() for your graph type
*
* \author Nicola Fioraio
* \ingroup registration
*/
template <typename GraphT>
class GraphHandler
{
public:
using Ptr = shared_ptr<GraphHandler<GraphT> >;
using ConstPtr = shared_ptr<const GraphHandler<GraphT> >;
using GraphPtr = shared_ptr<GraphT>;
using GraphConstPtr = shared_ptr<const GraphT>;
using Vertex = typename boost::graph_traits<GraphT>::vertex_descriptor;
using Edge = typename boost::graph_traits<GraphT>::edge_descriptor;
/** \brief Empty constructor. */
GraphHandler () : graph_impl_ (new GraphT ())
{
if (!init ())
throw InitFailedException ("Graph Initialization Failed", __FILE__, "pcl::registration::GraphHandler::GraphHandler ()", __LINE__);
}
/** \brief Destructor. */
~GraphHandler ()
{
deinit ();
}
/** \brief Clear the graph */
inline void
clear ()
{
deinit ();
graph_impl_.reset (new GraphT ());
if (!init ())
throw InitFailedException ("Graph Initialization Failed", __FILE__, "pcl::registration::GraphHandler::clear ()", __LINE__);
}
/** \brief Get a pointer to the BGL graph */
inline GraphConstPtr
getGraph () const
{
return graph_impl_;
}
/** \brief Get a pointer to the BGL graph */
inline GraphPtr
getGraph ()
{
return graph_impl_;
}
/** \brief Add a new point cloud to the graph and return the new vertex
* \param cloud the new point cloud
* \param pose the pose estimate
* \return a reference to the new vertex
*/
template <class PointT> inline Vertex
addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud, const Eigen::Matrix4f& pose)
{
return add_vertex (PoseEstimate<PointT> (cloud, pose), *graph_impl_);
}
/** \brief Add a new generic vertex created according to the given estimate
* \param estimate the parameters' estimate
* \return a reference to the new vertex
*/
template <class EstimateT> inline Vertex
addGenericVertex (const EstimateT& estimate)
{
return add_vertex (estimate, *graph_impl_);
}
/** \brief Add a new constraint between two poses
* \param v_start the first pose
* \param v_end the second pose
* \param relative_transformation the transformation from v_start to v_end
* \param information_matrix the uncertainty
* \return a reference to the new edge
*/
template <class InformationT> inline Edge
addPoseConstraint ( const Vertex& v_start, const Vertex& v_end,
const Eigen::Matrix4f& relative_transformation,
const InformationT& information_matrix)
{
return add_edge ( PoseMeasurement<Vertex, InformationT> (v_start, v_end, relative_transformation, information_matrix),
*graph_impl_);
}
/** \brief Add a generic constraint created according to the given measurement
* \param measurement the measurement
* \return a reference to the new edge
*/
template <class MeasurementT> inline Edge
addGenericConstraint (const MeasurementT& measurement)
{
return add_edge (measurement, *graph_impl_);
}
/** \brief Remove a vertex from the graph
* \param v the vertex
*/
inline void
removeVertex (const Vertex& v)
{
remove_vertex (v.v_, *graph_impl_);
}
/** \brief Remove a constraint from the graph
* \param e the edge
*/
inline void
removeConstraint (const Edge& e)
{
remove_edge(e.e_, *graph_impl_);
}
protected:
/** \brief This method is called right after the creation of graph_impl_ */
inline bool
init ()
{
return true;
}
/** \brief This method is called when graph_impl_ is going to be destroyed */
inline bool
deinit ()
{
return true;
}
private:
GraphPtr graph_impl_;
};
}
}
pcl::registration::GraphHandler::Edge
typename boost::graph_traits< GraphT >::edge_descriptor Edge
Definition: graph_handler.h:90
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::registration::GraphHandler::addGenericVertex
Vertex addGenericVertex(const EstimateT &estimate)
Add a new generic vertex created according to the given estimate.
Definition: graph_handler.h:145
pcl::registration::PoseEstimate
PoseEstimate struct
Definition: vertex_estimates.h:61
pcl::registration::GraphHandler::getGraph
GraphConstPtr getGraph() const
Get a pointer to the BGL graph.
Definition: graph_handler.h:117
pcl::registration::GraphHandler::addPoseConstraint
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.
Definition: graph_handler.h:158
pcl::registration::GraphHandler
GraphHandler class is a wrapper for a general SLAM graph The actual graph class must fulfill the foll...
Definition: graph_handler.h:81
pcl::registration::GraphHandler::GraphPtr
shared_ptr< GraphT > GraphPtr
Definition: graph_handler.h:86
pcl::registration::GraphHandler::GraphConstPtr
shared_ptr< const GraphT > GraphConstPtr
Definition: graph_handler.h:87
pcl::registration::GraphHandler::getGraph
GraphPtr getGraph()
Get a pointer to the BGL graph.
Definition: graph_handler.h:124
pcl::registration::GraphHandler::Ptr
shared_ptr< GraphHandler< GraphT > > Ptr
Definition: graph_handler.h:84
pcl::registration::GraphHandler::addPointCloud
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.
Definition: graph_handler.h:135
pcl::registration::GraphHandler::~GraphHandler
~GraphHandler()
Destructor.
Definition: graph_handler.h:100
pcl::registration::GraphHandler::Vertex
typename boost::graph_traits< GraphT >::vertex_descriptor Vertex
Definition: graph_handler.h:89
pcl::registration::GraphHandler::init
bool init()
This method is called right after the creation of graph_impl_.
Definition: graph_handler.h:197
pcl::registration::GraphHandler::addGenericConstraint
Edge addGenericConstraint(const MeasurementT &measurement)
Add a generic constraint created according to the given measurement.
Definition: graph_handler.h:171
pcl::registration::GraphHandler::ConstPtr
shared_ptr< const GraphHandler< GraphT > > ConstPtr
Definition: graph_handler.h:85
pcl::registration::GraphHandler::removeVertex
void removeVertex(const Vertex &v)
Remove a vertex from the graph.
Definition: graph_handler.h:180
pcl::InitFailedException
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:194
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
pcl::registration::GraphHandler::deinit
bool deinit()
This method is called when graph_impl_ is going to be destroyed.
Definition: graph_handler.h:204
pcl::registration::GraphHandler::removeConstraint
void removeConstraint(const Edge &e)
Remove a constraint from the graph.
Definition: graph_handler.h:189
pcl::registration::GraphHandler::clear
void clear()
Clear the graph.
Definition: graph_handler.h:107
pcl::Edge
Definition: edge.h:48
pcl::registration::PoseMeasurement
PoseMeasurement struct
Definition: edge_measurements.h:61
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
pcl::registration::GraphHandler::GraphHandler
GraphHandler()
Empty constructor.
Definition: graph_handler.h:93