spa_2d_conversion.cpp

Go to the documentation of this file.

#include <pose_graph/spa_2d_conversion.h>
#include <pose_graph/transforms.h>
#include <pose_graph/exception.h>
#include <pose_graph/utils.h>
#include <tf/transform_datatypes.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <set>

namespace pose_graph
{

using std::set;
using boost::tie;
using sba::Node2d;
using sba::SysSPA2d;
using sba::Con2dP2;
using geometry_msgs::Pose;


typedef std::pair<NodeId, NodeId> NodePair;

// Make a spa node given an initial pose estimate
Node2d makeNode (const double x, const double y, const double theta)
{
  Node2d n;
  n.trans(0) = x;
  n.trans(1) = y;
  n.trans(2) = 1.0;
  n.arot = theta;
  n.setTransform();
  n.setDr();
  return n;
}


// Get yaw from an eigen quaternion
double getYaw (const Quaterniond& q)
{
  const tf::Quaternion qt(q.x(), q.y(), q.z(), q.w());
  return tf::getYaw(qt);
}

Con2dP2 makeSpa2DConstraint (const PoseConstraint& constraint)
{
  Con2dP2 spa;
  spa.tmean[0] = constraint.translation[0];
  spa.tmean[1] = constraint.translation[1];
  spa.amean = getYaw(constraint.rotation);

  spa.prec = Matrix3d::Zero();
  spa.prec(0,0) = constraint.precision(0,0);
  spa.prec(1,1) = constraint.precision(1,1);

  spa.prec(2,2) = constraint.precision(5,5)*4;

  return spa;
}


// Turn a pose graph into a pointer to a Spa2d
Spa2DConversionResult poseGraphToSpa2D (const PoseGraph& pose_graph)
{
  Spa2DPtr spa(new SysSPA2d());
  map<NodeId, unsigned> node_indices;
  unsigned next_index=0;

  // 1. Add the nodes
  BOOST_FOREACH (const NodeId n, pose_graph.allNodes()) {
    const Pose p = pose_graph.getInitialPoseEstimate(n);
    Eigen3::Vector3d pos(p.position.x, p.position.y, tf::getYaw(p.orientation));
    spa->addNode(pos, next_index);
    ROS_DEBUG_STREAM_NAMED ("pose_graph_spa", "Adding node " << next_index << " with mean " << pos);
    node_indices[n] = next_index++;
  }

  // 2. Add the edge constraints
  BOOST_FOREACH (const EdgeId e, pose_graph.allEdges()) {

    NodePair ids = pose_graph.incidentNodes(e);
    const pose_graph::PoseConstraint constraint = pose_graph.getConstraint(e);
    Eigen3::Vector3d mean(constraint.translation[0], constraint.translation[1], getYaw(constraint.rotation));
    Eigen3::Matrix3d prec = Eigen3::Matrix3d::Zero();

    // The 6x6 matrix constraint.precision is in order x,y,z of position and x,y,z of quaternion orientation. 
    // The line below would be correct for diagonal precision matrices, if theta = z/2, but in fact theta=sin(z/2)
    // so this isn't quite right for theta far from 0.  The precisions are heuristically set anyway though...
    prec(2, 2) = constraint.precision(5, 5)*4;
    prec(0, 0) = constraint.precision(0, 0);
    prec(1, 1) = constraint.precision(1, 1);

    const unsigned ind1 = node_indices[ids.first];
    const unsigned ind2 = node_indices[ids.second];
    spa->addConstraint(ind1, ind2, mean, prec);
    ROS_DEBUG_STREAM_NAMED ("pose_graph_spa", "Adding constraint from " << ind1 << " to " << ind2 <<
                            " with mean " << mean << " and precision " << prec);
  }

  
  return Spa2DConversionResult(spa, node_indices);
}


// Return the Pose corresponding to a spa node
Pose getNodePose (const sba::Node2d& n)
{
  Pose pose;

  pose.position.x = n.trans(0);
  pose.position.y = n.trans(1);
  pose.position.z = 0.0;
  
  pose.orientation = tf::createQuaternionMsgFromYaw(n.arot);

  return pose;
}


// Convert and optimize a graph
NodePoseMap optimizeGraph2D (const PoseGraph& g)
{
  /******************************
   * 1. Convert graph to SPA2D
   ******************************/

  ROS_DEBUG_NAMED ("pose_graph_spa", "Converting graph with %zu nodes to spa", g.allNodes().size());
  Spa2DConversionResult res=poseGraphToSpa2D(g);

  /******************************
   * 2. Optimize
   ******************************/

  const size_t num_nodes = res.spa->nodes.size();
  const size_t num_constraints = res.spa->p2cons.size();

  // Shouldn't need this check, but spa dies otherwise
  if (num_nodes>=2 && num_constraints>=1) { 
    ROS_DEBUG_NAMED ("pose_graph_spa", "Performing spa with %zu nodes and %zu constraints",
                     num_nodes, num_constraints);
    res.spa->doSPA(10, 1.0e-4, SBA_BLOCK_JACOBIAN_PCG);
    ROS_DEBUG_NAMED ("pose_graph_spa", "Graph optimization complete");
  }
  else 
    ROS_DEBUG_NAMED ("pose_graph_spa", "Not optimizing graph with %zu nodes and %zu constraints",
                     num_nodes, num_constraints);
  
  /******************************
   * 3. Return optimized poses
   ******************************/

  NodePoseMap optimized_poses;
  BOOST_FOREACH (const NodeIndexMap::value_type entry, res.node_index_map) {
    optimized_poses[entry.first] = getNodePose(res.spa->nodes[entry.second]);
    ROS_DEBUG_STREAM_NAMED ("spa_result", "  Node " << entry.first <<
                            "; pose " << optimized_poses[entry.first]);
  }
  return optimized_poses;
}






} // namespace