RotationMatrix2QuaternionManifold Struct Reference

Functor used to compute the Jacobian via AD. More...

Public Member Functions
template<typename T >
bool	operator() (const T *rotMatSerialized, T *quaternion) const

Detailed Description

Functor used to compute the Jacobian via AD.

Definition at line 43 of file test_mat2quat_jacobian.cpp.

Member Function Documentation

template<typename T >

bool RotationMatrix2QuaternionManifold::operator() ( const T *  rotMatSerialized, T *  quaternion  ) const

inline

Definition at line 46 of file test_mat2quat_jacobian.cpp.

  {
    typename Eigen::Matrix<T, 3, 3, Eigen::ColMajor>::ConstMapType R(rotMatSerialized);

    T t = R.trace();
    if (t > T(0)) {
      cerr << "w";
      t = sqrt(t + T(1));
      //T w = T(0.5)*t;
      t = T(0.5)/t;
      quaternion[0] = (R(2,1) - R(1,2)) * t;
      quaternion[1] = (R(0,2) - R(2,0)) * t;
      quaternion[2] = (R(1,0) - R(0,1)) * t;
    } else {
      int i = 0;
      if (R(1,1) > R(0,0))
        i = 1;
      if (R(2,2) > R(i,i))
        i = 2;
      int j = (i+1)%3;
      int k = (j+1)%3;
      cerr << i;

      t = sqrt(R(i,i) - R(j,j) - R(k,k) + T(1.0));
      quaternion[i] = T(0.5) * t;
      t = T(0.5)/t;
      quaternion[j] = (R(j,i) + R(i,j)) * t;
      quaternion[k] = (R(k,i) + R(i,k)) * t;
      T w = (R(k,j) - R(j,k)) * t;
      // normalize to our manifold, such that w is positive
      if (w < 0.) {
        //cerr << "  normalizing w > 0  ";
        for (int l = 0; l < 3; ++l)
          quaternion[l] *= T(-1);
      }
    }
    return true;
  }

---

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

• /home/xuezhisd/CLionProjects/g2o/g2o/types/slam3d/test_mat2quat_jacobian.cpp