graph_mapper.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.h>
#include <graph_slam/global_map.h>
#include <graph_slam/constraint_generator.h>
#include <pose_graph/utils.h>
#include <graph_slam/scan_match_localization.h>
#include <pose_graph/spa_conversion.h>
#include <pose_graph/spa_2d_conversion.h>
#include <pose_graph/transforms.h>
#include <pose_graph/pose_graph_message.h>
#include <pose_graph/Edge.h>
#include <pose_graph/exception.h>
#include <graph_slam/diff_publisher.h>
#include <pose_graph/SaveGraph.h>
#include <graph_slam/GenerateGlobalMap.h>
#include <occupancy_grid_utils/ray_tracer.h>
#include <ros/ros.h>
#include <ros/time.h>
#include <rosbag/view.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/LaserScan.h>
#include <graph_slam/GraphLocalization.h>
#include <laser_geometry/laser_geometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <boost/thread.hpp>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/optional.hpp>
#include <string>
#include <algorithm>
#include <std_msgs/Int64.h>
#include <std_msgs/UInt64.h>
#include <queue>


namespace graph_slam
{

namespace gu=occupancy_grid_utils;
namespace sm=sensor_msgs;
namespace gm=geometry_msgs;
namespace nm=nav_msgs;
namespace vm=visualization_msgs;
namespace gs=graph_slam;

using namespace pose_graph;


using boost::format;
using XmlRpc::XmlRpcValue;
using std::string;
using std::map;
using Eigen3::Vector3d;
using Eigen3::Quaterniond;
using Eigen3::Affine3d;
using Eigen3::Translation3d;
using ros::NodeHandle;
using ros::WallTimerEvent;
using ros::WallDuration;
using ros::Duration;
using ros::Time;
using std_msgs::Int64;
using std_msgs::UInt64;
using boost::optional;
using gm::Pose;
using gm::PoseStamped;
using gu::LocalizedCloud;

typedef boost::mutex::scoped_lock Lock;
typedef set<NodeId> NodeSet;
typedef boost::shared_ptr<nm::OccupancyGrid> GridPtr;
typedef boost::shared_ptr<nm::OccupancyGrid const> GridConstPtr;

/************************************************************
 * Constants
 ************************************************************/

const double DEFAULT_POS_THRESHOLD = 2.0;
const double DEFAULT_ANGLE_THRESHOLD = 1.57;
const double STATUS_CHECK_INT = 5.0;

/************************************************************
 * Class that holds node state
 ************************************************************/

class GraphMapper
{
public:

  GraphMapper();

private:

  /****************************************
   * Ops
   ****************************************/
  
  // Initialize state from a saved graph
  void initializeFromFile (const string& fname);

  // Set up the constraint generators from the parameter server
  void setupConstraintGenerators () ;

  // Add a new node if necessary
  void updateGraph (const WallTimerEvent& e);

  // Update the grid node data structures based on last_node_
  // Specifically, grid_node_to_nodes_, node_to_grid_node_, and current_grid_node_
  void updateGrid ();

  // Update last_fixed_frame_pose_ to fixed-frame pose at time t
  void updateFixedFramePose (const Time& t);

  // Actually add a node to graph and attach last scan
  void addNewNode ();

  // Check if two poses are far enough apart to be separate nodes
  bool sufficientlyFarApart (const Pose& p1, const Pose& p2) const;

  // Save the last laser scan
  void scanCallback (const sm::LaserScan::ConstPtr& scan);

  // Visualize grid containing a particular node
  void visualizeGridCallback (const UInt64& n);

  // Service callback for saving graph 
  bool saveGraph (SaveGraph::Request& req, SaveGraph::Response& resp);

  // Service for getting global grid
  bool generateMap (GenerateGlobalMap::Request& req, GenerateGlobalMap::Response& resp);

  // Publish visualization
  void visualize (const WallTimerEvent& e);

  // Repeatedly optimize poses
  void optimizePosesTimed (const WallTimerEvent& e);

  // Use graph optimizer to improve pose estimates.  Requires holding lock.
  void optimizePoses ();

  // Publish the current graph
  void publishLocalization (const WallTimerEvent& e);

  // Get the most recent fixed frame pose.
  PoseStamped currentFixedFramePose () const;

  // Periodically check that the node state looks ok
  void statusCheck (const WallTimerEvent& e);

  // Figure out which grid node corresponds to a given node
  optional<NodeId> getGridNode (const NodeId n) const;  

  // Tell the constraint generators that we switched to an existing node
  // Assumes lock is held
  void updateConstraintLocalizations ();

  /****************************************
   * Parameters
   ****************************************/

  // The frame in which scans come in
  const string sensor_frame_;

  // The base frame of the robot
  const string base_frame_;

  // A locally fixed frame (typically, odometry)
  const string fixed_frame_;

  // Min displacement (in meters) before new graph node created
  const double pos_threshold_;

  // Min angular displacement (in radians) before new graph node created
  const double angle_threshold_;

  // Whether to do optimization
  const bool optimize_;

  // Whether to use 3d optimizer
  const bool use_spa_3d_;

  // The side length in meters of the local grids 
  const double local_grid_size_;

  // The resolution of the local grids
  const double local_grid_resolution_;

  /****************************************
   * Local state; access locked by mutex_
   ****************************************/

  // Is graph initialized.  Only matters when reading a graph on startup.
  bool initialized_;

  // Pointer to last scan
  LaserPtr last_scan_;

  // Last known fixed frame pose
  optional<PoseStamped> last_fixed_frame_pose_;

  // Maps from node to fixed frame pose at that node
  NodePoseMap node_fixed_frame_poses_;

  // Last added node's id
  optional<NodeId> last_node_;

  // The graph that's built up over time
  PoseGraph graph_;

  // This holds the results of optimization
  // At any point, its keys will be a subset (since new nodes may have been added) of the nodes in graph_
  NodePoseMap optimized_node_poses_;

  // Maps each node to the associated grid node
  map<NodeId, NodeId> node_to_grid_node_;
  
  // Maps each grid node to the set of nodes whose clouds are overlaid to form the grid
  map<NodeId, NodeSet> grid_node_to_nodes_;

  // Current grid node (if there is one)
  optional<NodeId> current_grid_node_;

  // Localization state
  optional<LocalizationState> localization_;

  // Queue of unpublished diffs to the graph
  std::queue<PoseGraphDiff> diffs_;

  // Node whose grid we visualize next
  optional<NodeId> node_to_visualize_;

  // Unique id of graphs/diffs we publish
  unsigned current_graph_id_;

  /****************************************
   * Associated objects
   ****************************************/

  mutable boost::mutex mutex_; 
  NodeHandle nh_;
  tf::TransformListener tf_;
  ros::WallTimer update_timer_, visualization_timer_, optimization_timer_, graph_publication_timer_, 
    status_check_timer_;
  ros::Subscriber scan_sub_, visualize_grid_sub_;
  ros::Publisher vis_marker_pub_, marker_array_pub_, vis_scan_pub_, current_frame_pub_, local_grid_pub_;
  ros::ServiceServer graph_save_srv_, generate_map_srv_;
  tf::TransformBroadcaster tf_broadcaster_;
  vector<ConstraintGenPtr> constraint_generators_;
  boost::shared_ptr<ScanMatchLocalization> localizer_;
  DiffPublisher diff_pub_;
  

};


/************************************************************
 * Utilities
 ***********************************************************/

unsigned getUniqueId () 
{
  static unsigned id=0;
  return id++;
}
  

/************************************************************
 * Initialization
 ************************************************************/

template <class T>
T getPrivateParam (const string& name, const T& default_val)
{
  T val;
  NodeHandle nh("~");
  nh.param(name, val, default_val);
  ROS_DEBUG_STREAM_NAMED ("init", "Set param " << name << " to value " 
                          << val << " (default was " << default_val << ")");
  return val;
}


void GraphMapper::setupConstraintGenerators () 
{
  NodeHandle private_nh("~");
  XmlRpcValue constraint_sources;
  if (!private_nh.getParam("constraint_sources", constraint_sources))
    throw NonexistentParamException("~constraint_sources");
  const XmlRpcValue::Type type = constraint_sources.getType();
  if (type != XmlRpcValue::TypeArray)
    throw PoseGraphException(format("~constraint_sources parameter was not an array (type was %1%)") % type);
  for (int i=0; i<constraint_sources.size(); i++) {
    XmlRpcValue source = constraint_sources[i];
    const XmlRpcValue::Type type = source.getType();
    if (type != XmlRpcValue::TypeString)
      throw PoseGraphException(format("~constraint_sources[%1%] was not a string (type was %1%)") % type);

    const NodeHandle source_nh(private_nh, (string) source);
    ROS_DEBUG_STREAM_NAMED ("init", "Initializing constraint " << (string) source);
    constraint_generators_.push_back(createConstraintGenerator(source_nh));
  }
}

  
void GraphMapper::initializeFromFile (const string& fname)
{
  Lock lock(mutex_);
  ROS_DEBUG_STREAM_NAMED ("init", "Reading graph from " << fname);
  graph_ = readFromFile(fname);
  const NodeSet nodes = graph_.allNodes();
  BOOST_FOREACH (const NodeId n, nodes) {
    node_fixed_frame_poses_[n] = graph_.getInitialPoseEstimate(n);
    ROS_ASSERT_MSG (graph_.hasScan(n), "Graph doesn't have scan for node %lu", n.getId());
    localizer_->addNode(n, graph_.getScan(n));
  }
  // Hack: rather than localizing, we just assume we're at node 1
  const NodeId n1 = *nodes.begin();
  last_node_ = n1;
  vector<NodeId> seq;
  seq.push_back(n1);
  BOOST_FOREACH (ConstraintGenPtr gen, constraint_generators_) {
    gen->initializeFromGraph(graph_, seq);
  }  
  ROS_DEBUG_STREAM_NAMED ("init", "Read graph with " << nodes.size() << " nodes; assuming we're at " << n1);
  optimizePoses();
  localization_ = LocalizationState(n1, poseToWorldTransform(node_fixed_frame_poses_[n1]),
                                    convertToPose(makePose2D(0, 0, 0)), ros::Time::now());
  while (!last_fixed_frame_pose_)
    updateFixedFramePose(ros::Time::now());
  updateConstraintLocalizations();
  diff_pub_.setGraph(graph_);
}

void GraphMapper::updateConstraintLocalizations ()
{
  gs::GraphLocalization loc;
  loc.node = localization_->ref_node.getId();
  loc.offset = localization_->node_frame_pose;
  loc.fixed_frame_pose = last_fixed_frame_pose_->pose;
  
  BOOST_FOREACH (ConstraintGenPtr gen, constraint_generators_) 
    gen->updateLocalization(loc);
}

double getLocalizationRate ()
{
  return getPrivateParam("localization_pub_rate", 5.0);
}

unsigned getLocalizationsPerScanMatch ()
{
  unsigned l = ceil(getLocalizationRate()/getPrivateParam("localization_scan_match_rate", 5.0));
  ROS_DEBUG_STREAM_NAMED ("init", "We'll do scan matching every " << l << " localizations");
  return l;
}




GraphMapper::GraphMapper () :
  sensor_frame_(getPrivateParam<string>("sensor_frame", "base_laser_link")),
  base_frame_(getPrivateParam<string>("base_frame", "base_footprint")),
  fixed_frame_(getPrivateParam<string>("fixed_frame", "odom")),
  pos_threshold_(getPrivateParam<double>("position_displacement_threshold", DEFAULT_POS_THRESHOLD)),
  angle_threshold_(getPrivateParam<double>("angle_displacement_threshold", DEFAULT_ANGLE_THRESHOLD)),
  optimize_(getPrivateParam<bool>("optimize", true)),
  use_spa_3d_(getPrivateParam<bool>("use_spa_3d", true)),
  local_grid_size_(getPrivateParam("local_grid_size", 20.0)),
  local_grid_resolution_(getPrivateParam("local_grid_resolution", 0.05)),
  initialized_(false), update_timer_(nh_.createWallTimer(WallDuration(0.5), &GraphMapper::updateGraph, this)),
  visualization_timer_(nh_.createWallTimer(WallDuration(2.0), &GraphMapper::visualize, this)),
  optimization_timer_(nh_.createWallTimer(WallDuration(1.0), &GraphMapper::optimizePosesTimed, this)),
  graph_publication_timer_(nh_.createWallTimer(WallDuration(1/getLocalizationRate()), &GraphMapper::publishLocalization, this)),
  status_check_timer_(nh_.createWallTimer(WallDuration(STATUS_CHECK_INT), &GraphMapper::statusCheck, this)),
  scan_sub_(nh_.subscribe("scan", 1, &GraphMapper::scanCallback, this)),
  visualize_grid_sub_(nh_.subscribe("visualize_grid", 10, &GraphMapper::visualizeGridCallback, this)),
  vis_marker_pub_(nh_.advertise<vm::Marker> ("visualization_marker", 10)),
  marker_array_pub_(nh_.advertise<vm::MarkerArray> ("visualization_marker_array", 1000)),
  vis_scan_pub_(nh_.advertise<sm::PointCloud> ("pose_graph_scans", 100)),
  current_frame_pub_(nh_.advertise<Int64> ("current_node", 10)),
  local_grid_pub_(nh_.advertise<nm::OccupancyGrid> ("graph_mapper_local_grid", 10)),
  graph_save_srv_(nh_.advertiseService("save_graph", &GraphMapper::saveGraph, this)),
  generate_map_srv_(nh_.advertiseService("generate_map", &GraphMapper::generateMap, this)),
  diff_pub_(nh_)
{
  // Get the laser's pose, relative to base.
  tf::Stamped<tf::Pose> ident;
  tf::Stamped<tf::Transform> laser_to_base;
  ident.setIdentity();
  ident.frame_id_ = sensor_frame_;
  ident.stamp_ = ros::Time();

  {
    bool received = false;
    while (!received) { // 
      if (!tf_.waitForTransform(base_frame_, ident.frame_id_, ros::Time::now(), ros::Duration(10.0))) {
        ROS_WARN_STREAM ("Still waiting for transformn betwee " << sensor_frame_ << " and " << base_frame_ <<
                         " during graph mapper initialization");
        ros::WallDuration(1.0).sleep();
      }
      else
        received = true;
    }
  }
  
  tf_.transformPose(base_frame_, ident, laser_to_base);
  gm::Pose2D laser_pose;
  laser_pose.x = laser_to_base.getOrigin().x();
  laser_pose.y = laser_to_base.getOrigin().y();
  laser_pose.theta = tf::getYaw(laser_to_base.getRotation());
  localizer_.reset(new ScanMatchLocalization(laser_pose, 5.0, getLocalizationsPerScanMatch()));
  
  string input_file = getPrivateParam("input_file", string());
  setupConstraintGenerators();
  if (!input_file.empty())
    initializeFromFile (input_file);
  initialized_ = true;
}

/************************************************************
 * Callbacks
 ************************************************************/

// Store the most recent laser scan
void GraphMapper::scanCallback (const LaserPtr& scan)
{
  Lock lock(mutex_);
  ROS_DEBUG_COND_NAMED (!last_scan_, "pose_graph_node", "First scan received");
  last_scan_ = scan;
  updateFixedFramePose(scan->header.stamp);
}

// Visualize the local grid for a particular node
void GraphMapper::visualizeGridCallback (const UInt64& n) 
{
  node_to_visualize_ = NodeId(n.data);
}

bool GraphMapper::saveGraph (SaveGraph::Request& req, SaveGraph::Response& resp)
{
  if (initialized_) {
    Lock lock(mutex_);
    rosbag::Bag bag(req.filename, rosbag::bagmode::Write);
    bag.write("graph", ros::Time::now(), poseGraphToRos(graph_));
    resp.succeeded = true;
  }
  else {
    ROS_WARN ("Can't save graph yet as it's not initialized");
    resp.succeeded = false;
  }
  return true;
}

bool GraphMapper::generateMap (GenerateGlobalMap::Request& req, GenerateGlobalMap::Response& resp)
{
  boost::shared_ptr<PoseGraph> copy;
  NodePoseMap opt_poses;
  
  // While holding lock, make copies of relevant objects
  {
    Lock lock(mutex_);
    copy.reset(new PoseGraph(graph_));
    opt_poses = optimized_node_poses_;
  }

  if (initialized_ && !opt_poses.empty()) {
    resp.map = *generateGlobalMap(*copy, opt_poses, req.resolution);
    resp.succeeded = true;
    local_grid_pub_.publish(resp.map);
  }
  else {
    ROS_WARN ("Can't generate global map as not yet fully initialized");
    resp.succeeded = false;
  }

  return true;
}

             
/************************************************************
 * Thread 1: periodically add a new node to graph
 * if we're far enough away from the last one, and connect
 * it to previous nodes by calling the constraint generators
 ************************************************************/

void GraphMapper::updateGraph (const WallTimerEvent& e)
{
  Pose id;
  id.orientation.w = 1.0;
  Lock lock(mutex_);
  if (!initialized_ || !last_scan_)
    return; // statusCheck will periodically print a warning in this case
  if (!last_fixed_frame_pose_)
    return; // A warning will have been printed in update
  else if (Time::now() - last_fixed_frame_pose_->header.stamp > Duration(0.2))
    ROS_WARN_STREAM ("Fixed frame pose " << *last_fixed_frame_pose_ << " out of date.  Skipping graph update.");

  if (!localization_ || sufficientlyFarApart(id, localization_->node_frame_pose)) {

    optimizePoses();
    addNewNode();

    // Also create the diff object to publish
    PoseGraphDiff diff;
    diff.id = current_graph_id_++;
    if (graph_.hasCloud(*last_node_)) {
      diff.cloud_ids.push_back(last_node_->getId());
      diff.clouds.push_back(*graph_.getCloud(*last_node_));
    }
    Node diff_node;
    diff_node.id = last_node_->getId();
    diff_node.initial_pose_estimate = graph_.getInitialPoseEstimate(*last_node_);
    diff.new_nodes.push_back(diff_node);
    
    BOOST_FOREACH (ConstraintGenPtr gen, constraint_generators_) {
      NodeConstraintVector constraints = gen->getConstraints(graph_, *last_node_, optimized_node_poses_);
      BOOST_FOREACH (const NodeConstraint& c, constraints) {
        const EdgeId e = graph_.addEdge(c.target, *last_node_, c.constraint);
        
        // Update the diff
        Edge diff_edge;
        diff_edge.src = c.target.getId();
        diff_edge.dest = last_node_->getId();
        diff_edge.constraint = constraintToRos(c.constraint);
        diff_edge.id = e.getId();
        diff.new_edges.push_back(diff_edge);
      }
    }
    diff_pub_.addDiff(&diff);

    // Update grid for visualization
    updateGrid();
  }
}

// Update the variable last_fixed_frame_pose_ to hold the
// fixed frame pose at time t
// Caller must hold a lock on mutex_
void GraphMapper::updateFixedFramePose (const Time& t)
{
  if (tf_.waitForTransform(fixed_frame_, base_frame_, t, ros::Duration(1.0))) {
    PoseStamped p;
    p.pose.position.x = p.pose.position.y = p.pose.position.z = 0.0;
    p.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
    p.header.frame_id = base_frame_;
    p.header.stamp = t;
    tf::Stamped<tf::Pose> base_pose, fixed_pose;
    tf::poseStampedMsgToTF(p, base_pose);

    tf_.transformPose(fixed_frame_, base_pose, fixed_pose);

    gm::PoseStamped fixed_pose_msg;
    tf::poseStampedTFToMsg(fixed_pose, fixed_pose_msg);
    last_fixed_frame_pose_ = fixed_pose_msg;
    
    ROS_DEBUG_STREAM_NAMED ("pose_graph_update_fixed_frame_pose", "Updated fixed frame pose to "
                            << toString(last_fixed_frame_pose_->pose) << " with stamp "
                            << last_fixed_frame_pose_->header.stamp.toSec());
                                      
  }
  else {
    ROS_WARN_STREAM ("Skipping update of fixed frame pose due to lack of transform from " << 
                     base_frame_ << " to " << fixed_frame_);
  }
}


// Are two poses sufficiently far apart that a new node should be added to the graph?
bool GraphMapper::sufficientlyFarApart (const Pose& p1, const Pose& p2) const
{
  const double dx = p1.position.x - p2.position.x;
  const double dy = p1.position.y - p2.position.y;
  const double dtheta = abs(tf::getYaw(p1.orientation) - tf::getYaw(p2.orientation));
  return (dx*dx + dy*dy > pos_threshold_*pos_threshold_) || (dtheta > angle_threshold_);
}

// Add a new node at the current position in the graph
void GraphMapper::addNewNode ()
{
  ROS_ASSERT (last_scan_ && last_fixed_frame_pose_);
  ROS_ASSERT_MSG (last_scan_->header.frame_id == sensor_frame_, 
                  "Code currently assumes scans are in default sensor frame");

  /****************************************
   * 1. Add the node itself
   ****************************************/

  NodeId n = graph_.addNode();
  graph_.attachScan(n, last_scan_);
  graph_.setInitialPoseEstimate(n, last_fixed_frame_pose_->pose);
  node_fixed_frame_poses_[n] = last_fixed_frame_pose_->pose;
  ros::Time node_stamp = last_fixed_frame_pose_->header.stamp;
  ROS_DEBUG_NAMED ("pose_graph_node_update", 
                   "Added node %lu with stamp %.2f, at [%.2f, %.2f, %.2f].",
                   n.getId(), node_stamp.toSec(), last_fixed_frame_pose_->pose.position.x, 
                   last_fixed_frame_pose_->pose.position.y, 
                   tf::getYaw(last_fixed_frame_pose_->pose.orientation));
  last_node_ = n;
  

  /****************************************
   * 2. Store the projected and transformed
   * laser scan
   ****************************************/

  // We want to store the entire cloud with respect to the current sensor frame
  // Unfortunately, transformLaserScanToPointCloud assumes the target frame is fixed
  // So we first project to a (more or less) fixed frame, then transform to the 
  // sensor frame at a particular time point

  try {
    const ros::Time start = last_scan_->header.stamp;
    const ros::Time end = start + ros::Duration(last_scan_->ranges.size()*last_scan_->time_increment);
    const ros::Duration wait(2.0);

    if (!tf_.waitForTransform(fixed_frame_, sensor_frame_, start, wait)) {
      ROS_WARN_STREAM ("Timed out waiting for transform from sensor to fixed frame at start time " <<
                       start << " of laser scan");
    }
    else if (!tf_.waitForTransform(fixed_frame_, sensor_frame_, end, ros::Duration(2.0))) {
      ROS_WARN_STREAM ("Timed out waiting for transform from sensor to fixed frame at end time " <<
                       end << " of laser scan");
    }

    // We think we can transform the cloud to the fixed frame 
    else {

      sm::PointCloud fixed_frame_cloud;
      laser_geometry::LaserProjection projector_;
      projector_.transformLaserScanToPointCloud (fixed_frame_, *last_scan_, fixed_frame_cloud, tf_);

      if (!tf_.waitForTransform(sensor_frame_, fixed_frame_, node_stamp, wait)) {
        ROS_WARN_STREAM ("Timed out waiting for transform from " << fixed_frame_ << " to " << sensor_frame_
                         << " at " << node_stamp);
      }

      // We think we can transform the (now fixed) cloud from the fixed to the sensor frame
      else {

        LocalizedCloud::Ptr sensor_frame_cloud(new LocalizedCloud());
        tf_.transformPointCloud (sensor_frame_, node_stamp, fixed_frame_cloud, 
                                 fixed_frame_, sensor_frame_cloud->cloud);
        geometry_msgs::PoseStamped id, sensor_pose;
        id.header.stamp = ros::Time();
        id.header.frame_id = sensor_frame_;
        id.pose.orientation.w = 1.0; // id is now the identity pose
        tf_.transformPose(base_frame_, id, sensor_pose);
        sensor_frame_cloud->sensor_pose = sensor_pose;

        graph_.attachCloud(n, sensor_frame_cloud);
      }
    }
  }

  // Because the above has race conditions.  This will become unnecessary once we have tf2.
  catch (tf::TransformException& e) {
    ROS_WARN_STREAM ("Received tf exception '" << e.what() <<
                     "' when trying to transform scan to cloud.  Skipping");
  }

  /****************************************
   * 3. Set the localization
   ****************************************/
  
  const Eigen3::Affine3d node_to_fixed_frame = poseToWorldTransform(node_fixed_frame_poses_[n]);
  localizer_->addNode(n, last_scan_);
  Pose identity;
  identity.orientation.w = 1.0;
  localization_ = LocalizationState(n, node_to_fixed_frame, identity, last_scan_->header.stamp);
  // In this case we don't have to update the constraint generators since we will call getconstraints
  // \todo Really needs to be made simpler

  ROS_DEBUG_STREAM_NAMED ("localization_switch", "Switching to newly added node " << n);

}


bool withinDistance (const PoseGraph& g, const NodeId n1, const NodeId n2, const double d)
{
  ShortestPathResult res=g.shortestPath(n1, n2);
  return res.path_found && res.cost < d;
}


void GraphMapper::updateGrid ()
{  
  // Start up a new grid if we're too far from the current one
  if (!(current_grid_node_ && 
        withinDistance(graph_, *current_grid_node_, *last_node_, local_grid_size_/3))) {
    current_grid_node_ = *last_node_;
    grid_node_to_nodes_[*last_node_] = NodeSet();
  }

  // Add this node to the current grid
  node_to_grid_node_[*last_node_] = *current_grid_node_;
  grid_node_to_nodes_[*current_grid_node_].insert(*last_node_);
}


/************************************************************
 * Thread 2: periodically optimize the entire
 * graph using sparse pose adjustment
 ************************************************************/


void GraphMapper::optimizePosesTimed (const WallTimerEvent& e)
{
  if (initialized_) {
    Lock lock(mutex_);
    optimizePoses();
  }
}

void GraphMapper::optimizePoses ()
{
  if (optimize_) {
    optimized_node_poses_ =
      use_spa_3d_ ?
      optimizeGraph(graph_) :
      optimizeGraph2D(graph_);
  }
  else
    optimized_node_poses_ = node_fixed_frame_poses_;
}



/************************************************************
 * Thread 3: publish visualization
 ************************************************************/


// Say you have a node at pose p and an outgoing edge labelled with constraint c.
// This returns the expected position of the node on the other side of the edge.
Pose applyConstraint (const Pose& p, const PoseConstraint& c)
{
  const Affine3d trans = poseToWorldTransform(p);
  const Pose rel_pose = getPose(c);
  return applyTransform(trans, rel_pose);
}


void addTransformedPoints (LocalizedCloud::ConstPtr cloud, const Pose& pose, sm::PointCloud* cloud_out)
{
  const Affine3d base_to_world = poseToWorldTransform(pose);
  const Affine3d sensor_to_base = poseToWorldTransform(cloud->sensor_pose.pose);
  const Affine3d sensor_to_world = base_to_world*sensor_to_base;

  BOOST_FOREACH (const gm::Point32 pt, cloud->cloud.points) 
    cloud_out->points.push_back(transformPoint(sensor_to_world, pt));
}


// Visualize graph and scans
void GraphMapper::visualize (const WallTimerEvent& e) 
{
  if (!initialized_)
    return;
  
  const unsigned NODE_MARKER_ID_BASE = 1000;
  
  Lock lock(mutex_);
  vm::Marker edges, loc;
  vm::Marker node_prototype;
  vm::MarkerArray nodes;
  sm::PointCloud cloud;

  unsigned next_ind=0;
  edges.header.frame_id = node_prototype.header.frame_id = loc.header.frame_id = OPTIMIZED_FRAME;
  node_prototype.id = -42;
  edges.id = next_ind++;
  loc.id = next_ind++;
  node_prototype.header.stamp = edges.header.stamp = loc.header.stamp = Time::now();
  edges.ns = node_prototype.ns = loc.ns = "pose_graph";
  node_prototype.type = vm::Marker::SPHERE;
  edges.type = vm::Marker::LINE_LIST;
  loc.type = vm::Marker::ARROW;
  edges.action = node_prototype.action = loc.action = vm::Marker::ADD;
  edges.scale.x = 0.05;
  node_prototype.scale.x = 0.1;
  node_prototype.scale.y = 0.1;
  node_prototype.scale.z = 0.1;
  node_prototype.pose.orientation.w = 1.0;
  loc.scale.x = 0.1;
  loc.scale.y = 0.2;
  edges.color.b = node_prototype.color.r = 1.0;
  loc.color.r = .64;
  loc.color.g = .1;
  loc.color.b = .9;
  edges.color.a = loc.color.a = node_prototype.color.a = 1.0;


  if (optimized_node_poses_.size() > 0) {


    BOOST_FOREACH (const NodePoseMap::value_type& entry, optimized_node_poses_) {

      // Publish sphere at node position
      const NodeId n = entry.first;
      const Pose pose = entry.second;
      vm::Marker node = node_prototype;
      const size_t node_ind = nodes.markers.size();
      nodes.markers.resize(node_ind+1);
      nodes.markers[node_ind] = node_prototype;
      nodes.markers[node_ind].pose.position = pose.position;
      nodes.markers[node_ind].id = n.getId() + NODE_MARKER_ID_BASE;

      // Add scan at node to point cloud
      if (graph_.hasCloud(n))
        addTransformedPoints(graph_.getCloud(n), pose, &cloud);
      // This loops over the constraints, and for each one that is rooted at the current node, draws a line
      BOOST_FOREACH (const EdgeId e, graph_.incidentEdges(n)) {
        if (graph_.incidentNodes(e).first == n) {
          edges.points.push_back(pose.position);
          const Pose pose2 = applyConstraint(entry.second, graph_.getConstraint(e));
          edges.points.push_back(pose2.position);
        }
      }
    }


    // Localization
    if (localization_) {
      NodePoseMap::const_iterator iter=optimized_node_poses_.find(localization_->ref_node);
      if (iter!=optimized_node_poses_.end()) {
        loc.points.push_back(iter->second.position);
        Eigen3::Affine3d node_to_opt = poseToWorldTransform(iter->second);
        loc.points.push_back(applyTransform(node_to_opt, localization_->node_frame_pose).position);
      }
    }
    else
      loc.action = vm::Marker::DELETE;
  }

  marker_array_pub_.publish(nodes);
  vis_marker_pub_.publish(edges);
  vis_marker_pub_.publish(loc);

  cloud.header.frame_id = OPTIMIZED_FRAME;
  cloud.header.stamp = ros::Time::now();
  vis_scan_pub_.publish(cloud);

}

optional<NodeId> GraphMapper::getGridNode (const NodeId n) const
{
  optional<NodeId> g;
  typedef map<NodeId, set<NodeId> > GridMap;
  BOOST_FOREACH (const GridMap::value_type& e, grid_node_to_nodes_) {
    if (contains(e.second, n)) {
      g = e.first;
      break;
    }
  }
  return g;
}




/************************************************************
 * Thread 4: publish localization and tf frames
 ************************************************************/

void TransformEigenToTF(const Eigen3::Affine3d &k, tf::Transform &t)
{
  t.setOrigin(tf::Vector3(k.matrix()(0,3), k.matrix()(1,3), k.matrix()(2,3)));
  t.setBasis(btMatrix3x3(k.matrix()(0,0), k.matrix()(0,1),k.matrix()(0,2),k.matrix()(1,0), k.matrix()(1,1),k.matrix()(1,2),k.matrix()(2,0), k.matrix()(2,1),k.matrix()(2,2)));
}

void GraphMapper::publishLocalization (const WallTimerEvent& e)
{
  if (!initialized_)
    return;
  Lock lock(mutex_);

  if (localization_ && last_fixed_frame_pose_) {
    // Make sure ref node has an optimized pose.  Ok since we hold the lock here.
    if (!contains(optimized_node_poses_, localization_->ref_node))
      optimizePoses();

    const NodeId old_ref_node = localization_->ref_node;
    localization_ = localizer_->nextState(*localization_, last_fixed_frame_pose_->pose,
                                             last_scan_, graph_, optimized_node_poses_);
    // \todo This is overly complicated and error-prone.  The interface to constraint generators really needs to be simplified.
    if (old_ref_node != localization_->ref_node) 
      updateConstraintLocalizations();
      
      
    ROS_DEBUG_STREAM_NAMED ("localization", "Localization is now " << toString2D(localization_->node_frame_pose)
                            << " w.r.t. " << localization_->ref_node);
    
    // Publish the identity of the current node
    Int64 frame_msg;
    frame_msg.data = localization_->ref_node.getId();
    current_frame_pub_.publish(frame_msg);

    std::vector<tf::StampedTransform> transforms;
    // Publish transform from optimized graph to fixed frame such that the odometric pose and
    // the node-frame pose of the current localization agree
    {
      tf::Transform opt_to_fixed;
      Affine3d opt_to_node(poseToWorldTransform(keyValue(optimized_node_poses_, localization_->ref_node)).inverse());
      TransformEigenToTF(localization_->node_to_fixed_frame*opt_to_node, opt_to_fixed);
      tf::StampedTransform stamped(opt_to_fixed, localization_->stamp, fixed_frame_, OPTIMIZED_FRAME);
      transforms.push_back(stamped);
    }

    // Publish optimized node poses
    const ros::Time now = ros::Time::now();
    BOOST_FOREACH (const NodePoseMap::value_type& e, optimized_node_poses_) {
      tf::Transform pose_to_opt;
      tf::poseMsgToTF(e.second, pose_to_opt);
      tf::StampedTransform stamped(pose_to_opt, now, OPTIMIZED_FRAME, PoseGraph::nodeFrameName(e.first));
      transforms.push_back(stamped);
    }

    
    tf_broadcaster_.sendTransform(transforms);
  }
}


/************************************************************
 * Thread 5: Occasionally check that everything looks ok
 ************************************************************/

void GraphMapper::statusCheck (const WallTimerEvent& e)
{
  static unsigned count=0;
  if (count++) {
    if (!last_node_ || !localization_ || !initialized_) {
      ROS_WARN_COND_NAMED (!last_node_, "graph_mapper_status_check", "No nodes added yet.");
      ROS_WARN_COND_NAMED (!localization_ && last_node_, "graph_mapper_status_check", "No localization");
      ROS_WARN_STREAM_NAMED ("graph_mapper_status_check", "scan_received = " << (last_scan_.get()!=0)
                             << ", fixed_frame_pose_valid_ = " << (bool) last_fixed_frame_pose_ <<
                             ", initialized_ = " << initialized_);
    }
  }
}

} // namespace graph_slam





int main (int argc, char** argv)
{
  ros::init(argc, argv, "graph_mapper");
  ros::AsyncSpinner spinner(3);
  spinner.start();
  graph_slam::GraphMapper node;
  ROS_INFO ("Initialization complete");
  ros::waitForShutdown();
  return 0;
}