Commit 40c08c62 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] log3 the shortest possible vector.

parent 1ae3842e
......@@ -412,8 +412,8 @@ namespace se3
// else : S = -Sp
// S / t = Sp / |t|
// S * S = - I2
// R = I2 + ( 1 - ct) / |t| * S + ( 1 - st / t ) * S * S
// = ( 1 - ct) / |t| * S + st / t * I2
// R = I2 + ( 1 - ct) / |t| * S + ( 1 - st / |t| ) * S * S
// = ( 1 - ct) / |t| * S + st / |t| * I2
//
// Ru = exp3 (w)
// tu = R * v = (1 - ct) / |t| * S * v + st / t * v
......@@ -493,7 +493,7 @@ namespace se3
TangentVector_t res;
res.head<2>() = nu.linear().head<2>();
res(2) = q_1(2) - q_0(2);
res(2) = nu.angular()(2);
return res;
}
......
......@@ -49,7 +49,7 @@ namespace se3
/// \brief Log: SO3 -> so3.
///
/// Pseudo-inverse of log from SO3 -> { v \in so3, ||v|| < 2pi }.
/// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
///
/// \param[in] R The rotation matrix.
///
......@@ -60,6 +60,9 @@ namespace se3
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D, 3, 3);
Eigen::AngleAxis<typename D::Scalar> angleAxis(R);
assert(0 <= angleAxis.angle() && angleAxis.angle() <= 2 * M_PI);
if (angleAxis.angle() > M_PI)
return -(2*M_PI - angleAxis.angle()) * angleAxis.axis();
return angleAxis.axis() * angleAxis.angle();
}
......
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