test_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.h>
#include <pose_graph/pose_graph_message.h>
#include <pose_graph/utils.h>
#include <pose_graph/exception.h>
#include <pose_graph/spa_conversion.h>
#include <pose_graph/spa_2d_conversion.h>
#include <tf/transform_datatypes.h>
#include <std_msgs/Int16.h>
#include <gtest/gtest.h>
#include <iostream>
#include <boost/assign.hpp>
#include <boost/foreach.hpp>


using namespace pose_graph;
using namespace std;
using Eigen3::Quaterniond;
using Eigen3::Vector3d;
using boost::assign::operator+=;
using occupancy_grid_utils::LocalizedCloud;
using sensor_msgs::LaserScan;

namespace geometry_msgs
{

bool operator==(const gm::Pose& p1, const gm::Pose& p2)
{
  return (p1.position.x == p2.position.x) &&
    (p1.position.y == p2.position.y) &&
    (p1.position.z == p2.position.z) &&
    (p1.orientation.x == p2.orientation.x) &&
    (p1.orientation.y == p2.orientation.y) &&
    (p1.orientation.z == p2.orientation.z) &&
    (p1.orientation.w == p2.orientation.w);
}

} // namespace geometry_msgs

template <class T>
ostream& operator<< (ostream& str, const set<T>& s)
{
  std::copy(s.begin(), s.end(), std::ostream_iterator<NodeId>(str, " "));
  return str;
}


template <class S>
bool equalSets (set<S> s1, set<S> s2)
{
  if (s1.size()==s2.size()) {
    BOOST_FOREACH (S x, s1) {
      if (s2.find(x)==s2.end()) {
        return false;
      }
    }
    return true;
  }
  else {
    cout << "\nSets aren't equal:\n  S1: ";
    copy(s1.begin(), s1.end(), ostream_iterator<S>(cout, " "));
    cout << "\n  S2: ";
    copy(s2.begin(), s2.end(), ostream_iterator<S>(cout, " "));
    return false;
  }
}

gm::Pose makePose (const double x, const double y, const double theta)
{
  gm::Pose p;
  p.position.x = x;
  p.position.y = y;
  p.orientation = tf::createQuaternionMsgFromYaw(theta);
  return p;
}

void printGraph (const PoseGraph& g)
{
  BOOST_FOREACH (const NodeId n, g.allNodes()) {
    cout << "\nNode " << n << ".  Edges: ";
    set<EdgeId> edges = g.incidentEdges(n);
    copy(edges.begin(), edges.end(), ostream_iterator<EdgeId>(cout, " "));
  }
  cout << "\n";
}

const double TOL=1e-3;

bool approxEqual (const gm::Quaternion q1, const gm::Quaternion q2)
{
  const double dx = q1.x - q2.x;
  const double dy = q1.y - q2.y;
  const double dz = q1.z - q2.z;
  const double dw = q1.w - q2.w;
  return dx*dx + dy*dy + dz*dz + dw*dw < TOL;
}

bool approxEqualPoses (const gm::Pose& p1, const gm::Pose& p2)
{
  return (euclideanDistance(p1.position, p2.position) < TOL) &&
    approxEqual(p1.orientation, p2.orientation);
}



TEST(PoseGraph, GraphOps)
{
  PoseGraph g;
  PoseConstraint constraint(Vector3d(1, 2, 3), Quaterniond(1, 0, 0, 0));
  PoseConstraint constraint2(Vector3d(2, -1, 1), Quaterniond(0, 1, 0, 0));

  const set<NodeId> empty;
  EXPECT_PRED2(equalSets<NodeId>, empty, g.allNodes());


  /****************************************
   * Add a few nodes and edges
   ****************************************/


  NodeId n1 = g.addNode();
  NodeId n2 = g.addNode();
  NodeId n3 = g.addNode();
  EdgeId e12 = g.addEdge(n1, n2, constraint);
  EdgeId e23 = g.addEdge(n2, n3, constraint2);
  EdgeId e23b = g.addEdge(n2, n3, constraint);

  set<EdgeId> edges1, edges2, edges;
  edges1.insert(e12);
  edges2.insert(e12);
  edges2.insert(e23);
  edges2.insert(e23b);
  edges.insert(e12);
  edges.insert(e23);
  edges.insert(e23b);

  set<NodeId> nodes;
  nodes.insert(n1);
  nodes.insert(n2);
  nodes.insert(n3);

  gm::Pose pose, identity;
  identity.orientation.w=1.0;
  pose.position.y = 42.0;
  pose.orientation.z = 24.0;
  g.setInitialPoseEstimate(n2, pose);
  EXPECT_THROW(g.setInitialPoseEstimate(NodeId(42), pose), UnknownNodeIdException);
  EXPECT_EQ(identity, g.getInitialPoseEstimate(n1));
  EXPECT_EQ(pose, g.getInitialPoseEstimate(n2));

  /*
  cout << "\nEdges2: ";
  copy(edges2.begin(), edges2.end(), ostream_iterator<EdgeId>(cout, " "));
  */

  /****************************************
   * Check they were added correctly
   ****************************************/

  EXPECT_TRUE(e23!=e23b);
  EXPECT_TRUE(equalSets(g.incidentEdges(n1), edges1));
  EXPECT_TRUE(equalSets(g.incidentEdges(n2), edges2));

  /****************************************
   * Debug cloud attaching
   ****************************************/

  LocalizedCloud::Ptr s1(new LocalizedCloud());
  LocalizedCloud::Ptr s3(new LocalizedCloud());
  s1->cloud.points.resize(1);
  s3->cloud.points.resize(2);
  EXPECT_TRUE(!g.hasCloud(n3));
  g.attachCloud(n3, s3);
  EXPECT_TRUE(g.hasCloud(n3));
  EXPECT_TRUE(!g.hasCloud(n1));
  g.attachCloud(n1, s1);

  EXPECT_EQ(2u, g.getCloud(n3)->cloud.points.size());
  EXPECT_EQ(1u, g.getCloud(n1)->cloud.points.size());
  


  /****************************************
   * Test the copy constructor
   ****************************************/

  const PoseGraph g2(g);

  EXPECT_TRUE(equalSets(g2.incidentEdges(n1), edges1));
  EXPECT_TRUE(equalSets(g2.incidentEdges(n2), edges2));
  EXPECT_TRUE(equalSets(g2.allEdges(), edges));
  EXPECT_TRUE(equalSets(g2.allNodes(), nodes));

  /****************************************
   * Conversion to Ros messages
   ****************************************/

  const PoseGraphMessage m = poseGraphToRos(g2);
  const PoseGraph g3 = poseGraphFromRos(m);
  
  EXPECT_TRUE(equalSets(g3.incidentEdges(n1), edges1));
  EXPECT_TRUE(equalSets(g3.incidentEdges(n2), edges2));
  EXPECT_TRUE(equalSets(g3.allEdges(), edges));
  EXPECT_TRUE(equalSets(g3.allNodes(), nodes));

  /****************************************
   * File input/output
   ****************************************/

  // Currently doesn't work

  /*

  string filename="/tmp/test_pose_graph.bag";
  writeToFile(g3, filename);
  const PoseGraph g4 = readFromFile(filename);

  EXPECT_TRUE(equalSets(g4.incidentEdges(n1), edges1));
  EXPECT_TRUE(equalSets(g4.incidentEdges(n2), edges2));
  EXPECT_TRUE(equalSets(g4.allEdges(), edges));
  EXPECT_TRUE(equalSets(g4.allNodes(), nodes));
  */

  // Instead
  const PoseGraph g4(g3);
  

  /****************************************
   * Test edge lengths were added and
   * copied correctly
   ****************************************/
  EXPECT_EQ(sqrt(14), g4.edgeLength(e12));
  EXPECT_EQ(sqrt(6), g4.edgeLength(e23));

  /****************************************
   * Initial pose estimates also
   ****************************************/

  EXPECT_EQ(identity, g4.getInitialPoseEstimate(n1));
  EXPECT_EQ(pose, g4.getInitialPoseEstimate(n2));
  EXPECT_THROW(g4.getInitialPoseEstimate(NodeId(42)), UnknownNodeIdException);


  /****************************************
   * Make sure the clouds are still there
   ****************************************/
  EXPECT_EQ(2u, g4.getCloud(n3)->cloud.points.size());
  EXPECT_EQ(1u, g4.getCloud(n1)->cloud.points.size());



  /****************************************
   * Test nearbyNodes
   ****************************************/
  
  set<NodeId> neighbors1, neighbors2;
  neighbors1.insert(n2);
  neighbors2.insert(n2);
  neighbors2.insert(n3);

  EXPECT_TRUE(equalSets(g4.nearbyNodes(n2, 2.0), neighbors1));
  EXPECT_TRUE(equalSets(g4.nearbyNodes(n2, 3.0), neighbors2));
  EXPECT_TRUE(equalSets(g4.nearbyNodes(n2, 4.0), nodes));

  /****************************************
   * Subgraph
   ****************************************/

  PoseGraph g5(g4);
  const NodeId n4=g5.addNode();
  const EdgeId e42=g5.addEdge(n4, n2, constraint2);
  g5.addEdge(n1, n4, constraint);
  set<NodeId> nodes234;
  nodes234 += n2, n3, n4;
  const PoseGraph g6 = g5.subgraph(nodes234);

  EXPECT_PRED2 (equalSets<NodeId>, nodes234, g6.allNodes());
  set<EdgeId> edges234;
  edges234 += e23b, e23, e42;
  EXPECT_TRUE (equalSets<EdgeId>(edges234, g6.allEdges()));
  EXPECT_TRUE (equalSets<EdgeId>(edges234, g6.incidentEdges(n2)));
  set<EdgeId> edges42;
  edges42 += e42;
  EXPECT_TRUE (equalSets<EdgeId>(edges42, g6.incidentEdges(n4)));


}

TEST(PoseGraph, ShortestPath) 
{
  PoseGraph g;
  const PoseConstraint constraint(Vector3d(1, 2, 3), Quaterniond(1, 0, 0, 0));
  const PoseConstraint constraint2(Vector3d(2, -1, 1), Quaterniond(0.7071, 0, 0, 0.7071));
  const PoseConstraint constraint3(Vector3d(1, 0, 0), Quaterniond(0.9239,0,0,0.3827));

  const NodeId n1 = g.addNode();
  const NodeId n2 = g.addNode();
  const NodeId n3 = g.addNode();
  const NodeId n4 = g.addNode();
  const NodeId n5 = g.addNode();
  const NodeId n6 = g.addNode();
  g.addEdge(n1, n2, constraint);
  g.addEdge(n2, n3, constraint2);
  g.addEdge(n1, n3, constraint3);
  g.addEdge(n1, n4, constraint3);
  g.addEdge(n5, n6, constraint);

  const ShortestPathResult res = g.shortestPath(n1, n2);
  EXPECT_EQ(3u, res.nodes.size());
  EXPECT_EQ(n1, res.nodes[0]);
  EXPECT_EQ(n3, res.nodes[1]);
  EXPECT_EQ(n2, res.nodes[2]);
  EXPECT_EQ(1+sqrt(6), res.cost);
  EXPECT_TRUE(res.path_found);
  EXPECT_EQ(2u, res.edges.size());
  typedef std::pair<NodeId, NodeId> Nodes;
  Nodes nds1 = g.incidentNodes(res.edges[0]);
  Nodes nds2 = g.incidentNodes(res.edges[1]);
  EXPECT_EQ(n1, min(nds1.first, nds1.second));
  EXPECT_EQ(n3, max(nds1.first, nds1.second));
  EXPECT_EQ(n2, min(nds2.first, nds2.second));
  EXPECT_EQ(n3, max(nds2.first, nds2.second));  
 
  const ShortestPathResult res2 = g.shortestPath(n2, n4);
  EXPECT_EQ(4u, res2.nodes.size());
  EXPECT_EQ(2+sqrt(6), res2.cost);
  EXPECT_EQ(n4, res2.nodes[3]);
  EXPECT_EQ(n2, res2.nodes[0]);
  EXPECT_EQ(n1, res2.nodes[2]);
  EXPECT_EQ(n3, res2.nodes[1]);
  EXPECT_EQ(3u, res2.edges.size());
  EXPECT_TRUE(res2.path_found);

  const ShortestPathResult res3 = g.shortestPath(n5, n1);
  EXPECT_TRUE(!res3.path_found);

  const gm::Pose rel_pose = g.relativePose(n2, n1);
  gm::Pose expected_pose;
  expected_pose.position.x = 0.2928617;
  expected_pose.position.y = 2.12130969762;
  expected_pose.position.z = -1;
  expected_pose.orientation.z = -0.38267653;
  expected_pose.orientation.w = 0.923882353;
  
  EXPECT_PRED2(approxEqualPoses, rel_pose, expected_pose);
}


double dist (const gm::Pose& p, const gm::Pose& p2)
{
  const double dx=p.position.x-p2.position.x;
  const double dy=p.position.y-p2.position.y;
  return sqrt(dx*dx+dy*dy);
}


TEST(PoseGraph, Optimization) 
{
  PoseGraph g;

  const NodeId n1 = g.addNode();
  const NodeId n2 = g.addNode();
  const NodeId n3 = g.addNode();

  Eigen3::Matrix<double, 6, 6> p1, p2;
  p1 = Eigen3::MatrixXd::Identity(6,6);
  p2 = p1;

  const PoseConstraint c12(Vector3d(1,0,0), Quaterniond(1, 0, 0, 0), p1);
  const PoseConstraint c13(Vector3d(0.5,1,0), Quaterniond(1, 0, 0, 0), p1);
  const PoseConstraint c32(Vector3d(0.5,-1,0), Quaterniond(1, 0, 0, 0), p2);

  g.addEdge(n1, n2, c12);
  g.addEdge(n1, n3, c13);
  g.addEdge(n3, n2, c32);

  g.setInitialPoseEstimate(n1, makePose(0, 0, 0));
  g.setInitialPoseEstimate(n2, makePose(2, 5, 0));
  g.setInitialPoseEstimate(n3, makePose(9, 1, 0));


  NodePoseMap opt=optimizeGraph(g);
  
  EXPECT_TRUE (0.98 < dist(opt[n1], opt[n2]));
  EXPECT_TRUE (dist(opt[n1], opt[n2]) < 1.02);
  EXPECT_TRUE (1.1 < dist(opt[n3], opt[n2])); 
  EXPECT_TRUE (dist(opt[n3], opt[n2]) < 1.2);
  EXPECT_TRUE (1.1 < dist(opt[n1], opt[n3]));
  EXPECT_TRUE (dist(opt[n1], opt[n3]) < 1.2);

  NodePoseMap opt2d=optimizeGraph2D(g);
  
  EXPECT_TRUE (0.98 < dist(opt2d[n1], opt2d[n2]));
  EXPECT_TRUE (dist(opt2d[n1], opt2d[n2]) < 1.02);
  EXPECT_TRUE (1.1 < dist(opt2d[n3], opt2d[n2])); 
  EXPECT_TRUE (dist(opt2d[n3], opt2d[n2]) < 1.2);
  EXPECT_TRUE (1.1 < dist(opt2d[n1], opt2d[n3]));
  EXPECT_TRUE (dist(opt2d[n1], opt2d[n3]) < 1.2);


}


const double PI = 3.14159265;

LaserScan::Ptr makeScan ()
{
  LaserScan::Ptr scan(new LaserScan());
  scan->angle_min = -PI/2;
  scan->angle_max = PI/2;
  scan->angle_increment = PI/4;
  scan->range_min = 0;
  scan->range_max = 10;
  scan->ranges.resize(5);
  fill(scan->ranges.begin(), scan->ranges.end(), 20);
  return scan;
}


int main (int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}