pose_graph_message.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_message.h>
#include <pose_graph/exception.h>
#include <pose_graph/utils.h>
#include <rosbag/view.h>
#include <ros/time.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/thread.hpp>

#define foreach BOOST_FOREACH

namespace pose_graph
{

using boost::tie;
using std::string;
using boost::mutex;
using Eigen3::Quaterniond;
using Eigen3::Vector3d;
using occupancy_grid_utils::LocalizedCloud;
using std::map;
using std::vector;
using sensor_msgs::LaserScan;

const string DUMMY_TOPIC("dummy");

/************************************************************
 * Converting between PoseGraph and PoseGraphMessage
 ************************************************************/

// Uses the assumption that tf::Pose and tf::Transform are the same type
// (will not compile otherwise)
PoseWithPrecision constraintToRos (const PoseConstraint& constraint)
{
  PoseWithPrecision m;
  m.pose.position.x = constraint.translation.x();
  m.pose.position.y = constraint.translation.y();
  m.pose.position.z = constraint.translation.z();

  const Quaterniond::Coefficients& coeffs = constraint.rotation.coeffs();
  m.pose.orientation.x = coeffs.x();
  m.pose.orientation.y = coeffs.y();
  m.pose.orientation.z = coeffs.z();
  m.pose.orientation.w = coeffs.w();

  unsigned ind=0;
  for (unsigned r=0; r<6; r++)
    for (unsigned c=0; c<6; c++) 
      m.precision[ind++] = constraint.precision(r,c);

  return m;
}


PoseGraphMessage poseGraphToRos (const PoseGraph& g)
{
  PoseGraphMessage m;

  foreach (const NodeId n, g.allNodes()) {
    Node node;
    node.id = n.getId();
    node.initial_pose_estimate = g.getInitialPoseEstimate(n);
    m.nodes.push_back(node);
    if (g.hasCloud(n)) {
      m.cloud_ids.push_back(n.getId());
      m.clouds.push_back(*g.getCloud(n));
    }
    if (g.hasScan(n)) {
      m.scan_ids.push_back(n.getId());
      m.scans.push_back(*g.getScan(n));
    }
  }

  foreach (const EdgeId e, g.allEdges()) {
    Edge edge;
    edge.id = e.getId();
    std::pair<NodeId, NodeId> nodes = g.incidentNodes(e);
    edge.src = nodes.first.getId();
    edge.dest = nodes.second.getId();
    edge.constraint = constraintToRos(g.getConstraint(e));
    m.edges.push_back(edge);
  }
  
  return m;
}


PoseConstraint constraintFromRos (const PoseWithPrecision& m)
{
  PoseConstraint constraint;
  constraint.translation = Vector3d(m.pose.position.x, m.pose.position.y, m.pose.position.z);
  constraint.rotation = Quaterniond(m.pose.orientation.w, m.pose.orientation.x, m.pose.orientation.y,
                                    m.pose.orientation.z);
  unsigned ind=0;
  for (unsigned r=0; r<6; r++)
    for (unsigned c=0; c<6; c++)
      constraint.precision(r,c) = m.precision[ind++];
  return constraint;
}


PoseGraph poseGraphFromRos (const PoseGraphMessage& m)
{
  PoseGraph g;
  
  foreach (const Node& n, m.nodes) {
    g.addNode(NodeId(n.id));
    g.setInitialPoseEstimate(NodeId(n.id), n.initial_pose_estimate);
  }

  foreach (const Edge& e, m.edges) 
    g.addEdge(NodeId(e.src), NodeId(e.dest), constraintFromRos(e.constraint), EdgeId(e.id));

  for (unsigned i=0; i<m.cloud_ids.size(); i++) {
    LocalizedCloud::ConstPtr cloud(new LocalizedCloud(m.clouds[i]));
    g.attachCloud(NodeId(m.cloud_ids[i]), cloud);
  }

  for (unsigned i=0; i<m.scan_ids.size(); i++) {
    LaserScan::ConstPtr scan(new LaserScan(m.scans[i]));
    g.attachScan(NodeId(m.scan_ids[i]), scan);
  }

  return g;
}

/************************************************************
 * Diffs
 ************************************************************/

void applyDiff (PoseGraph* graph, const PoseGraphDiff& msg)
{
  std::set<NodeId> new_nodes;
  BOOST_FOREACH (const Node n, msg.new_nodes) {
    graph->addNode(NodeId(n.id));
    new_nodes.insert(NodeId(n.id));
    graph->setInitialPoseEstimate(NodeId(n.id), n.initial_pose_estimate);
  }
  BOOST_FOREACH (const Edge& e, msg.new_edges)
    graph->addEdge(NodeId(e.src), NodeId(e.dest), constraintFromRos(e.constraint), EdgeId(e.id));
  const unsigned num_clouds = msg.cloud_ids.size();
  ROS_ASSERT (msg.clouds.size()==num_clouds);
  for (unsigned i=0; i<num_clouds; i++) {
    LocalizedCloud::ConstPtr cloud(new LocalizedCloud(msg.clouds[i]));
    graph->attachCloud(NodeId(msg.cloud_ids[i]), cloud);
  }
}

// Compose diffs. Later changes overwrite earlier ones.
PoseGraphDiff composeDiffs (const vector<PoseGraphDiff::ConstPtr>& diffs)
{
  PoseGraphDiff d;
  typedef map<unsigned, const Node*> NodeMap;
  typedef map<unsigned, const Edge*> EdgeMap;
  typedef map<unsigned, const LocalizedCloud*> CloudMap;

  NodeMap nodes;
  EdgeMap edges;
  CloudMap clouds;

  BOOST_FOREACH (PoseGraphDiff::ConstPtr diff, diffs) {
    BOOST_FOREACH (const Node& node, diff->new_nodes)
      nodes[node.id] = &node;
    BOOST_FOREACH (const Edge& edge, diff->new_edges)
      edges[edge.id] = &edge;
    unsigned i=0;
    BOOST_FOREACH (const LocalizedCloud& cloud, diff->clouds)
      clouds[diff->cloud_ids[i++]] = &cloud;
  }

  BOOST_FOREACH (const NodeMap::value_type& n, nodes)
    d.new_nodes.push_back(*n.second);
  BOOST_FOREACH (const EdgeMap::value_type& e, edges) 
    d.new_edges.push_back(*e.second);
  BOOST_FOREACH (const CloudMap::value_type& c, clouds) {
    d.cloud_ids.push_back(c.first);
    d.clouds.push_back(*c.second);
  }

  return d;
}

/************************************************************
 * File read/write using bags
 ***********************************************************/

PoseGraph readFromFile (const string& fname)
{
  rosbag::Bag bag(fname, rosbag::bagmode::Read);
  rosbag::View view(bag);
  unsigned num_entries = std::distance(view.begin(), view.end());
  ROS_ASSERT_MSG (num_entries == 1, "Input file must contain exactly one message; it had %u", num_entries);
  PoseGraphMessage::ConstPtr m = view.begin()->instantiate<PoseGraphMessage>();
  return poseGraphFromRos(*m);
}

} // namespace pose_graph