Transformation.hpp

Go to the documentation of this file.

/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  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 Autonomous Systems Lab / ETH Zurich 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.
 *
 *  Created on: Dec 2, 2014
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

namespace okvis {

namespace kinematics {

__inline__ double sinc(double x) {
  if (fabs(x) > 1e-6) {
    return sin(x) / x;
  } else {
    static const double c_2 = 1.0 / 6.0;
    static const double c_4 = 1.0 / 120.0;
    static const double c_6 = 1.0 / 5040.0;
    const double x_2 = x * x;
    const double x_4 = x_2 * x_2;
    const double x_6 = x_2 * x_2 * x_2;
    return 1.0 - c_2 * x_2 + c_4 * x_4 - c_6 * x_6;
  }
}

__inline__ Eigen::Quaterniond deltaQ(const Eigen::Vector3d& dAlpha)
{
  Eigen::Vector4d dq;
  double halfnorm = 0.5 * dAlpha.template tail<3>().norm();
  dq.template head<3>() = sinc(halfnorm) * 0.5 * dAlpha.template tail<3>();
  dq[3] = cos(halfnorm);
  return Eigen::Quaterniond(dq);
}

// Right Jacobian, see Forster et al. RSS 2015 eqn. (8)
__inline__ Eigen::Matrix3d rightJacobian(const Eigen::Vector3d & PhiVec) {
  const double Phi = PhiVec.norm();
  Eigen::Matrix3d retMat = Eigen::Matrix3d::Identity();
  const Eigen::Matrix3d Phi_x = okvis::kinematics::crossMx(PhiVec);
  const Eigen::Matrix3d Phi_x2 = Phi_x*Phi_x;
  if(Phi < 1.0e-4) {
    retMat += -0.5*Phi_x + 1.0/6.0*Phi_x2;
  } else {
    const double Phi2 = Phi*Phi;
    const double Phi3 = Phi2*Phi;
    retMat += -(1.0-cos(Phi))/(Phi2)*Phi_x + (Phi-sin(Phi))/Phi3*Phi_x2;
  }
  return retMat;
}

inline Transformation::Transformation(const Transformation & other)
    : parameters_(other.parameters_),
      r_(&parameters_[0]),
      q_(&parameters_[3]),
      C_(other.C_) {

}

inline Transformation::Transformation(Transformation && other)
    : parameters_(std::move(other.parameters_)),
      r_(&parameters_[0]),
      q_(&parameters_[3]),
      C_(std::move(other.C_)) {

}

inline Transformation::Transformation()
    : r_(&parameters_[0]),
      q_(&parameters_[3]),
      C_(Eigen::Matrix3d::Identity()) {
  r_ = Eigen::Vector3d(0.0, 0.0, 0.0);
  q_ = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
}

inline Transformation::Transformation(const Eigen::Vector3d & r_AB,
                                      const Eigen::Quaterniond& q_AB)
    : r_(&parameters_[0]),
      q_(&parameters_[3]) {
  r_ = r_AB;
  q_ = q_AB.normalized();
  updateC();
}
inline Transformation::Transformation(const Eigen::Matrix4d & T_AB)
    : r_(&parameters_[0]),
      q_(&parameters_[3]),
      C_(T_AB.topLeftCorner<3, 3>()) {
  r_ = (T_AB.topRightCorner<3, 1>());
  q_ = (T_AB.topLeftCorner<3, 3>());
  assert(fabs(T_AB(3, 0)) < 1.0e-12);
  assert(fabs(T_AB(3, 1)) < 1.0e-12);
  assert(fabs(T_AB(3, 2)) < 1.0e-12);
  assert(fabs(T_AB(3, 3) - 1.0) < 1.0e-12);
}
inline Transformation::~Transformation() {

}

template<typename Derived_coeffs>
inline bool Transformation::setCoeffs(
    const Eigen::MatrixBase<Derived_coeffs> & coeffs) {
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_coeffs, 7);
  parameters_ = coeffs;
  updateC();
  return true;
}

// The underlying transformation
inline Eigen::Matrix4d Transformation::T() const {
  Eigen::Matrix4d T_ret;
  T_ret.topLeftCorner<3, 3>() = C_;
  T_ret.topRightCorner<3, 1>() = r_;
  T_ret.bottomLeftCorner<1, 3>().setZero();
  T_ret(3, 3) = 1.0;
  return T_ret;
}

// return the rotation matrix
inline const Eigen::Matrix3d & Transformation::C() const {
  return C_;
}

// return the translation vector
inline const Eigen::Map<Eigen::Vector3d> & Transformation::r() const {
  return r_;
}

inline const Eigen::Map<Eigen::Quaterniond> & Transformation::q() const {
  return q_;
}

inline Eigen::Matrix<double, 3, 4> Transformation::T3x4() const {
  Eigen::Matrix<double, 3, 4> T3x4_ret;
  T3x4_ret.topLeftCorner<3, 3>() = C_;
  T3x4_ret.topRightCorner<3, 1>() = r_;
  return T3x4_ret;
}
// Return a copy of the transformation inverted.
inline Transformation Transformation::inverse() const {
  return Transformation(-(C_.transpose() * r_), q_.inverse());
}

// Set this to a random transformation.
inline void Transformation::setRandom() {
  setRandom(1.0, M_PI);
}
// Set this to a random transformation with bounded rotation and translation.
inline void Transformation::setRandom(double translationMaxMeters,
                                      double rotationMaxRadians) {
  // Create a random unit-length axis.
  Eigen::Vector3d axis = rotationMaxRadians * Eigen::Vector3d::Random();
  // Create a random rotation angle in radians.
  Eigen::Vector3d r = translationMaxMeters * Eigen::Vector3d::Random();
  r_ = r;
  q_ = Eigen::AngleAxisd(axis.norm(), axis.normalized());
  updateC();
}

// Setters
inline void Transformation::set(const Eigen::Matrix4d & T_AB) {
  r_ = (T_AB.topRightCorner<3, 1>());
  q_ = (T_AB.topLeftCorner<3, 3>());
  updateC();
}
inline void Transformation::set(const Eigen::Vector3d & r_AB,
                                const Eigen::Quaternion<double> & q_AB) {
  r_ = r_AB;
  q_ = q_AB.normalized();
  updateC();
}
// Set this transformation to identity
inline void Transformation::setIdentity() {
  q_.setIdentity();
  r_.setZero();
  C_.setIdentity();
}

inline Transformation Transformation::Identity() {
  return Transformation();
}

// operator*
inline Transformation Transformation::operator*(
    const Transformation & rhs) const {
  return Transformation(C_ * rhs.r_ + r_, q_ * rhs.q_);
}
inline Eigen::Vector3d Transformation::operator*(
    const Eigen::Vector3d & rhs) const {
  return C_ * rhs;
}
inline Eigen::Vector4d Transformation::operator*(
    const Eigen::Vector4d & rhs) const {
  const double s = rhs[3];
  Eigen::Vector4d retVec;
  retVec.head<3>() = C_ * rhs.head<3>() + r_ * s;
  retVec[3] = s;
  return retVec;
}

inline Transformation& Transformation::operator=(const Transformation & rhs) {
  parameters_ = rhs.parameters_;
  C_ = rhs.C_;
  r_ = Eigen::Map<Eigen::Vector3d>(&parameters_[0]);
  q_ = Eigen::Map<Eigen::Quaterniond>(&parameters_[3]);
  return *this;
}

inline void Transformation::updateC() {
  C_ = q_.toRotationMatrix();
}

// apply small update:
template<typename Derived_delta>
inline bool Transformation::oplus(
    const Eigen::MatrixBase<Derived_delta> & delta) {
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_delta, 6);
  r_ += delta.template head<3>();
  Eigen::Vector4d dq;
  double halfnorm = 0.5 * delta.template tail<3>().norm();
  dq.template head<3>() = sinc(halfnorm) * 0.5 * delta.template tail<3>();
  dq[3] = cos(halfnorm);
  q_ = (Eigen::Quaterniond(dq) * q_);
  q_.normalize();
  updateC();
  return true;
}

template<typename Derived_delta, typename Derived_jacobian>
inline bool Transformation::oplus(
    const Eigen::MatrixBase<Derived_delta> & delta,
    const Eigen::MatrixBase<Derived_jacobian> & jacobian) {
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_delta, 6);
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 7, 6);
  if (!oplus(delta)) {
    return false;
  }
  return oplusJacobian(jacobian);
}

template<typename Derived_jacobian>
inline bool Transformation::oplusJacobian(
    const Eigen::MatrixBase<Derived_jacobian> & jacobian) const {
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 7, 6);
  Eigen::Matrix<double, 4, 3> S = Eigen::Matrix<double, 4, 3>::Zero();
  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).setZero();
  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian)
      .template topLeftCorner<3, 3>().setIdentity();
  S(0, 0) = 0.5;
  S(1, 1) = 0.5;
  S(2, 2) = 0.5;
  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian)
      .template bottomRightCorner<4, 3>() = okvis::kinematics::oplus(q_) * S;
  return true;
}

template <typename Derived_jacobian>
inline bool Transformation::liftJacobian(const Eigen::MatrixBase<Derived_jacobian> & jacobian) const
{
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 6, 7);
  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).setZero();
  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).template topLeftCorner<3,3>()
      = Eigen::Matrix3d::Identity();
  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).template bottomRightCorner<3,4>()
      = 2*okvis::kinematics::oplus(q_.inverse()).template topLeftCorner<3,4>();
  return true;
}

}  // namespace kinematics
}  // namespace okvis