/// \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();
}
typename T::Scalar norm(const Eigen::MatrixBase<T> &x) {
return x.norm();
}