Beispiel #1
0
 /// \brief Exp: so3 -> SO3.
 ///
 /// Return the integral of the input angular velocity during time 1.
 template <typename D> Eigen::Matrix<typename D::Scalar,3,3,D::Options>
 exp3(const Eigen::MatrixBase<D> & v)
 {
   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
   typename D::Scalar nv = v.norm();
   return Eigen::AngleAxis<typename D::Scalar>(nv, v / nv).matrix();
 }
Beispiel #2
0
typename T::Scalar norm(const Eigen::MatrixBase<T> &x) {
    return x.norm();
}