From e0fe051d2b2ab7b8be75085b8b5f10a725e7b2d4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 5 Nov 2016 11:37:40 +0100
Subject: [PATCH] Use small-angle approx instead of null
---
src/local_parametrization_homogeneous.cpp | 16 +++++++----
src/local_parametrization_quaternion.cpp | 35 ++++++++++++++---------
2 files changed, 31 insertions(+), 20 deletions(-)
diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp
index 6995b7f8c..62c6ea0aa 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 fe222dcaf..fddf08b73 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;
}
--
GitLab