pose.hpp

/*
 * pose.hpp
 *
 *  Copyright (c) 2013 Kumar Robotics. All rights reserved.
 *
 *  This file is part of kr_math.
 *
 *  Created on: 28/06/2014
 *      Author: gareth
 */

#ifndef KR_MATH_POSE_H_
#define KR_MATH_POSE_H_

/* GTSAM include first since it needs to be included before Eigen as they
 * have a custom Eigen version where they overload some Eigen classes. Having it
 * after the Eigen include prevents their Eigen header being included due to
 * header guards
 */
#ifdef KR_MATH_GTSAM_CONVERSIONS
#include <gtsam/geometry/Pose3.h>
#endif

#include "base_types.hpp"
#include <Eigen/Geometry>

#ifdef KR_MATH_ROS_CONVERSIONS
#include <geometry_msgs/Pose.h>
#endif

namespace kr {

/**
 * @brief Pose, an orientation and position in R3.
 *
 * @note Some important notes on convention:
 *
 * When we write _A to B_ transformation (or transform), it is in reference to
 * the operation effecuated by matrix H, such that:
 *
 * b = H * a, where a is a vector in the A frame and b is that same vector
 * in the B frame.
 *
 * kr::Pose stores `q`, which rotates vectors from body to world frame,
 * and `p`, the center of the body as expressed in the world frame.
 *
 * Transforming a vector v from world to camera (body) is performed with:
 *
 * vb = R(q') * (vw - p), where R(.) maps from S(4) to SO(3).
 *
 * It is important to note that p is equivalent to the body origin in world
 * frame, often denoted wTb.
 *
 */
template <typename Scalar> struct Pose {
private:
  kr::Quat<Scalar> q_; /**< Orientation (body to world transform, or wRb) */
  kr::Vec3<Scalar> p_; /**< Position (origin to body vector) */

public:
  /**
   * @brief Construct with identity pose.
   */
  Pose() : q_(1, 0, 0, 0) { p_[0] = p_[1] = p_[2] = 0; }

  /**
   * @brief Copy-ctor
   */
  Pose(const Pose<Scalar> &pose) : q_(pose.q_), p_(pose.p_) {}

  /**
   * @brief Construct from quaternion and position vector.
   * @param wQb Body to world rotation.
   * @param pInW Origin to body vector.
   */
  Pose(const Quat<Scalar> &wQb, const Vec3<Scalar> &pInW) : q_(wQb), p_(pInW) {}

  /**
   * @brief Construct from a 4x4 homogenous transformation matrix.
   * @param bHw 4x4 matrix representing the body to world transform.
   *
   * @note wHb is expected to perform the operation: vw = wHb * vb.
   */
  Pose(const kr::Mat4<Scalar>& wHb) :
    q_(wHb.template block<3,3>(0,0)), p_(wHb.template block<3,1>(0,3)) {}

#ifdef KR_MATH_ROS_CONVERSIONS
  /**
   * @brief Construct pose from ROS message.
   * @param geo Instance of geometry_msgs::Pose.
   * @note Loss of accuracy will occur if Scalar is float32.
   */
  Pose(const geometry_msgs::Pose &geo) {
    q_ = kr::Quat<Scalar>(geo.orientation.w, geo.orientation.x,
                         geo.orientation.y, geo.orientation.z);
    p_[0] = geo.position.x;
    p_[1] = geo.position.y;
    p_[2] = geo.position.z;
  }

  /**
   * @brief Explicit cast to ROS geometry_msgs::Pose.
   */
  explicit operator geometry_msgs::Pose() const {
    geometry_msgs::Pose geo;
    geo.orientation.w = q_.w();
    geo.orientation.x = q_.x();
    geo.orientation.y = q_.y();
    geo.orientation.z = q_.z();
    geo.position.x = p_[0];
    geo.position.y = p_[1];
    geo.position.z = p_[2];
    return geo;
  }
#endif

#ifdef KR_MATH_GTSAM_CONVERSIONS
  /**
   * @brief Construct pose from GTSAM Pose3.
   * @param gtpose Instance of GTSAM 6DOF pose. GTSAM poses store a body to
   * world transformation.
   */
  Pose(const gtsam::Pose3& gtpose) :
    q_(gtpose.rotation().toQuaternion().cast<Scalar>()),
    p_(gtpose.translation().vector().cast<Scalar>()) {}

  /**
   * @brief Explicit cast to GTSAM Pose3.
   * @note Converts to the GTSAM convention of body to world.
   */
  explicit operator gtsam::Pose3() const {
    const gtsam::Rot3 rot(q_.template cast<double>());
    return gtsam::Pose3(rot, gtsam::Point3(p_.template cast<double>()));
  }
#endif

  /**
   * @brief Position of this pose in the world frame.
   * @return kr::Vec3
   */
  const kr::Vec3<Scalar>& p() const { return p_; }
  kr::Vec3<Scalar>& p() { return p_; }

  /**
   * @brief Quaternion that performs the body to world rotation on vectors.
   * @return kr::Quat
   */
  const kr::Quat<Scalar>& q() const { return q_; }
  kr::Quat<Scalar>& q() { return q_; }

  /**
   * @brief Construct a pose from an rvec/tvec pair.
   *
   * @param rvec Rotation vector. If R = exp(rvec), then R*v will rotate vector
   * v from world to body frame.
   * @param tvec Translation vector, or t = -R*p.
   *
   * @return The corresponding pose.
   *
   * @note The vectors accepted here are in the same form produced by OpenCV.
   */
  static Pose<Scalar> fromVectors(const Vec3<Scalar>& rvec,
                                  const Vec3<Scalar>& tvec) {
    Vec3<Scalar> rnorm(0.0,0.0,0.0);
    const Scalar rn = rvec.norm();
    if (rn > std::numeric_limits<Scalar>::epsilon()*10) {
      rnorm = rvec / rn;
    }
    const Eigen::AngleAxis<Scalar> aa(rn, rnorm);
    Pose<Scalar> pose;
    pose.q_ = kr::Quat<Scalar>(aa).conjugate();
    pose.p_ = -(pose.q_.matrix() * tvec);
    return pose;
  }

  /**
   * @brief Convert this pose so that it is expressed in the body frame of the
   * argument.
   *
   * @param alt Treat as identity reference frame.
   * @return The receiver, as expressed in frame alt.
   */
  Pose<Scalar> expressedIn(const Pose<Scalar> &alt) const {
    const kr::Vec3<Scalar> pn = alt.bRw() * (p_ - alt.p_);
    return Pose(alt.q_.conjugate() * q_, pn);
  }

  /**
   * @brief Generate the inverse transformation.
   * @return Inverse of this pose: [R^T, -R * p]
   *
   * @note If H is the homo. matrix representation of the receiver, then the
   * returned pose will have representation H' where H * H' = eye(4).
   */
  Pose<Scalar> inverse() const {
    return Pose<Scalar>().expressedIn(*this);
  }

  /**
   * @brief Difference between this pose and the argument in world frame.
   * @param alt Pose to subtract from the receiver.
   *
   * @return The difference between the receiver and the argument, as expressed
   * in the world frame.
   */
  Pose<Scalar> difference(const Pose<Scalar> &alt) const {
    return Pose(alt.q_.conjugate() * q_, p_ - alt.p_);
  }

  /**
   * @brief Translation vector of this pose.
   * @return Vector corresponding to: t = -bRw * p.
   */
  kr::Vec3<Scalar> translation() const { return bRw() * -p_; }

  /**
   * @brief Compose a pose onto the receiver.
   * @param rhs Pose to multiply/add onto this one.
   * @return New pose after composition.
   *
   * @note It is assumed that `rhs` expresses a pose in the body frame of the
   * receiver. This operation therefore rotates `rhs` position vector into the
   * world frame prior to addition. Orientation is composed from the right,
   * corresponding to a multiplication in the body frame.
   */
  Pose<Scalar> composeInBody(const Pose<Scalar>& rhs) const {
    return Pose(q_ * rhs.q_, p_ + q_.matrix() * rhs.p_);
  }

  /**
   * @brief Transform a point into the body frame of this pose.
   * @param v Point to transform, expressed in the world frame.
   * @return Point `v` after conversion to the body frame.
   */
  kr::Vec3<Scalar> transformToBody(const kr::Vec3<Scalar>& v) const {
    return bRw() * (v - p_);
  }

  /**
   * @brief Transform a point from the body frame of this pose.
   * @param v Point to transform, expressed in body frame.
   * @return Point `v` after conversion to the world frame.
   */
  kr::Vec3<Scalar> transformFromBody(const kr::Vec3<Scalar>& v) const {
    return wRb()*v + p_;
  }

  /**
   * @brief SE(3) transformation corresponding to this pose.
   * @return 4x4 matrix that performs the body to world transformation: wHb
   */
  kr::Mat4<Scalar> matrix() const {
    kr::Mat4<Scalar> H;
    H.template block<3,3>(0,0) = q_.matrix();
    H.template block<3,1>(0,3) = p_;
    H(3,0) = H(3,1) = H(3,2) = 0;
    H(3,3) = 1;
    return H;
  }

  /**
   * @brief Shorthand for body to world rotation.
   * @note wP = wRb * bP + wTb
   */
  kr::Mat3<Scalar> wRb() const {
    return q_.matrix();
  }

  /**
   * @brief Shorthand for world to body roration.
   * @note bP = bRw * wP + bTw
   */
  kr::Mat3<Scalar> bRw() const {
    return q_.conjugate().matrix();
  }

  /**
   * @brief bTw Translation from body to world in body frame
   * @note bP = bRw * wP + bTw
   * @note Equivalent to the 'translation' vector.
   */
  kr::Vec3<Scalar> bTw() const {
    return translation(); //  -(bRw() * wTb())
  }

  /**
   * @brief Shorthand for translation vector from world origin to body.
   * @note wP = wRb * bP + wTb
   */
  const kr::Vec3<Scalar>& wTb() const {
    return p_;
  }
};

typedef Pose<float> Posef;
typedef Pose<double> Posed;

} // namespace kr

#endif // KR_MATH_POSE_H_