se3quat.h

// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// 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.
//
// 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
// HOLDER 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 G2O_SE3QUAT_H_
#define G2O_SE3QUAT_H_
 
#include "se3_ops.h"
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
namespace NDEVR {
  using namespace Eigen;
 
  typedef Eigen::Matrix<g_type, 6, 1> Vector6d; 
  typedef Eigen::Matrix<g_type, 7, 1> Vector7d; 

  class SE3Quat {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
 
    protected:
 
      Quaternion<g_type> _r;    
      Vector3<g_type> m_t;  
 
 
    public:
      SE3Quat(){
        _r.setIdentity();
        m_t.setZero();
      }

      SE3Quat(const Matrix3<g_type>& R, const Vector3<g_type>& t)
          : _r(Quaternion<g_type>(R))
          , m_t(t)
      {
        normalizeRotation();
      }

      SE3Quat(const Quaternion<g_type>& q, const Vector3<g_type>& t)
          : _r(q)
          , m_t(t)
      {
        normalizeRotation();
      }

      template <typename Derived>
        explicit SE3Quat(const MatrixBase<Derived>& v)
        {
          assert((v.size() == 6 || v.size() == 7) && "Vector dimension does not match");
          if (v.size() == 6) {
            for (int i=0; i<3; i++){
              m_t[i]=v[i];
              _r.coeffs()(i)=v[i+3];
            }
            _r.w() = 0.; // recover the positive w
            if (_r.norm()>1.){
              _r.normalize();
            } else {
              g_type w2=1.-_r.squaredNorm();
              _r.w()= (w2<0.) ? 0. : sqrt(w2);
            }
          }
          else if (v.size() == 7) {
            int idx = 0;
            for (int i=0; i<3; ++i, ++idx)
              m_t(i) = v(idx);
            for (int i=0; i<4; ++i, ++idx)
              _r.coeffs()(i) = v(idx);
            normalizeRotation();
          }
        }

      inline const Vector3<g_type>& translation() const {return m_t;}
      inline void setTranslation(const Vector3<g_type>& t_) {m_t = t_;}
      inline const Quaternion<g_type>& rotation() const {return _r;}
      void setRotation(const Quaternion<g_type>& r_) { _r = r_; }

      inline SE3Quat operator* (const SE3Quat& tr2) const{
        SE3Quat result(*this);
        result.m_t += _r*tr2.m_t;
        result._r*=tr2._r;
        result.normalizeRotation();
        return result;
      }

      inline SE3Quat& operator*= (const SE3Quat& tr2){
        m_t+=_r*tr2.m_t;
        _r*=tr2._r;
        normalizeRotation();
        return *this;
      }

      inline Vector3<g_type> operator* (const Vector3<g_type>& v) const {
        return m_t+_r*v;
      }

      inline SE3Quat inverse() const{
        SE3Quat ret;
        ret._r=_r.conjugate();
        ret.m_t=ret._r*(m_t*-1.);
        return ret;
      }

      inline g_type operator [](int i) const {
        assert(i<7);
        if (i<3)
          return m_t[i];
        return _r.coeffs()[i-3];
      }
 

      inline Vector7d toVector() const{
        Vector7d v;
        v[0]=m_t(0);
        v[1]=m_t(1);
        v[2]=m_t(2);
        v[3]=_r.x();
        v[4]=_r.y();
        v[5]=_r.z();
        v[6]=_r.w();
        return v;
      }

      inline void fromVector(const Vector7d& v){
        _r=Quaternion<g_type>(v[6], v[3], v[4], v[5]);
        m_t=Vector3<g_type>(v[0], v[1], v[2]);
      }

      inline Vector6d toMinimalVector() const{
        Vector6d v;
        v[0]=m_t(0);
        v[1]=m_t(1);
        v[2]=m_t(2);
        v[3]=_r.x();
        v[4]=_r.y();
        v[5]=_r.z();
        return v;
      }

      inline void fromMinimalVector(const Vector6d& v){
        g_type w = g_type(1.)-v[3]*v[3]-v[4]*v[4]-v[5]*v[5];
        if (w>0){
          _r=Quaternion<g_type>(sqrt(w), v[3], v[4], v[5]);
        } else {
          _r=Quaternion<g_type>(0, -v[3], -v[4], -v[5]);
        }
        m_t=Vector3<g_type>(v[0], v[1], v[2]);
      }
 
 

      Vector6d log() const {
        Vector6d res;
        Matrix3<g_type> _R = _r.toRotationMatrix();
        g_type d =  g_type(0.5)*(_R(0,0)+_R(1,1)+_R(2,2)-1);
        Vector3<g_type> omega;
        Vector3<g_type> upsilon;
 
 
        Vector3<g_type> dR = deltaR(_R);
        Matrix3<g_type> V_inv;
 
        if (d>0.99999)
        {
 
          omega=0.5*dR;
          Matrix3<g_type> Omega = skew(omega);
          V_inv = Matrix3<g_type>::Identity()- 0.5*Omega + (1./12.)*(Omega*Omega);
        }
        else
        {
          g_type d_clamped = (d < g_type(-1.0)) ? g_type(-1.0) : ((d > g_type(1.0)) ? g_type(1.0) : d);
          g_type theta = acos(d_clamped);
          g_type sin_theta = sqrt(g_type(1.0) - d_clamped * d_clamped);
          if (sin_theta < g_type(1e-10))
          {
            // Near 180-degree rotation: use limit approximation
            omega = g_type(PI<fltp08>() / 2.0) * dR.normalized();
            Matrix3<g_type> Omega = skew(omega);
            V_inv = Matrix3<g_type>::Identity() - g_type(0.5) * Omega + (g_type(1.0) / g_type(12.0)) * (Omega * Omega);
          }
          else
          {
            omega = theta / (g_type(2.0) * sin_theta) * dR;
            Matrix3<g_type> Omega = skew(omega);
            g_type half_theta = theta / g_type(2.0);
            V_inv = ( Matrix3<g_type>::Identity() - g_type(0.5) * Omega
                + ( g_type(1.0) - theta / (g_type(2.0) * tan(half_theta))) / (theta * theta) * (Omega * Omega) );
          }
        }
 
        upsilon = V_inv*m_t;
        for (int i=0; i<3;i++){
          res[i]=omega[i];
        }
        for (int i=0; i<3;i++){
          res[i+3]=upsilon[i];
        }
 
        return res;
 
      }

      Vector3<g_type> map(const Vector3<g_type>& xyz) const
      {
        return _r*xyz + m_t;
      }
 

      static SE3Quat exp(const Vector6d & update)
      {
        Vector3<g_type> omega;
        for (int i=0; i<3; i++)
          omega[i]=update[i];
        Vector3<g_type> upsilon;
        for (int i=0; i<3; i++)
          upsilon[i]=update[i+3];
 
        g_type theta = omega.norm();
        Matrix3<g_type> Omega = skew(omega);
 
        Matrix3<g_type> R;
        Matrix3<g_type> V;
        if (theta<0.00001f)
        {
          Matrix3<g_type> Omega2 = Omega*Omega;
          R = (Matrix3<g_type>::Identity() + Omega + g_type(0.5)*Omega2);
          V = (Matrix3<g_type>::Identity() + g_type(0.5)*Omega + g_type(1./6.)*Omega2);
        }
        else
        {
          Matrix3<g_type> Omega2 = Omega*Omega;
 
          R = (Matrix3<g_type>::Identity()
              + sin(theta)/theta *Omega
              + (1-cos(theta))/(theta*theta)*Omega2);
 
          V = (Matrix3<g_type>::Identity()
              + (1-cos(theta))/(theta*theta)*Omega
              + (theta-sin(theta))/(pow(theta,3))*Omega2);
        }
        return SE3Quat(Quaternion<g_type>(R),V*upsilon);
      }

      Eigen::Matrix<g_type, 6, 6> adj() const
      {
        Matrix3<g_type> R = _r.toRotationMatrix();
        Eigen::Matrix<g_type, 6, 6> res;
        res.block(0,0,3,3) = R;
        res.block(3,3,3,3) = R;
        res.block(3,0,3,3) = skew(m_t)*R;
        res.block(0,3,3,3) = Matrix3<g_type>::Zero(3,3);
        return res;
      }

      Eigen::Matrix<g_type,4,4> to_homogeneous_matrix() const
      {
        Eigen::Matrix<g_type,4,4> homogeneous_matrix;
        homogeneous_matrix.setIdentity();
        homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix();
        homogeneous_matrix.col(3).head(3) = translation();
 
        return homogeneous_matrix;
      }

      void normalizeRotation(){
        if (_r.w()<0){
          _r.coeffs() *= -1;
        }
        _r.normalize();
      }

      operator Eigen::Transform<g_type, 3, 1>() const
      {
        Eigen::Transform<g_type, 3, 1> result = (Eigen::Transform<g_type, 3, 1>)rotation();
        result.translation() = translation();
        return result;
      }
      public:
          EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
 
  inline std::ostream& operator <<(std::ostream& out_str, const SE3Quat& se3)
  {
    out_str << se3.to_homogeneous_matrix()  << std::endl;
    return out_str;
  }
 
} // end namespace
 
#endif