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