Commit e729e656 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux
Browse files

In exp3, modify assertion

  Previous implementation did not accept a vector of dynamic size of size 3.
  This commit fixes the issue.
parent b9279764
......@@ -40,7 +40,9 @@ namespace se3
template <typename D> Eigen::Matrix<typename D::Scalar,3,3,Eigen::internal::traits<D>::Options>
exp3(const Eigen::MatrixBase<D> & v)
assert (v.size () == 3);
typename D::Scalar nv = v.norm();
if (nv > 1e-14)
return Eigen::AngleAxis<typename D::Scalar>(nv, v / nv).matrix();
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment