utils.h

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.
 *
 */


#ifndef POSE_GRAPH_UTILS_H
#define POSE_GRAPH_UTILS_H


#include <ros/assert.h>
#include <map>
#include <string>
#include <boost/lexical_cast.hpp>
#include <pose_graph/geometry.h>
#include <pose_graph/transforms.h>
#include <geometry_msgs/Pose2D.h>
#include <boost/format.hpp>
#include <Eigen3/Dense>
#include <numeric>

namespace pose_graph
{
namespace gm=geometry_msgs;

using std::map;
using std::pair;
using std::string;

using boost::format;


// Check if a container contains a key
template <class Key, class Container>
bool contains (const Container& container, const Key& key)
{
  return container.find(key)!=container.end();
}



// Get value corresponding to a key in a map
template <class K, class V, class C, class A>
const V& keyValue (const map<K, V, C, A>& m, const K& key)
{
  typename map<K, V, C, A>::const_iterator pos = m.find(key);
  ROS_ASSERT_MSG (pos!=m.end(), "Map did not contain a key that it was expected to");
  return pos->second;
}


/************************************************************
 * toString
 ************************************************************/

inline
string toString (const gm::Pose2D& pose)
{
  return (format("[%.2f, %.2f, %.2f]") % pose.x % pose.y % pose.theta).str();
}

inline
string toString (const gm::Point& p)
{
  return (format("[%.2f, %.2f]") % p.x % p.y).str();
}

inline
string toString (const Eigen3::Affine3d& trans)
{
  const Eigen3::Vector3d v(trans.translation());
  const Eigen3::Quaterniond q(trans.rotation());
  return (format("[[%.2f, %.2f, %.2f], [%.2f, %.2f, %.2f, %.2f]]") %
          v.x() % v.y() % v.z() % q.x() % q.y() % q.z() % q.w()).str();
}

inline
string toString2D (const gm::Pose& pose)
{
  gm::Pose2D p = projectToPose2D(pose);
  
  return (format("(%2f, %2f, %2f)") % p.x % p.y %
          p.theta).str();
}

inline
void poseEigenToMsg(const Eigen3::Affine3d &e, geometry_msgs::Pose &m)
{
  m.position.x = e.translation()[0];
  m.position.y = e.translation()[1];
  m.position.z = e.translation()[2];
  Eigen3::Quaterniond q = (Eigen3::Quaterniond)e.linear();
  m.orientation.x = q.x();
  m.orientation.y = q.y();
  m.orientation.z = q.z();
  m.orientation.w = q.w();
  if (m.orientation.w < 0) {
    m.orientation.x *= -1;
    m.orientation.y *= -1;
    m.orientation.z *= -1;
    m.orientation.w *= -1;
  }
}


inline
string toString2D (const Eigen3::Affine3d& trans)
{
  gm::Pose p;
  poseEigenToMsg(trans, p);
  return toString2D(p);
}


inline
string toString (const PoseConstraint& constraint)
{
  const Eigen3::Vector3d v(constraint.translation);
  const Eigen3::Quaterniond q(constraint.rotation);
  return (format("[[%.2f, %.2f, %.2f], [%.2f, %.2f, %.2f, %.2f]]") %
          v.x() % v.y() % v.z() % q.x() % q.x() % q.y() % q.w()).str();
}
  

inline
string toString (const gm::Pose& pose)
{
  return (format("[(%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f, %.2f)]") %
          pose.position.x % pose.position.y % pose.position.z %
          pose.orientation.x % pose.orientation.y % pose.orientation.z % pose.orientation.w).str();
}


template <class T>
std::string concatenate (const std::string& s, const T& x)
{
  return s + std::string(" ") + boost::lexical_cast<std::string>(x);
}


template <class T>
std::string toString (const std::set<T>& set)
{
  const std::string s = std::accumulate(set.begin(), set.end(), std::string(""), concatenate<T>);
  return std::string("[") + s + std::string("]");
}

template <class T>
std::string toString (const std::vector<T>& v)
{
  const std::string s = std::accumulate(v.begin(), v.end(), std::string(""), concatenate<T>);
  return std::string("[") + s + std::string("]");
}

inline
std::string toString (const PrecisionMatrix& m)
{
  return (format("(x-x: %.2f, y-y: %.2f, x-y: %.2f, th-th: %.2f)") % m(0,0) % m(1,1) % m(0,1) % m(5,5)).str();
}




} // namespace


#endif // Include guard