spa_conversion.cpp

Go to the documentation of this file.

#include <pose_graph/spa_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::Node;
using sba::SysSPA;
using sba::ConP2;
using geometry_msgs::Pose;

// Make a spa node given an initial pose estimate
Node makeNode (const Pose& initial_pose)
{
  Node n;
  n.trans = translationToVector4d(initial_pose.position);
  n.qrot = quaternionMsgToVector4d(initial_pose.orientation);
  n.normRot();
  n.setTransform();
  n.setDr();
  return n;
}


// Turn a PoseConstraint into a ConP2
ConP2 makeSpaConstraint (const PoseConstraint& constraint)
{
  ConP2 spa;
  spa.tmean = constraint.translation;
  spa.qpmean = constraint.rotation.inverse();

  spa.prec = constraint.precision;

  return spa;
}


// Turn a pose graph into a SysSpa
SpaConversionResult poseGraphToSpa (const PoseGraph& pose_graph)
{
  SpaPtr spa(new SysSPA());
  map<NodeId, unsigned> node_indices;
  unsigned next_index=0;

  // 1. Add the nodes
  BOOST_FOREACH (const NodeId n, pose_graph.allNodes()) {
    spa->nodes.push_back(makeNode(pose_graph.getInitialPoseEstimate(n)));
    node_indices[n] = next_index++;
  }

  // 2. Add the edge constraints
  BOOST_FOREACH (const EdgeId e, pose_graph.allEdges()) {
    
    std::pair<NodeId, NodeId> ids = pose_graph.incidentNodes(e);
    const pose_graph::PoseConstraint constraint = pose_graph.getConstraint(e);
    ConP2 spa_constraint = makeSpaConstraint(constraint);

    spa_constraint.ndr = node_indices[ids.first];
    spa_constraint.nd1 = node_indices[ids.second];
    spa->p2cons.push_back(spa_constraint);
    ROS_DEBUG_STREAM_NAMED ("pose_graph_spa", "Adding constraint with tmean " << spa_constraint.tmean << 
                            ", qpmean " << spa_constraint.qpmean.coeffs() << " and precision "
                            << spa_constraint.prec << " from " << 
                            spa_constraint.ndr << " to " << spa_constraint.nd1);
  }

  return SpaConversionResult(spa, node_indices);
}


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

  pose.position.x = n.trans(0);
  pose.position.y = n.trans(1);
  pose.position.z = n.trans(2);

  // Per documentation, qrot is in xyzw order
  // Since Eigen uses wxyz, n.qrot(w) is not the same as n.qrot[3]
  pose.orientation.x = n.qrot.x();
  pose.orientation.y = n.qrot.y();
  pose.orientation.z = n.qrot.z();
  pose.orientation.w = n.qrot.w();

  return pose;
}


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

  ROS_DEBUG_NAMED ("pose_graph_spa", "Converting graph with %zu nodes to spa", g.allNodes().size());
  SpaConversionResult res=poseGraphToSpa(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);
    int num_iters = res.spa->doSPA(10, 1.0e-4, true);
    ROS_DEBUG_NAMED ("pose_graph_spa", "Graph optimization complete; took %d iterations", num_iters);
  }
  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