Commit aa6b5178 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[Minor][C++] Clean code and fix conversion issue.

parent b6c3684b
...@@ -94,7 +94,7 @@ namespace se3 ...@@ -94,7 +94,7 @@ namespace se3
const Scalar alpha = ((Scalar)3 - N2) / 2; const Scalar alpha = ((Scalar)3 - N2) / 2;
const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() *= alpha; const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() *= alpha;
#ifndef NDEBUG #ifndef NDEBUG
const Scalar M = 3 * std::pow((Scalar)1-epsilon, (Scalar)-5/2) / 4; const Scalar M = 3 * std::pow((Scalar)1.-epsilon, ((Scalar)-5)/2) / 4;
assert(std::fabs(q.norm() - 1) <= assert(std::fabs(q.norm() - 1) <=
std::max(M * sqrt(N2) * (N2 - 1)*(N2 - 1) / 2, Eigen::NumTraits<Scalar>::epsilon())); std::max(M * sqrt(N2) * (N2 - 1)*(N2 - 1) / 2, Eigen::NumTraits<Scalar>::epsilon()));
#endif #endif
...@@ -104,8 +104,8 @@ namespace se3 ...@@ -104,8 +104,8 @@ namespace se3
template <typename D> template <typename D>
void uniformRandom (const Eigen::QuaternionBase<D> & q) void uniformRandom (const Eigen::QuaternionBase<D> & q)
{ {
// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D, 4);
typedef typename D::Scalar Scalar; typedef typename D::Scalar Scalar;
typedef Eigen::QuaternionBase<D> Base;
// Rotational part // Rotational part
const Scalar u1 = (Scalar)rand() / RAND_MAX; const Scalar u1 = (Scalar)rand() / RAND_MAX;
...@@ -118,10 +118,10 @@ namespace se3 ...@@ -118,10 +118,10 @@ namespace se3
Scalar s2,c2; SINCOS(2.*PI*u2,&s2,&c2); Scalar s2,c2; SINCOS(2.*PI*u2,&s2,&c2);
Scalar s3,c3; SINCOS(2.*PI*u3,&s3,&c3); Scalar s3,c3; SINCOS(2.*PI*u3,&s3,&c3);
const_cast <Eigen::QuaternionBase<D> &> (q).w() = mult1 * s2; const_cast <Base &> (q).w() = mult1 * s2;
const_cast <Eigen::QuaternionBase<D> &> (q).x() = mult1 * c2; const_cast <Base &> (q).x() = mult1 * c2;
const_cast <Eigen::QuaternionBase<D> &> (q).y() = mult2 * s3; const_cast <Base &> (q).y() = mult2 * s3;
const_cast <Eigen::QuaternionBase<D> &> (q).z() = mult2 * c3; const_cast <Base &> (q).z() = mult2 * c3;
} }
} }
#endif //#ifndef __math_quaternion_hpp__ #endif //#ifndef __math_quaternion_hpp__
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