diff --git a/src/examples/test_local_param.cpp b/src/examples/test_local_param.cpp
index d355cb0193815aa4f927c55c475f6f35bc1ab657..0e34b0182cfac9b5b7ba2c77a0e8f8f160f8cf62 100644
--- a/src/examples/test_local_param.cpp
+++ b/src/examples/test_local_param.cpp
@@ -37,8 +37,8 @@ int main(){
cout << "q = " << q.transpose() << " with norm = " << q.norm() << "\nda = " << da.transpose() << endl;
cout << "qo = " << qo.transpose() << " with norm = " << qo.norm() << endl;
- LocalParametrizationQuaternion Qpar;
- LocalParametrizationQuaternion Qpar_loc(DQ_LOCAL);
+ LocalParametrizationQuaternion<DQ_GLOBAL> Qpar;
+ LocalParametrizationQuaternion<DQ_LOCAL> Qpar_loc;
cout << "\nGLOBAL D_QUAT plus()" << endl;
Map<const VectorXs> q_const(q.data(),4);
diff --git a/src/local_parametrization_quaternion.cpp b/src/local_parametrization_quaternion.cpp
index dd9c22ec2b7fea541c7660f5340b7f51928f14be..a0364f8daf918814cc97841fa9f9bd5da972d079 100644
--- a/src/local_parametrization_quaternion.cpp
+++ b/src/local_parametrization_quaternion.cpp
@@ -2,19 +2,10 @@
namespace wolf {
-LocalParametrizationQuaternion::LocalParametrizationQuaternion(QuaternionDeltaReference _delta_ref) :
- LocalParametrizationBase(4, 3),
- delta_reference_(_delta_ref)
-{
-}
-
-LocalParametrizationQuaternion::~LocalParametrizationQuaternion()
-{
-}
-
-bool LocalParametrizationQuaternion::plus(const Eigen::Map<const Eigen::VectorXs>& _q,
- const Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
+template <>
+bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::plus(const Eigen::Map<const Eigen::VectorXs>& _q,
+ const Eigen::Map<const Eigen::VectorXs>& _delta_theta,
+ Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
@@ -35,12 +26,8 @@ bool LocalParametrizationQuaternion::plus(const Eigen::Map<const Eigen::VectorXs
Quaternions dq(AngleAxis<Scalar>(angle, axis));
// result as a quaternion
- if (delta_reference_ == DQ_GLOBAL)
- // the delta is in global reference: dq * q
- _q_plus_delta_theta = (dq * Map<const Quaternions>(_q.data())).coeffs();
- else
- // the delta is in local reference: q * dq
- _q_plus_delta_theta = (Map<const Quaternions>(_q.data()) * dq).coeffs();
+ // the delta is in local reference: q * dq
+ _q_plus_delta_theta = (Map<const Quaternions>(_q.data()) * dq).coeffs();
}
else // Consider null rotation
@@ -50,30 +37,72 @@ bool LocalParametrizationQuaternion::plus(const Eigen::Map<const Eigen::VectorXs
return true;
}
-bool LocalParametrizationQuaternion::computeJacobian(const Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+template <>
+bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::plus(const Eigen::Map<const Eigen::VectorXs>& _q,
+ const Eigen::Map<const Eigen::VectorXs>& _delta_theta,
+ Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
{
+
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
- assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
+ assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta.");
+ assert(_q_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion.");
+
+ assert(fabs(1.0 - _q.norm()) < Constants::EPS && "Quaternion not normalized.");
using namespace Eigen;
- if (delta_reference_ == DQ_GLOBAL) // See comments in method plus()
+
+ double angle = _delta_theta.norm();
+ if (angle > Constants::EPS)
{
- _jacobian << -_q(0), -_q(1), -_q(2),
- _q(3), _q(2), -_q(1),
- -_q(2), _q(3), _q(0),
- _q(1), -_q(0), _q(3);
- _jacobian /= 2;
+ // 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 global reference: dq * q
+ _q_plus_delta_theta = (dq * Map<const Quaternions>(_q.data())).coeffs();
}
- else
+ else // Consider null rotation
{
- _jacobian << -_q(0), -_q(1), -_q(2),
- _q(3), -_q(2), _q(1),
- _q(2), _q(3), -_q(0),
- -_q(1), _q(0), _q(3);
- _jacobian /= 2;
+ _q_plus_delta_theta = _q;
}
return true;
}
+template <>
+bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::computeJacobian(const Eigen::Map<const Eigen::VectorXs>& _q,
+ Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+{
+ assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
+ assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
+
+ using namespace Eigen;
+ _jacobian << -_q(0), -_q(1), -_q(2),
+ _q(3), -_q(2), _q(1),
+ _q(2), _q(3), -_q(0),
+ -_q(1), _q(0), _q(3);
+ _jacobian /= 2;
+
+ return true;
+}
+
+template <>
+bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::computeJacobian(const Eigen::Map<const Eigen::VectorXs>& _q,
+ Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+{
+ assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
+ assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
+
+ using namespace Eigen;
+ _jacobian << -_q(0), -_q(1), -_q(2),
+ _q(3), _q(2), -_q(1),
+ -_q(2), _q(3), _q(0),
+ _q(1), -_q(0), _q(3);
+ _jacobian /= 2;
+
+ return true;
+}
+
} // namespace wolf
diff --git a/src/local_parametrization_quaternion.h b/src/local_parametrization_quaternion.h
index f3c73f895131958bb51dafa2b7f9cb4d502475a8..5401ba748e095cccb66da57e18495b50574298f1 100644
--- a/src/local_parametrization_quaternion.h
+++ b/src/local_parametrization_quaternion.h
@@ -48,20 +48,38 @@ typedef enum {
* \f]
*
*/
+template <unsigned int DeltaReference/* = DQ_GLOBAL*/>
class LocalParametrizationQuaternion : public LocalParametrizationBase
{
- private:
- QuaternionDeltaReference delta_reference_;
- public:
- LocalParametrizationQuaternion(QuaternionDeltaReference _delta_ref = DQ_GLOBAL);
- virtual ~LocalParametrizationQuaternion();
+protected:
- virtual bool plus(const Eigen::Map<const Eigen::VectorXs>& _q,
- const Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const;
- virtual bool computeJacobian(const Eigen::Map<const Eigen::VectorXs>& _q, Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
+ QuaternionDeltaReference delta_reference_;
+
+public:
+
+ LocalParametrizationQuaternion() :
+ LocalParametrizationBase(4, 3)
+ {
+ //
+ }
+
+ virtual ~LocalParametrizationQuaternion()
+ {
+ //
+ }
+
+ virtual bool plus(const Eigen::Map<const Eigen::VectorXs>& _q,
+ const Eigen::Map<const Eigen::VectorXs>& _delta_theta,
+ Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const;
+
+ virtual bool computeJacobian(const Eigen::Map<const Eigen::VectorXs>& _q,
+ Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
};
+// @TODO : considere typedef
+// typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionG;
+// typedef LocalParametrizationQuaternion<DQ_LOCAL> LocalParametrizationQuaternionL;
+
} // namespace wolf
#endif /* LOCAL_PARAMETRIZATION_QUATERNION_H_ */
diff --git a/src/state_quaternion.h b/src/state_quaternion.h
index 094b10e167774f98ef59b454df9ccdd3c0b7e841..76f7843232266c712b46dbc3bd62fd4d8a46b838 100644
--- a/src/state_quaternion.h
+++ b/src/state_quaternion.h
@@ -28,13 +28,13 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaternions _quaternion, bo
}
inline StateQuaternion::StateQuaternion(const Eigen::VectorXs _state, bool _fixed) :
- StateBlock(_state, _fixed, new LocalParametrizationQuaternion)
+ StateBlock(_state, _fixed, new LocalParametrizationQuaternion<DQ_GLOBAL>)
{
assert(_state.size() == 4 && "The quaternion state vector must be of size 4");
}
inline StateQuaternion::StateQuaternion(bool _fixed) :
- StateBlock(4, _fixed, new LocalParametrizationQuaternion)
+ StateBlock(4, _fixed, new LocalParametrizationQuaternion<DQ_GLOBAL>)
{
state_ = Eigen::Quaternions::Identity().coeffs();
//