pose_graph.cpp

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.
 *
 */



#include <pose_graph/pose_graph_impl.h>
#include <pose_graph/exception.h>
#include <pose_graph/utils.h>
#include <pose_graph/transforms.h>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/lexical_cast.hpp>
#include <ros/assert.h>
#include <ros/console.h>
#include <list>
#include <boost/optional.hpp>

namespace pose_graph
{

using boost::tie;
using boost::bind;
using boost::ref;
using boost::optional;
using boost::associative_property_map;
using std::pair;
using std::map;
using std::list;
using std::string;
using geometry_msgs::Pose;
using sensor_msgs::LaserScan;

typedef set<EdgeId> EdgeIdSet;
typedef set<NodeId> NodeIdSet;
typedef PoseGraphAdjacencyList::out_edge_iterator OutEdgeIter;
typedef PoseGraphAdjacencyList::in_edge_iterator InEdgeIter;
typedef PoseGraphAdjacencyList::vertex_iterator VertexIter;
typedef pair<NodeId, NodeId> IncidentNodes;
using occupancy_grid_utils::LocalizedCloud;




/************************************************************
 * Internal utilities
 ************************************************************/

bool PoseGraphImpl::nodeIdExists (const NodeId id) const
{
  return contains(vertex_map_, id);
}

bool PoseGraphImpl::edgeIdExists (const EdgeId id) const
{
  return contains(edge_map_, id);
}

PoseGraphVertex PoseGraphImpl::idVertex (const NodeId id) const
{
  if (!nodeIdExists(id)) 
    throw UnknownNodeIdException(id);
  else
    return vertex_map_.find(id)->second;
}

PoseGraphEdge PoseGraphImpl::idEdge (const EdgeId id) const
{
  if (!edgeIdExists(id))
    throw UnknownEdgeIdException(id);
  else 
    return edge_map_.find(id)->second;
}


NodeId vertexId (const PoseGraphAdjacencyList& graph, const PoseGraphVertex& v)
{
  return graph[v].id;
}

EdgeId edgeId (const PoseGraphAdjacencyList& graph, const PoseGraphEdge& e)
{
  return graph[e].id;
}


// Get the key from a map entry
template <class K, class V>
K getKey (const pair<K, V> entry)
{
  return entry.first;
}
  

// Return the other node incident to a given edge
NodeId PoseGraphImpl::otherNode (const NodeId n, const EdgeId e) const
{
  IncidentNodes nodes = incidentNodes(e);
  if (nodes.first == n) 
    return nodes.second;
  else if (nodes.second == n)
    return nodes.first;
  else
    ROS_ASSERT_MSG(false, "Code assumption violated: neither node %lu or %lu of %lu equals %lu",
                   nodes.first.getId(), nodes.second.getId(), e.getId(), n.getId());
  return NodeId(-1); // Never happens
}


/************************************************************
 * Creation
 ************************************************************/

PoseGraphImpl::PoseGraphImpl () : next_node_id_(1), next_edge_id_(1)
{
}


PoseGraphImpl::PoseGraphImpl (const PoseGraphImpl& g) : next_node_id_(1), next_edge_id_(1)
{
  initializeFrom(g);
}

PoseGraphImpl& PoseGraphImpl::operator= (const PoseGraphImpl& g)
{
  if (this!=&g)
    initializeFrom(g);
  return *this;
}
  

void PoseGraphImpl::initializeFrom (const PoseGraphImpl& g)
{
  graph_.clear();
  next_node_id_ = NodeId(1);
  next_edge_id_ = EdgeId(1);
  vertex_map_.clear();
  edge_map_.clear();
  constraints_.clear();
  clouds_.clear();
  scans_.clear();
  
  ROS_ASSERT_MSG (allNodes().empty() && allEdges().empty(),
                  "Can't assign to a nonempty PoseGraph");

  const NodeIdSet nodes = g.allNodes();
  BOOST_FOREACH (const NodeId n, nodes) {
    addNode(n);
    setInitialPoseEstimate(n, g.getInitialPoseEstimate(n));
  }

  BOOST_FOREACH (const NodeId n, nodes) {
    BOOST_FOREACH (const EdgeId e, g.incidentEdges(n)) {
      IncidentNodes incident_nodes = g.incidentNodes(e);
      if (n==incident_nodes.first)
        addEdge(n, incident_nodes.second, g.getConstraint(e), e);
    }
      
    if (g.hasCloud(n))
      attachCloud(n, g.getCloud(n));
    if (g.hasScan(n))
      attachScan(n, g.getScan(n));
  }
}




/************************************************************
 * Basic graph ops
 ************************************************************/

/****************************************
 * Nodes
 ****************************************/


// Uses next_node_id_ to generate an id, and adds node
NodeId PoseGraphImpl::addNode ()
{
  while (nodeIdExists(next_node_id_))
    next_node_id_++;

  addNode(next_node_id_);
  return next_node_id_;
}

// Adds a node with fixed id
void PoseGraphImpl::addNode (const NodeId id)
{
  ROS_DEBUG_STREAM_NAMED ("add_node", "Adding node " << id);
  if (nodeIdExists(id)) 
    throw DuplicateNodeIdException(id);
  else 
    vertex_map_[id] = add_vertex(NodeInfo(id), graph_);
}

// Sets node initial pose estimate
void PoseGraphImpl::setInitialPoseEstimate (const NodeId n, const Pose& pose)
{
  graph_[idVertex(n)].initial_pose_estimate = pose;
}

    
    
/****************************************
 * Edges
 ****************************************/

// Uses next_edge_id_ to generate an id, and adds edge
EdgeId PoseGraphImpl::addEdge (const NodeId from, const NodeId to, const PoseConstraint& constraint)
{
  while (edgeIdExists(next_edge_id_))
    next_edge_id_++;
  addEdge(from, to, constraint, next_edge_id_);
  return next_edge_id_;
}


// Adds an edge with fixed id
void PoseGraphImpl::addEdge (const NodeId from, const NodeId to, const PoseConstraint& constraint, const EdgeId id)
{
  ROS_DEBUG_STREAM_NAMED ("add_edge", "Adding edge " << id << " from " << from << " to " << to);

  const PoseGraphVertex from_vertex = idVertex(from);
  const PoseGraphVertex to_vertex = idVertex(to);

  pair<PoseGraphEdge, bool> result = add_edge(from_vertex, to_vertex, 
                                              EdgeInfo(id, from, to, length(constraint)), graph_);
  ROS_ASSERT(result.second);
  edge_map_[id] = result.first;
  constraints_[id] = constraint;
}

EdgeInfo::EdgeInfo(const EdgeId id, const NodeId from, const NodeId to, const double length) : 
  id(id), from(from), to(to), length(length)
{
}

// Only used temporarily by boost graph operations
EdgeInfo::EdgeInfo() : id(-1), from(-1), to(-1), length(-1.0)
{} 


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


// Gets set of node ids
NodeIdSet PoseGraphImpl::allNodes () const
{
  NodeIdSet nodes;
  transform (vertex_map_.begin(), vertex_map_.end(), inserter(nodes, nodes.begin()), &getKey<NodeId, PoseGraphVertex>);
  return nodes;
}


// Gets set of edge ids
EdgeIdSet PoseGraphImpl::allEdges () const
{
  EdgeIdSet edges;
  transform (edge_map_.begin(), edge_map_.end(), inserter(edges, edges.begin()), &getKey<EdgeId, PoseGraphEdge>);
  return edges;
}


// Gets pair of node ids incident to an edge
IncidentNodes PoseGraphImpl::incidentNodes (const EdgeId e) const
{
  PoseGraphEdge edge = idEdge(e);
  return IncidentNodes(graph_[source(edge, graph_)].id, graph_[target(edge, graph_)].id);
}


// Gets set of edge ids incident to this node
EdgeIdSet PoseGraphImpl::incidentEdges (const NodeId n) const
{
  EdgeIdSet ids;
  PoseGraphVertex v = idVertex(n);

  OutEdgeIter out_iter, out_end;
  tie(out_iter, out_end) = out_edges(v, graph_);
  transform(out_iter, out_end, inserter(ids, ids.begin()), bind(edgeId, ref(graph_), _1));

  return ids;
}

NodeIdSet PoseGraphImpl::neighbors (const NodeId n) const
{
  NodeIdSet neighbors;
  BOOST_FOREACH (const EdgeId e, incidentEdges(n))
    neighbors.insert(otherNode(n, e));
  return neighbors;
}


// Get the pose constraint attached to an edge
const PoseConstraint& PoseGraphImpl::getConstraint (const EdgeId e) const
{
  if (edgeIdExists(e)) {
    return keyValue(constraints_, e);
  }
  else
    throw UnknownEdgeIdException(e);
}


// Initial pose estimate for a node
const Pose& PoseGraphImpl::getInitialPoseEstimate (const NodeId n) const
{
  return graph_[idVertex(n)].initial_pose_estimate;
}
  

// Length of an edge
double PoseGraphImpl::edgeLength (const EdgeId id) const
{
  return graph_[idEdge(id)].length;
}

Pose addRelativePose (const PoseGraphImpl* g, const Pose& pose, const EdgeId e)
{

  return Pose();
}
  

Pose PoseGraphImpl::relativePose (const NodeId n, const NodeId ref) const
{
  ShortestPathResult path = shortestPath(ref, n);
  if (!path.path_found)
    throw DisconnectedNodesException(ref, n);

  Eigen3::Affine3d trans;
  trans = Eigen3::AngleAxis<double>(0, Eigen3::Vector3d(0, 0, 1));
  
  for (unsigned i=0; i<path.edges.size(); i++) {
    const EdgeId e = path.edges[i];
    const Eigen3::Affine3d rel_trans = poseToWorldTransform(getPose(getConstraint(e)));
    const NodeId from = incidentNodes(e).first;
    const NodeId to = incidentNodes(e).second;
    if (from == path.nodes[i]) {
      ROS_ASSERT (to == path.nodes[i+1]);
      trans = trans*rel_trans;
    }
    else {
      ROS_ASSERT (to == path.nodes[i]);
      trans = trans*rel_trans.inverse();
    }
  }
  return convertToPose(trans);
                    
}




/************************************************************
 * Path planning
 ************************************************************/


// Internal function that wraps the actual BGL call
PoseGraphImpl::DijkstraResult PoseGraphImpl::dijkstra (const PoseGraphVertex& src) const
{
  // Define some additional map types to store the weights and indices used within Dijkstra's algorithm
  typedef map<PoseGraphEdge, double> WeightMap;
  typedef map<PoseGraphVertex, unsigned> IndexMap;

  // dijkstra_shortest_paths further requires std::map to be wrapped in a "Property Map"
  typedef associative_property_map<DistanceMap> DistancePMap;
  typedef associative_property_map<PredecessorMap> PredecessorPMap;
  typedef associative_property_map<WeightMap> WeightPMap;
  typedef associative_property_map<IndexMap> IndexPMap;

  // OK, step 1: set the indices arbitrarily, because bgl won't do this for you
  IndexMap imap;
  IndexPMap index_pmap(imap);
  { 
    unsigned ind=0;
    BOOST_FOREACH (const NodeId id, allNodes())
      imap[idVertex(id)] = ind++;
  }

  // Step 2: set the weights, using the edge lengths
  WeightMap wmap;
  WeightPMap weight_pmap(wmap);
  BOOST_FOREACH (const EdgeId id, allEdges())
    wmap[idEdge(id)] = edgeLength(id);

  // Step 3: set up output maps
  DistanceMap dmap;
  DistancePMap distance_pmap(dmap);
  PredecessorMap pmap;
  PredecessorPMap predecessor_pmap(pmap);

  // Step 4: make the call
  boost::dijkstra_shortest_paths(graph_, src, weight_map(weight_pmap).vertex_index_map(index_pmap).
                                 distance_map(distance_pmap).predecessor_map(predecessor_pmap));

  // Step 5: Return
  return DijkstraResult(dmap, pmap);
}


EdgeId PoseGraphImpl::shortestEdgeBetween (const NodeId n1, const NodeId n2) const
{
  optional<EdgeId> shortest;
  optional<double> length;
  const PoseGraphVertex node = idVertex(n1);
  BOOST_FOREACH (const PoseGraphEdge& e, out_edges(node, graph_)) {
    const EdgeId id = graph_[e].id;
    if (otherNode(n1, id) == n2 &&
        (!length || *length > edgeLength(id))) {
      shortest = id;
      length = edgeLength(id);
    }
  }
  ROS_ASSERT (shortest && length);
  return *shortest;
}

// Finds shortest path using Dijkstra's algorithm
ShortestPathResult PoseGraphImpl::shortestPath(const NodeId src, const NodeId dest) const
{
  // Make the call
  const PoseGraphVertex v = idVertex(src);
  const PoseGraphVertex w = idVertex(dest);
  DijkstraResult result = dijkstra(v);
  ROS_ASSERT_MSG (contains(result.first, w), "%lu not found in shortest path distances from %lu",
                  dest.getId(), src.getId());

  // Extract the path
  if (dest==src || result.second[w] !=w) {

    list<NodeId> path;
    for (PoseGraphVertex x = w; x!=result.second[x]; x=result.second[x])
      path.push_front(vertexId(graph_, x));
    path.push_front(src);

    // Fill in the return value; for now we don't fill in the edges field
    ShortestPathResult ret_val;
    copy(path.begin(), path.end(), back_inserter(ret_val.nodes));
    ROS_ASSERT (path.begin()!=path.end());
    transform(path.begin(), --path.end(), ++path.begin(), back_inserter(ret_val.edges),
              bind(&PoseGraphImpl::shortestEdgeBetween, this, _1, _2));
    ret_val.path_found = true;
    ret_val.cost = result.first[w];
    return ret_val;
  }

  else {
    ShortestPathResult ret_val;
    ret_val.path_found = false;
    return ret_val;
  }
}


bool PoseGraphImpl::outsideRadius (NodeId n, double r, const PoseGraphImpl::DistanceMap& distances) const
{
  return keyValue(distances, idVertex(n)) > r;
}


// Uses Dijkstra propagation (over the entire graph, which is inefficient), and selects the nodes within a given radius
NodeIdSet PoseGraphImpl::nearbyNodes (const NodeId src, const double radius) const
{
  // Set up the call to Dijkstra's algorithm
  const PoseGraphVertex v = idVertex(src);
  const DijkstraResult result = dijkstra(v);

  // Get the nodes that are within the radius
  const NodeIdSet nodes = allNodes();
  NodeIdSet nearby_nodes;
  remove_copy_if(nodes.begin(), nodes.end(), inserter(nearby_nodes, nearby_nodes.begin()), bind(&PoseGraphImpl::outsideRadius, this, _1, radius, ref(result.first)));
  return nearby_nodes;  
}





/************************************************************
 * Point clouds 
 ************************************************************/

void PoseGraphImpl::attachCloud (const NodeId id, LocalizedCloud::ConstPtr cloud)
{
  const PoseGraphVertex v = idVertex(id);
  clouds_[v] = cloud;
}


LocalizedCloud::ConstPtr PoseGraphImpl::getCloud (const NodeId id) const
{
  
  const PoseGraphVertex v = idVertex(id);
  if (contains(clouds_, v))
    return keyValue(clouds_, v);
  else 
    throw PoseGraphException (boost::format("No cloud found for node %1%") % id );
}

bool PoseGraphImpl::hasCloud (const NodeId id) const
{
  const PoseGraphVertex v = idVertex(id);
  return contains(clouds_, v);
}

PoseGraph PoseGraphImpl::subgraph (const set<NodeId>& nodes) const
{
  // Add the nodes
  PoseGraph g;
  BOOST_FOREACH (const NodeId n, nodes) 
    g.addNode(n);

  // Add the edges
  BOOST_FOREACH (const NodeId n, nodes) {
    BOOST_FOREACH (const EdgeId e, incidentEdges(n)) {
      if (g.nodeIdExists(otherNode(n, e)) &&
          !g.edgeIdExists(e)) {
        const IncidentNodes nodes=incidentNodes(e);
        g.addEdge(nodes.first, nodes.second, getConstraint(e), e);
      }
    }
  }  
  return g;
}

/************************************************************
 * Laser Scans
 ***********************************************************/


void PoseGraphImpl::attachScan (const NodeId id, LaserScan::ConstPtr scan)
{
  const PoseGraphVertex v = idVertex(id);
  scans_[v] = scan;
}


LaserScan::ConstPtr PoseGraphImpl::getScan (const NodeId id) const
{
  const PoseGraphVertex v = idVertex(id);
  if (contains(scans_, v))
    return keyValue(scans_, v);
  else 
    throw PoseGraphException (boost::format("No scan found for node %1%") % id );
}

bool PoseGraphImpl::hasScan (const NodeId id) const
{
  const PoseGraphVertex v = idVertex(id);
  return contains(scans_, v);
}



/************************************************************
 * The interface class just forwards all operations
 ************************************************************/

PoseGraph::~PoseGraph() {}

PoseGraph::PoseGraph () : impl_(new PoseGraphImpl()) {}

PoseGraph::PoseGraph (const PoseGraph& g) : impl_(new PoseGraphImpl(*g.impl_)) {}

PoseGraph& PoseGraph::operator= (const PoseGraph& g) 
{
  if (this!=&g)
    *impl_ = *(g.impl_);
  return *this;
}

NodeId PoseGraph::addNode () { return impl_->addNode(); }

void PoseGraph::addNode (const NodeId id) { impl_->addNode(id); }

EdgeId PoseGraph::addEdge (const NodeId from, const NodeId to, const PoseConstraint& c)
{
  return impl_->addEdge (from, to, c);
}

void PoseGraph::addEdge (const NodeId from, const NodeId to, const PoseConstraint& c, const EdgeId e)
{
  impl_->addEdge (from, to, c, e);
}


void PoseGraph::setInitialPoseEstimate (const NodeId n, const Pose& pose)
{
  impl_->setInitialPoseEstimate(n, pose);
}

const Pose& PoseGraph::getInitialPoseEstimate (const NodeId n) const
{
  return impl_->getInitialPoseEstimate(n);
}

set<NodeId> PoseGraph::allNodes () const { return impl_->allNodes(); }

set<EdgeId> PoseGraph::allEdges () const { return impl_->allEdges(); }

const PoseConstraint& PoseGraph::getConstraint (const EdgeId id) const 
{ 
  return impl_->getConstraint(id);
}

double PoseGraph::edgeLength (const EdgeId id) const { return impl_->edgeLength(id); }

set<EdgeId> PoseGraph::incidentEdges (const NodeId n) const { return impl_->incidentEdges(n); }

NodeIdSet PoseGraph::neighbors (const NodeId n) const { return impl_->neighbors(n); }

pair<NodeId, NodeId> PoseGraph::incidentNodes (const EdgeId e) const { return impl_->incidentNodes(e); }

bool PoseGraph::nodeIdExists (const NodeId id) const { return impl_->nodeIdExists(id); }

bool PoseGraph::edgeIdExists (const EdgeId id) const { return impl_->edgeIdExists(id); }

PoseGraph PoseGraph::subgraph (const set<NodeId>& nodes) const
{ return impl_->subgraph(nodes); }

ShortestPathResult PoseGraph::shortestPath (const NodeId src, const NodeId dest) const
{
  return impl_->shortestPath (src, dest);
}

set<NodeId> PoseGraph::nearbyNodes (const NodeId src, const double radius) const
{
  return impl_->nearbyNodes (src, radius);
}

void PoseGraph::attachCloud (const NodeId id, LocalizedCloud::ConstPtr cloud)
{
  impl_->attachCloud(id, cloud);
}

LocalizedCloud::ConstPtr PoseGraph::getCloud (const NodeId id) const
{
  return impl_->getCloud(id);
}

bool PoseGraph::hasCloud (const NodeId id) const
{
  return impl_->hasCloud (id);
}

void PoseGraph::attachScan (const NodeId id, LaserScan::ConstPtr scan)
{
  impl_->attachScan(id, scan);
}

LaserScan::ConstPtr PoseGraph::getScan (const NodeId id) const
{
  return impl_->getScan(id);
}

bool PoseGraph::hasScan (const NodeId id) const
{
  return impl_->hasScan(id);
}

string PoseGraph::nodeFrameName (const NodeId id) 
{
  return string("node") + boost::lexical_cast<string>(id);
}

Pose PoseGraph::relativePose (const NodeId n, const NodeId ref) const
{
  return impl_->relativePose(n, ref);
}

NodeId PoseGraph::otherNode (const NodeId n, const EdgeId e) const
{
  return impl_->otherNode(n, e);
}

} // namespace pose_graph