geometry.cpp

Go to the documentation of this file.

#include <pose_graph/geometry.h>
#include <tf/transform_datatypes.h>
#include <boost/foreach.hpp>

namespace pose_graph 
{

namespace sm=sensor_msgs;
namespace gm=geometry_msgs;

using Eigen3::Quaterniond;
using Eigen3::Vector3d;
using Eigen3::Affine3d;
using geometry_msgs::Pose;
using geometry_msgs::Point;
using geometry_msgs::Point32;
using std::min;

PoseConstraint::PoseConstraint () : translation(Vector3d(0,0,0)), rotation(Quaterniond(0,0,0,1))
{
  precision = Eigen3::MatrixXd::Identity(6, 6);  
}

PoseConstraint::PoseConstraint (const Vector3d& translation, const Quaterniond& rotation)
  : translation(translation), rotation(rotation)
{
  precision = Eigen3::MatrixXd::Identity(6, 6);
}

PoseConstraint::PoseConstraint (const Eigen3::Vector3d& translation, const Eigen3::Quaterniond& rotation, 
                                const Eigen3::Matrix<double, 6, 6>& precision) :
  translation(translation), rotation(rotation), precision(precision)
{
}



// Print a constraint
ostream& operator<< (ostream& str, const PoseConstraint& constraint)
{
  tf::Quaternion q(constraint.rotation.x(), constraint.rotation.y(),
                   constraint.rotation.z(), constraint.rotation.w());
  str << "[" << constraint.translation.x() << ", " << constraint.translation.y() 
      << ", " << tf::getYaw(q) << "]";
  return str;
}


// Length of a vector
double length (const Vector3d& v)
{
  return sqrt(v.dot(v));
}

// Length of translation part of pose constraint
double length (const PoseConstraint& constraint)
{
  return length(constraint.translation);
}

double length (const gm::Point& p)
{
  return sqrt(p.x*p.x + p.y+p.y + p.z+p.z);
}

// Given a pose constraint, return the expected relative pose
Pose getPose (const PoseConstraint& c)
{
  Pose p;
  p.position.x = c.translation.x();
  p.position.y = c.translation.y();
  p.position.z = c.translation.z();
  p.orientation.x = c.rotation.x();
  p.orientation.y = c.rotation.y();
  p.orientation.z = c.rotation.z();
  p.orientation.w = c.rotation.w();
  return p;
}




// Given a transform between two frames, get a PoseConstraint.  
PoseConstraint makeConstraint (const Affine3d& trans, const Eigen3::Matrix<double, 6, 6>& prec)
{
  return PoseConstraint(trans.translation(), Quaterniond(trans.rotation()), prec);
}


double euclideanDistance (const Point& p1, const Point& p2)
{
  return length(Vector3d(p1.x-p2.x, p1.y-p2.y, p1.z-p2.z));
}


Point convertToPoint (const Point32& p)
{
  Point p2;
  p2.x = p.x;
  p2.y = p.y;
  p2.z = p.z;
  return p2;
}


gm::Point computeBarycenter (const sm::LaserScan& scan)
{
  gm::Point sum;
  double angle=scan.angle_min;
  unsigned num_hits=0;

  BOOST_FOREACH (const double range, scan.ranges) {
    ROS_ASSERT (angle>=scan.angle_min && angle<=scan.angle_max+scan.angle_increment/2);
    if (range <= scan.range_max) {
      sum.x += range*cos(angle);
      sum.y += range*sin(angle);
      num_hits++;
    }
    angle += scan.angle_increment;
  }
  ROS_ASSERT_MSG (num_hits>0, "Can't find barycenter of scan: no in-range hits out of %zu", scan.ranges.size());

  gm::Point result;
  result.x = sum.x/num_hits;
  result.y = sum.y/num_hits;
  return result;
}
  

double euclideanDistance (const gm::Pose2D& p1, const gm::Pose2D& p2, const double mult)
{
  const double dx = p1.x - p2.x;
  const double dy = p1.y - p2.y;
  const double dtheta1 = fabs(p1.theta - p2.theta);
  const double dtheta2 = p1.theta + 2*M_PI-p2.theta;
  const double dtheta3 = p2.theta + 2*M_PI-p1.theta;
  const double dtheta = min(dtheta1, min(dtheta2, dtheta3));

  return sqrt(dx*dx + dy*dy) + mult*dtheta;
}

PrecisionMatrix makePrecisionMatrix (const double x_prec, const double y_prec, const double theta_prec)
{
  PrecisionMatrix prec = Eigen3::MatrixXd::Identity(6,6);
  prec(0,0) = x_prec;
  prec(1,1) = y_prec;
  prec(5,5) = theta_prec/4;
  return prec;
}

PrecisionMatrix makePrecisionMatrix (const double x_prec, const double y_prec, const double xy_prec,
                                     const double theta_prec)
{
  PrecisionMatrix prec = Eigen3::MatrixXd::Identity(6,6);
  prec(0,0) = x_prec;
  prec(1,1) = y_prec;
  prec(0,1) = xy_prec;
  prec(1,0) = xy_prec;
  prec(5,5) = theta_prec/4;
  return prec;
}

} // namespace