pose_graph_impl.h

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Willow Garage, 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 Willow Garage, Inc. 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.
 *
 */


#ifndef POSE_GRAPH_POSE_GRAPH_IMPL_H
#define POSE_GRAPH_POSE_GRAPH_IMPL_H

#include "pose_graph.h"

#define BOOST_NO_HASH
#include <boost/graph/adjacency_list.hpp>

#include <boost/graph/graph_traits.hpp>

namespace pose_graph {

using boost::listS;
using boost::multisetS;
using boost::undirectedS;
using boost::adjacency_list;
using boost::graph_traits;
using std::map;
using std::vector;
using std::set;
using std::pair;

/************************************************************
 * Graph-related typedefs
 ************************************************************/

// Data structure containing info about a node in the pose graph
struct NodeInfo
{
  NodeInfo(NodeId id) : id(id) { initial_pose_estimate.orientation.w = 1.0; }
  NodeInfo() : id(-1) {} // Only used temporarily by boost graph operations
  NodeId id;
  geometry_msgs::Pose initial_pose_estimate;
};

// Data structure containing info about an edge in the pose graph
struct EdgeInfo
{
  EdgeInfo(EdgeId id, NodeId from, NodeId to, double length);
  EdgeInfo(); // Only used temporarily by boost graph operations

  // Unique id for this edge
  const EdgeId id;

  // Edges are undirected in the underlying graph, but we distinguish
  // between the from and to node ids here.  The constraint attached
  // to an edge specifies the pose of the to node in terms of the from node.
  const NodeId from, to;

  // Expectation of length of edge (given Gaussian pose constraint)
  const double length;
};


// Map from edge id to pose constraint
typedef Eigen3::aligned_allocator<pair<EdgeId, PoseConstraint> > Allocator;
typedef map<EdgeId, PoseConstraint, std::less<EdgeId>, Allocator> PoseConstraintMap;


// boost graph and related types
typedef adjacency_list<multisetS, listS, undirectedS, NodeInfo, EdgeInfo> PoseGraphAdjacencyList;
typedef graph_traits<PoseGraphAdjacencyList>::edge_descriptor PoseGraphEdge;
typedef graph_traits<PoseGraphAdjacencyList>::vertex_descriptor PoseGraphVertex;


/************************************************************
 * Main implementation data structure
 ************************************************************/


class PoseGraphImpl
{
public:


  /****************************************
   * Creation
   ****************************************/
  
  PoseGraphImpl ();

  PoseGraphImpl (const PoseGraphImpl& g);

  PoseGraphImpl& operator= (const PoseGraphImpl& g);
  
  
  /****************************************
   * Basic graph modification ops
   ****************************************/

  NodeId addNode ();

  void addNode (NodeId id);

  EdgeId addEdge (NodeId from, NodeId to, const PoseConstraint& constraint);

  void addEdge (NodeId from, NodeId to, const PoseConstraint& constraint, EdgeId id);

  void setInitialPoseEstimate (NodeId n, const geometry_msgs::Pose& pose);

  const geometry_msgs::Pose& getInitialPoseEstimate (NodeId n) const;


  /****************************************
   * Basic const ops
   ****************************************/

  set<NodeId> allNodes() const;

  set<EdgeId> allEdges() const;

  const PoseConstraint& getConstraint (EdgeId id) const;

  double edgeLength (EdgeId id) const;

  set<EdgeId> incidentEdges (NodeId n) const;

  pair<NodeId, NodeId> incidentNodes (EdgeId e) const;

  std::set<NodeId> neighbors (NodeId n) const;

  bool nodeIdExists (NodeId id) const;

  bool edgeIdExists (EdgeId id) const;

  PoseGraph subgraph (const std::set<NodeId>& nodes) const;

  geometry_msgs::Pose relativePose (NodeId n, NodeId ref) const;

  NodeId otherNode (const NodeId n, const EdgeId e) const;

  /****************************************
   * Graph search
   ****************************************/

  ShortestPathResult shortestPath(NodeId src, NodeId dest) const;

  set<NodeId> nearbyNodes (NodeId src, double radius) const;


  /****************************************
   * Point clouds attached to nodes
   ****************************************/

  void attachCloud (NodeId id, occupancy_grid_utils::LocalizedCloud::ConstPtr cloud);

  occupancy_grid_utils::LocalizedCloud::ConstPtr getCloud (NodeId id) const;

  bool hasCloud (NodeId id) const;

  /****************************************
   * Laser scans attached to nodes
   ****************************************/
  
  void attachScan (NodeId id, sensor_msgs::LaserScan::ConstPtr cloud);

  sensor_msgs::LaserScanConstPtr getScan (NodeId id) const;

  bool hasScan (NodeId id) const;

  
private:


  /******************************
   * Typedefs
   ******************************/

  typedef map<PoseGraphVertex, double> DistanceMap;
  typedef map<PoseGraphVertex, PoseGraphVertex> PredecessorMap;
  typedef pair<DistanceMap, PredecessorMap> DijkstraResult;

  
  /******************************
   * Private methods
   ******************************/

  // Initialization, used in copy constructor and assignment
  void initializeFrom (const PoseGraphImpl& g);

  // Gets vertex corresponding to a node id
  PoseGraphVertex idVertex (NodeId id) const;

  // Gets edge corresponding to an edge id
  PoseGraphEdge idEdge(EdgeId id) const;

  // Wraps call to dijkstra_shortest_paths
  DijkstraResult dijkstra (const PoseGraphVertex& src) const;

  // Checks if a node's distance in a distance map is above a threshold
  bool outsideRadius (NodeId n, double r, const DistanceMap& distances) const;

  EdgeId shortestEdgeBetween (NodeId n1, NodeId n2) const;

  /******************************
   * Fields
   ******************************/

  // The underlying boost graph 
  PoseGraphAdjacencyList graph_;

  // Id that will be used for the next new node
  NodeId next_node_id_;

  // Id that will be used for the next new edge
  EdgeId next_edge_id_;

  // Map from NodeId to Vertex
  map<NodeId, PoseGraphVertex> vertex_map_;

  // Map from EdgeId to Edge
  map<EdgeId, PoseGraphEdge> edge_map_;

  // Map from EdgeId to PoseConstraint
  PoseConstraintMap constraints_;

  // Map from vertex to point cloud
  map<PoseGraphVertex, occupancy_grid_utils::LocalizedCloud::ConstPtr> clouds_;

  // Map from vertex to laser scan
  map<PoseGraphVertex, sensor_msgs::LaserScan::ConstPtr> scans_;
};



  

  

  
   

  




} //namespace pose_graph


#endif // POSE_GRAPH_POSE_GRAPH_H