transforms.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/transforms.h>
#include <tf/transform_datatypes.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/lexical_cast.hpp>



#include <ros/ros.h>

namespace pose_graph
{

namespace nm=nav_msgs;
namespace gm=geometry_msgs;

using Eigen3::Translation3d;
using Eigen3::Quaterniond;
using std::string;
using nav_msgs::OccupancyGrid;

// Turn a ROS Pose into an Eigen Transform (the one that maps from the pose frame to the world frame)
Affine3d poseToWorldTransform (const Pose& p)
{
  return
    Translation3d(p.position.x, p.position.y, p.position.z) *
    Quaterniond(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z);
}


// Transform from pose1's frame to pose2's frame
Affine3d relativeTransform (const Pose& pose1, const Pose& pose2)
{
  const Affine3d inv2(poseToWorldTransform(pose2).inverse());
  const Affine3d result=inv2*poseToWorldTransform(pose1);
  return result;
}

// Transform that sends pose1 to pose2
Affine3d transformBetween (const Pose& pose1, const Pose& pose2)
{
  const Affine3d inv1(poseToWorldTransform(pose1).inverse());
  return poseToWorldTransform(pose2) * inv1;
}


// Apply an Eigen Transform to a ROS Pose
Pose applyTransform (const Affine3d& trans, const Pose& pose)
{
  const Affine3d pose_to_world = poseToWorldTransform(pose);
  const Affine3d new_pose_to_world = trans*pose_to_world;
  Pose new_pose;
  new_pose.position.x = new_pose_to_world.translation().x();
  new_pose.position.y = new_pose_to_world.translation().y();
  new_pose.position.z = new_pose_to_world.translation().z();
  const Quaterniond rotation(new_pose_to_world.rotation());
  new_pose.orientation.x = rotation.x();
  new_pose.orientation.y = rotation.y();
  new_pose.orientation.z = rotation.z();
  new_pose.orientation.w = rotation.w();

  return new_pose;
}

// Apply an Eigen transform to a ROS point
Point32 transformPoint (const Affine3d& trans, const Point32& point)
{
  Point32 ret;
  Vector3d result = trans*Vector3d(point.x, point.y, point.z);
  ret.x = result.x();
  ret.y = result.y();
  ret.z = result.z();

  return ret;
}

// Apply an Eigen transform to a ROS point
Point transformPoint (const Affine3d& trans, const Point& point)
{
  Point ret;
  Vector3d result = trans*Vector3d(point.x, point.y, point.z);
  ret.x = result.x();
  ret.y = result.y();
  ret.z = result.z();

  return ret;
}


Pose perturbPose (const Pose& pose, double r)
{
  boost::mt19937 rng(ros::Time::now().toNSec());
  boost::uniform_real<float> u(-r, r);
  boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > gen(rng, u);

  Pose perturbed;
  perturbed.orientation = pose.orientation;
  perturbed.position.x = pose.position.x + gen();
  perturbed.position.y = pose.position.y + gen();
  perturbed.position.z = pose.position.z;

  return perturbed;
}

Point pointInReferenceFrame (const Point& p, const Pose& pose)
{
  return transformPoint(poseToWorldTransform(pose), p);
}



Eigen3::Vector4d quaternionMsgToVector4d (const geometry_msgs::Quaternion& m)
{
  return Eigen3::Vector4d(m.x, m.y, m.z, m.w);
}

Eigen3::Vector4d translationToVector4d(const geometry_msgs::Point& p)
{
  return Eigen3::Vector4d(p.x, p.y, p.z, 1.0);
}



Pose2D projectToPose2D (const Pose& pose)
{
  Pose2D pose2d;
  pose2d.x = pose.position.x;
  pose2d.y = pose.position.y;
  pose2d.theta = tf::getYaw(pose.orientation);
  return pose2d;
}



Affine3d relativeTransform (const Pose2D& pose1, const Pose2D& pose2)
{
  return relativeTransform(convertToPose(pose1), convertToPose(pose2));
}

Pose relativePose (const Pose& pose1, const Pose& pose2)
{
  const Affine3d toPose2Coords(poseToWorldTransform(pose2).inverse());
  return applyTransform(toPose2Coords, pose1);
}


Pose convertToPose (const Pose2D& pose2d)
{
  Pose p;
  p.position.x = pose2d.x;
  p.position.y = pose2d.y;
  p.position.z = 0.0;
  p.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
  return p;
}

Pose convertToPose (const Affine3d trans)
{
  Pose id;
  id.orientation.w = 1.0;
  return applyTransform(trans, id);
}

OccupancyGrid transformGrid (const Affine3d& trans, const OccupancyGrid& g)
{
  OccupancyGrid g2(g);
  g2.info.origin = applyTransform(trans, g.info.origin);
  return g2;
}

Pose2D makePose2D (double x, double y, double theta)
{
  Pose2D p;
  p.x = x;
  p.y = y;
  p.theta = theta;
  return p;
}


nm::MapMetaData getMapInfo (const gm::Pose& pose, const double size, const double resolution)
{
  const Eigen3::Affine3d node_ref_transform = poseToWorldTransform(pose);

  gm::Pose local_origin;
  local_origin.position.x = -size/2;
  local_origin.position.y = -size/2;
  local_origin.orientation.w = 1.0;

  nm::MapMetaData info;
  info.resolution = resolution;
  info.width = size/resolution;
  info.height = size/resolution;
  info.origin = applyTransform(node_ref_transform, local_origin);

  return info;
}


} // namespace pose_graph