Loading...
Searching...
No Matches
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/edge_measurements.h>
#include <pcl/registration/vertex_estimates.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_;
};
} // namespace registration
} // namespace pcl
pcl::Edge
Definition edge.h:49
pcl::InitFailedException
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition exceptions.h:197
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:414
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:85
pcl::registration::GraphHandler::addGenericVertex
Vertex addGenericVertex(const EstimateT &estimate)
Add a new generic vertex created according to the given estimate.
Definition graph_handler.h:154
pcl::registration::GraphHandler::getGraph
GraphConstPtr getGraph() const
Get a pointer to the BGL graph.
Definition graph_handler.h:123
pcl::registration::GraphHandler::init
bool init()
This method is called right after the creation of graph_impl_.
Definition graph_handler.h:210
pcl::registration::GraphHandler::deinit
bool deinit()
This method is called when graph_impl_ is going to be destroyed.
Definition graph_handler.h:217
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:142
pcl::registration::GraphHandler::GraphConstPtr
shared_ptr< const GraphT > GraphConstPtr
Definition graph_handler.h:90
pcl::registration::GraphHandler::ConstPtr
shared_ptr< const GraphHandler< GraphT > > ConstPtr
Definition graph_handler.h:88
pcl::registration::GraphHandler::getGraph
GraphPtr getGraph()
Get a pointer to the BGL graph.
Definition graph_handler.h:130
pcl::registration::GraphHandler::removeVertex
void removeVertex(const Vertex &v)
Remove a vertex from the graph.
Definition graph_handler.h:193
pcl::registration::GraphHandler::GraphPtr
shared_ptr< GraphT > GraphPtr
Definition graph_handler.h:89
pcl::registration::GraphHandler::Ptr
shared_ptr< GraphHandler< GraphT > > Ptr
Definition graph_handler.h:87
pcl::registration::GraphHandler::Edge
typename boost::graph_traits< GraphT >::edge_descriptor Edge
Definition graph_handler.h:93
pcl::registration::GraphHandler::~GraphHandler
~GraphHandler()
Destructor.
Definition graph_handler.h:106
pcl::registration::GraphHandler::GraphHandler
GraphHandler()
Empty constructor.
Definition graph_handler.h:96
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:168
pcl::registration::GraphHandler::addGenericConstraint
Edge addGenericConstraint(const MeasurementT &measurement)
Add a generic constraint created according to the given measurement.
Definition graph_handler.h:184
pcl::registration::GraphHandler::removeConstraint
void removeConstraint(const Edge &e)
Remove a constraint from the graph.
Definition graph_handler.h:202
pcl::registration::GraphHandler::Vertex
typename boost::graph_traits< GraphT >::vertex_descriptor Vertex
Definition graph_handler.h:92
pcl::registration::GraphHandler::clear
void clear()
Clear the graph.
Definition graph_handler.h:110
pcl
Definition convolution.h:46
pcl::registration::PoseEstimate
PoseEstimate struct
Definition vertex_estimates.h:58
pcl::registration::PoseMeasurement
PoseMeasurement struct
Definition edge_measurements.h:59