diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp index 6995b7f8caaa626b36444d08e71d941d49aea8b6..62c6ea0aaff95d1eab6049ba96a0304a06068f9d 100644 --- a/src/local_parametrization_homogeneous.cpp +++ b/src/local_parametrization_homogeneous.cpp @@ -32,21 +32,25 @@ bool LocalParametrizationHomogeneous::plus(const Eigen::Map<const Eigen::VectorX using namespace Eigen; double angle = _delta.norm(); + Quaternions dq; if (angle > Constants::EPS_SMALL) { // compute rotation axis -- this guarantees unity norm Vector3s axis = _delta.normalized(); - // express delta as a quaternion -- this is exp(delta) - Quaternions dq(AngleAxis<Scalar>(angle, axis)); + // express delta as a quaternion using the angle-axis helper + dq = AngleAxis<Scalar>(angle, axis); - // result as a homogeneous point -- we use the quaternion product for keeping in the 4-sphere - _h_plus_delta = ( dq * Map<const Quaternions>( _h.data() ) ) .coeffs(); } - else + else // Consider small angle approx { - _h_plus_delta = _h; + dq.w() = 1; + dq.vec() = _delta/2; } + + // result as a homogeneous point -- we use the quaternion product for keeping in the 4-sphere + _h_plus_delta = (dq * Map<const Quaternions>(_h.data())).coeffs(); + return true; } diff --git a/src/local_parametrization_quaternion.cpp b/src/local_parametrization_quaternion.cpp index fe222dcaf52ac481ca0b1022442c7b88945e0ae1..fddf08b73acb33bc74a2e65c3d4d4a993500088e 100644 --- a/src/local_parametrization_quaternion.cpp +++ b/src/local_parametrization_quaternion.cpp @@ -1,5 +1,5 @@ #include "local_parametrization_quaternion.h" - +#include <iostream> namespace wolf { template <> @@ -17,23 +17,26 @@ bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::plus(const Eigen::Map<const using namespace Eigen; double angle = _delta_theta.norm(); + Quaternions dq; if (angle > Constants::EPS_SMALL) { // compute rotation axis -- this guarantees unity norm Vector3s axis = _delta_theta / angle; // express delta_theta as a quaternion using the angle-axis helper - Quaternions dq(AngleAxis<Scalar>(angle, axis)); - - // result as a quaternion - // the delta is in local reference: q * dq - _q_plus_delta_theta = (Map<const Quaternions>(_q.data()) * dq).coeffs(); + dq = AngleAxis<Scalar>(angle, axis); } - else // Consider null rotation + else // Consider small angle approx { - _q_plus_delta_theta = _q; + dq.w() = 1; + dq.vec() = _delta_theta/2; } + + // result as a quaternion + // the delta is in global reference: dq * q + _q_plus_delta_theta = (Map<const Quaternions>(_q.data()) * dq).coeffs(); + return true; } @@ -52,22 +55,26 @@ bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::plus(const Eigen::Map<cons using namespace Eigen; double angle = _delta_theta.norm(); + Quaternions dq; if (angle > Constants::EPS_SMALL) { // compute rotation axis -- this guarantees unity norm Vector3s axis = _delta_theta / angle; // express delta_theta as a quaternion using the angle-axis helper - Quaternions dq(AngleAxis<Scalar>(angle, axis)); + dq = AngleAxis<Scalar>(angle, axis); - // result as a quaternion - // the delta is in global reference: dq * q - _q_plus_delta_theta = (dq * Map<const Quaternions>(_q.data())).coeffs(); } - else // Consider null rotation + else // Consider small angle approx { - _q_plus_delta_theta = _q; + dq.w() = 1; + dq.vec() = _delta_theta/2; } + + // result as a quaternion + // the delta is in global reference: dq * q + _q_plus_delta_theta = (dq * Map<const Quaternions>(_q.data())).coeffs(); + return true; }