g2o::SE3Quat Class Reference

#include <se3quat.h>

Inheritance diagram for g2o::SE3Quat:

Public Member Functions
	SE3Quat ()
	SE3Quat (const Matrix3D &R, const Vector3D &t)
	SE3Quat (const Eigen::Quaterniond &q, const Vector3D &t)
template<typename Derived >
	SE3Quat (const Eigen::MatrixBase< Derived > &v)
const Vector3D &	translation () const
void	setTranslation (const Vector3D &t_)
const Eigen::Quaterniond &	rotation () const
void	setRotation (const Eigen::Quaterniond &r_)
SE3Quat	operator* (const SE3Quat &tr2) const
SE3Quat &	operator*= (const SE3Quat &tr2)
Vector3D	operator* (const Vector3D &v) const
SE3Quat	inverse () const
double	operator[] (int i) const
Vector7d	toVector () const
void	fromVector (const Vector7d &v)
Vector6d	toMinimalVector () const
void	fromMinimalVector (const Vector6d &v)
Vector6d	log () const
Vector3D	map (const Vector3D &xyz) const
Eigen::Matrix< double, 6, 6, Eigen::ColMajor >	adj () const
Eigen::Matrix< double, 4, 4, Eigen::ColMajor >	to_homogeneous_matrix () const
void	normalizeRotation ()
	operator Isometry3D () const

Static Public Member Functions
static SE3Quat	exp (const Vector6d &update)

Public Attributes
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Protected Attributes
Eigen::Quaterniond	_r
Vector3D	_t

Detailed Description

Definition at line 40 of file se3quat.h.

Constructor & Destructor Documentation

g2o::SE3Quat::SE3Quat ( )

inline

Definition at line 50 of file se3quat.h.

               {
        _r.setIdentity();
        _t.setZero();
      }

g2o::SE3Quat::SE3Quat ( const Matrix3D &  R, const Vector3D &  t  )

inline

Definition at line 55 of file se3quat.h.

                                                   :_r(Eigen::Quaterniond(R)),_t(t){ 
        normalizeRotation();
      }

g2o::SE3Quat::SE3Quat ( const Eigen::Quaterniond &  q, const Vector3D &  t  )

inline

Definition at line 59 of file se3quat.h.

                                                           :_r(q),_t(t){
        normalizeRotation();
      }

template<typename Derived >

g2o::SE3Quat::SE3Quat ( const Eigen::MatrixBase< Derived > &  v )

inlineexplicit

templaized constructor which allows v to be an arbitrary Eigen Vector type, e.g., Vector6d or Map<Vector6d>

Definition at line 67 of file se3quat.h.

        {
          assert((v.size() == 6 || v.size() == 7) && "Vector dimension does not match");
          if (v.size() == 6) {
            for (int i=0; i<3; i++){
              _t[i]=v[i];
              _r.coeffs()(i)=v[i+3];
            }
            _r.w() = 0.; // recover the positive w
            if (_r.norm()>1.){
              _r.normalize();
            } else {
              double 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)
              _t(i) = v(idx);
            for (int i=0; i<4; ++i, ++idx)
              _r.coeffs()(i) = v(idx);
            normalizeRotation();
          }
        }

Member Function Documentation

Eigen::Matrix<double, 6, 6, Eigen::ColMajor> g2o::SE3Quat::adj ( ) const

inline

Definition at line 256 of file se3quat.h.

References g2o::skew().

Referenced by g2o::EdgeSE3Expmap::linearizeOplus().

      {
        Matrix3D R = _r.toRotationMatrix();
        Eigen::Matrix<double, 6, 6, Eigen::ColMajor> res;
        res.block(0,0,3,3) = R;
        res.block(3,3,3,3) = R;
        res.block(3,0,3,3) = skew(_t)*R;
        res.block(0,3,3,3) = Matrix3D::Zero(3,3);
        return res;
      }

static SE3Quat g2o::SE3Quat::exp ( const Vector6d &  update )

inlinestatic

Definition at line 220 of file se3quat.h.

References g2o::skew().

Referenced by g2o::VertexSE3Expmap::oplusImpl().

      {
        Vector3D omega;
        for (int i=0; i<3; i++)
          omega[i]=update[i];
        Vector3D upsilon;
        for (int i=0; i<3; i++)
          upsilon[i]=update[i+3];

        double theta = omega.norm();
        Matrix3D Omega = skew(omega);

        Matrix3D R;
        Matrix3D V;
        if (theta<0.00001)
        {
          //TODO: CHECK WHETHER THIS IS CORRECT!!!
          R = (Matrix3D::Identity() + Omega + Omega*Omega);

          V = R;
        }
        else
        {
          Matrix3D Omega2 = Omega*Omega;

          R = (Matrix3D::Identity()
              + sin(theta)/theta *Omega
              + (1-cos(theta))/(theta*theta)*Omega2);

          V = (Matrix3D::Identity()
              + (1-cos(theta))/(theta*theta)*Omega
              + (theta-sin(theta))/(pow(theta,3))*Omega2);
        }
        return SE3Quat(Eigen::Quaterniond(R),V*upsilon);
      }

void g2o::SE3Quat::fromMinimalVector ( const Vector6d &  v )

inline

Definition at line 163 of file se3quat.h.

                                                      {
        double w = 1.-v[3]*v[3]-v[4]*v[4]-v[5]*v[5];
        if (w>0){
          _r=Eigen::Quaterniond(sqrt(w), v[3], v[4], v[5]);
        } else {
          _r=Eigen::Quaterniond(0, -v[3], -v[4], -v[5]);
        }
        _t=Vector3D(v[0], v[1], v[2]);
      }

void g2o::SE3Quat::fromVector ( const Vector7d &  v )

inline

Definition at line 147 of file se3quat.h.

Referenced by g2o::VertexSE3Expmap::read(), and g2o::EdgeSE3Expmap::read().

                                               {
        _r=Eigen::Quaterniond(v[6], v[3], v[4], v[5]);
        _t=Vector3D(v[0], v[1], v[2]);
      }

SE3Quat g2o::SE3Quat::inverse ( ) const

inline

Definition at line 120 of file se3quat.h.

References _r, and _t.

Referenced by g2o::deprecated::EdgeSE3::computeError(), g2o::EdgeSE3Expmap::computeError(), g2o::EdgeProjectPSI2UV::computeError(), g2o::EdgeSBACam::computeError(), g2o::deprecated::EdgeSE3Prior::initialEstimate(), g2o::deprecated::EdgeSE3Offset::initialEstimate(), g2o::EdgeSBAScale::initialEstimate(), g2o::deprecated::EdgeSE3::linearizeOplus(), g2o::EdgeSE3Expmap::linearizeOplus(), g2o::EdgeProjectPSI2UV::linearizeOplus(), main(), g2o::VertexSE3Expmap::read(), g2o::EdgeSE3Expmap::read(), g2o::deprecated::EdgeSE3Prior::setMeasurement(), g2o::deprecated::EdgeSE3Offset::setMeasurement(), g2o::deprecated::EdgeSE3::setMeasurement(), g2o::EdgeSBACam::setMeasurement(), g2o::deprecated::EdgeSE3::setMeasurementFromState(), g2o::EdgeSBACam::setMeasurementFromState(), and g2o::deprecated::CacheSE3Offset::updateImpl().

                                    {
        SE3Quat ret;
        ret._r=_r.conjugate();
        ret._t=ret._r*(_t*-1.);
        return ret;
      }

Vector6d g2o::SE3Quat::log ( ) const

inline

Definition at line 175 of file se3quat.h.

References g2o::deltaR(), and g2o::skew().

Referenced by g2o::EdgeSE3Expmap::computeError().

                           {
        Vector6d res;
        Matrix3D _R = _r.toRotationMatrix();
        double d =  0.5*(_R(0,0)+_R(1,1)+_R(2,2)-1);
        Vector3D omega;
        Vector3D upsilon;


        Vector3D dR = deltaR(_R);
        Matrix3D V_inv;

        if (d>0.99999)
        {

          omega=0.5*dR;
          Matrix3D Omega = skew(omega);
          V_inv = Matrix3D::Identity()- 0.5*Omega + (1./12.)*(Omega*Omega);
        }
        else
        {
          double theta = acos(d);
          omega = theta/(2*sqrt(1-d*d))*dR;
          Matrix3D Omega = skew(omega);
          V_inv = ( Matrix3D::Identity() - 0.5*Omega
              + ( 1-theta/(2*tan(theta/2)))/(theta*theta)*(Omega*Omega) );
        }

        upsilon = V_inv*_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;

      }

Vector3D g2o::SE3Quat::map ( const Vector3D &  xyz ) const

inline

Definition at line 214 of file se3quat.h.

Referenced by g2o::EdgeProjectXYZ2UV::computeError(), and g2o::EdgeProjectXYZ2UVU::computeError().

      {
        return _r*xyz + _t;
      }

void g2o::SE3Quat::normalizeRotation ( )

inline

Definition at line 277 of file se3quat.h.

Referenced by operator*().

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

g2o::SE3Quat::operator Isometry3D ( ) const

inline

cast SE3Quat into an Isometry3D

Definition at line 287 of file se3quat.h.

      {
        Isometry3D result = (Isometry3D) rotation();
        result.translation() = translation();
        return result;
      }

SE3Quat g2o::SE3Quat::operator* ( const SE3Quat &  tr2 ) const

inline

Definition at line 101 of file se3quat.h.

References _r, _t, and normalizeRotation().

                                                         {
        SE3Quat result(*this);
        result._t += _r*tr2._t;
        result._r*=tr2._r;
        result.normalizeRotation();
        return result;
      }

Vector3D g2o::SE3Quat::operator* ( const Vector3D &  v ) const

inline

Definition at line 116 of file se3quat.h.

                                                          {
        return _t+_r*v;
      }

SE3Quat& g2o::SE3Quat::operator*= ( const SE3Quat &  tr2 )

inline

Definition at line 109 of file se3quat.h.

References _r, and _t.

                                                     {
        _t+=_r*tr2._t;
        _r*=tr2._r;
        normalizeRotation();
        return *this;
      }

double g2o::SE3Quat::operator[] ( int  i ) const

inline

Definition at line 127 of file se3quat.h.

                                             {
        assert(i<7);
        if (i<3)
          return _t[i];
        return _r.coeffs()[i-3];
      }

const Eigen::Quaterniond& g2o::SE3Quat::rotation ( ) const

inline

Definition at line 97 of file se3quat.h.

Referenced by g2o::deprecated::EdgeSE3Prior::computeError(), g2o::deprecated::EdgeSE3Offset::computeError(), g2o::deprecated::EdgeSE3::computeError(), g2o::EdgeSBACam::computeError(), g2o::d_Tinvpsi_d_psi(), g2o::internal::fromSE3Quat(), g2o::deprecated::EdgeSE3Prior::initialEstimate(), g2o::deprecated::jac_quat3_euler3(), g2o::deprecated::EdgeSE3Prior::linearizeOplus(), g2o::deprecated::EdgeSE3Offset::linearizeOplus(), g2o::deprecated::EdgeSE3::linearizeOplus(), g2o::EdgeProjectPSI2UV::linearizeOplus(), main(), g2o::deprecated::VertexSE3WriteGnuplotAction::operator()(), g2o::deprecated::EdgeSE3WriteGnuplotAction::operator()(), g2o::operator<<(), g2o::deprecated::ParameterSE3Offset::setOffset(), g2o::deprecated::CacheSE3Offset::setOffsetParam(), g2o::deprecated::CacheSE3Offset::updateImpl(), and g2o::VertexCam::write().

{return _r;}

void g2o::SE3Quat::setRotation ( const Eigen::Quaterniond &  r_ )

inline

Definition at line 99 of file se3quat.h.

Referenced by g2o::deprecated::EdgeSE3Prior::initialEstimate().

{_r=r_;}

void g2o::SE3Quat::setTranslation ( const Vector3D &  t_ )

inline

Definition at line 95 of file se3quat.h.

Referenced by g2o::deprecated::EdgeSE3Prior::initialEstimate(), and g2o::EdgeSBAScale::initialEstimate().

{_t = t_;}

Eigen::Matrix<double,4,4,Eigen::ColMajor> g2o::SE3Quat::to_homogeneous_matrix ( ) const

inline

Definition at line 267 of file se3quat.h.

Referenced by g2o::operator<<().

      {
        Eigen::Matrix<double,4,4,Eigen::ColMajor> homogeneous_matrix;
        homogeneous_matrix.setIdentity();
        homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix();
        homogeneous_matrix.col(3).head(3) = translation();

        return homogeneous_matrix;
      }

Vector6d g2o::SE3Quat::toMinimalVector ( ) const

inline

Definition at line 152 of file se3quat.h.

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

Vector7d g2o::SE3Quat::toVector ( ) const

inline

Definition at line 135 of file se3quat.h.

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

const Vector3D& g2o::SE3Quat::translation ( ) const

inline

Definition at line 93 of file se3quat.h.

Referenced by g2o::deprecated::EdgeSE3Prior::computeError(), g2o::deprecated::EdgeSE3Offset::computeError(), g2o::deprecated::EdgeSE3::computeError(), g2o::EdgeSBACam::computeError(), g2o::EdgeSBAScale::computeError(), g2o::internal::fromSE3Quat(), g2o::deprecated::EdgeSE3Prior::initialEstimate(), g2o::deprecated::EdgeSE3PointXYZDisparity::initialEstimate(), g2o::EdgeSBAScale::initialEstimate(), g2o::deprecated::jac_quat3_euler3(), g2o::deprecated::EdgeSE3Prior::linearizeOplus(), g2o::deprecated::EdgeSE3Offset::linearizeOplus(), g2o::deprecated::EdgeSE3::linearizeOplus(), g2o::EdgeProjectP2MC::linearizeOplus(), g2o::EdgeProjectP2SC::linearizeOplus(), g2o::EdgeProjectP2MC_Intrinsics::linearizeOplus(), main(), g2o::deprecated::VertexSE3WriteGnuplotAction::operator()(), g2o::deprecated::EdgeSE3WriteGnuplotAction::operator()(), g2o::operator<<(), g2o::deprecated::CacheSE3Offset::setOffsetParam(), g2o::deprecated::CacheSE3Offset::updateImpl(), and g2o::VertexCam::write().

{return _t;}

Member Data Documentation

Eigen::Quaterniond g2o::SE3Quat::_r

protected

Definition at line 46 of file se3quat.h.

Referenced by inverse(), operator*(), and operator*=().

Vector3D g2o::SE3Quat::_t

protected

Definition at line 47 of file se3quat.h.

Referenced by inverse(), operator*(), and operator*=().

g2o::SE3Quat::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 42 of file se3quat.h.

---

The documentation for this class was generated from the following file:

• /home/xuezhisd/CLionProjects/g2o/g2o/types/slam3d/se3quat.h