Skip to content
Snippets Groups Projects
Commit 8c691915 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Revert "Local parametrizations are header-only"

This reverts commit 39d1fcc0.
parent 39d1fcc0
No related branches found
No related tags found
No related merge requests found
Pipeline #5343 failed
...@@ -314,6 +314,9 @@ SET(SRCS_FRAME ...@@ -314,6 +314,9 @@ SET(SRCS_FRAME
) )
SET(SRCS_STATE_BLOCK SET(SRCS_STATE_BLOCK
src/state_block/has_state_blocks.cpp src/state_block/has_state_blocks.cpp
src/state_block/local_parametrization_base.cpp
src/state_block/local_parametrization_homogeneous.cpp
src/state_block/local_parametrization_quaternion.cpp
src/state_block/state_block.cpp src/state_block/state_block.cpp
) )
SET(SRCS_COMMON SET(SRCS_COMMON
......
...@@ -19,8 +19,8 @@ class LocalParametrizationBase{ ...@@ -19,8 +19,8 @@ class LocalParametrizationBase{
unsigned int global_size_; unsigned int global_size_;
unsigned int local_size_; unsigned int local_size_;
public: public:
LocalParametrizationBase (unsigned int LOCAL_PARAMETRIZATION_BASE_H_ _global_size, unsigned int _local_size); LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size);
virtual ~LocalParametrizationBase ( ) = default; virtual ~LocalParametrizationBase();
virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x, virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x,
Eigen::Map<const Eigen::VectorXd>& _delta, Eigen::Map<const Eigen::VectorXd>& _delta,
...@@ -30,27 +30,10 @@ class LocalParametrizationBase{ ...@@ -30,27 +30,10 @@ class LocalParametrizationBase{
Eigen::Map<const Eigen::VectorXd>& _x2, Eigen::Map<const Eigen::VectorXd>& _x2,
Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0; Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
unsigned int getLocalSize ( ) const; unsigned int getLocalSize() const;
unsigned int getGlobalSize ( ) const; unsigned int getGlobalSize() const;
}; };
inline LocalParametrizationBase::LocalParametrizationBase (unsigned int _global_size,
unsigned int _local_size)
: global_size_(_global_size),
local_size_(_local_size)
{
}
inline unsigned int LocalParametrizationBase::getLocalSize ( ) const
{
return local_size_;
}
inline unsigned int LocalParametrizationBase::getGlobalSize ( ) const
{
return global_size_;
}
} // namespace wolf } // namespace wolf
#endif /* LOCAL_PARAMETRIZATION_BASE_H_ */ #endif /* LOCAL_PARAMETRIZATION_BASE_H_ */
...@@ -37,75 +37,18 @@ namespace wolf { ...@@ -37,75 +37,18 @@ namespace wolf {
class LocalParametrizationHomogeneous : public LocalParametrizationBase class LocalParametrizationHomogeneous : public LocalParametrizationBase
{ {
public: public:
LocalParametrizationHomogeneous ( ); LocalParametrizationHomogeneous();
virtual ~LocalParametrizationHomogeneous() = default; virtual ~LocalParametrizationHomogeneous();
virtual bool plus (Eigen::Map<const Eigen::VectorXd>& _h, virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<const Eigen::VectorXd>& _delta, Eigen::Map<const Eigen::VectorXd>& _delta,
Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const; Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const;
virtual bool computeJacobian (Eigen::Map<const Eigen::VectorXd>& _h, virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const;
Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const; virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _h1,
virtual bool minus (Eigen::Map<const Eigen::VectorXd>& _h1, Eigen::Map<const Eigen::VectorXd>& _h2,
Eigen::Map<const Eigen::VectorXd>& _h2, Eigen::Map<Eigen::VectorXd>& _h2_minus_h1);
Eigen::Map<Eigen::VectorXd>& _h2_minus_h1);
}; };
}
//////////////// IMPLEMENTATION ////////////////
#include "core/math/rotations.h"
namespace wolf
{
inline LocalParametrizationHomogeneous::LocalParametrizationHomogeneous ( )
: LocalParametrizationBase(4, 3)
{
//
}
inline bool LocalParametrizationHomogeneous::plus (Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<const Eigen::VectorXd>& _delta,
Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const
{
assert(_h.size() == global_size_ && "Wrong size of input homogeneous point.");
assert(_delta.size() == local_size_ && "Wrong size of input delta.");
assert(_h_plus_delta.size() == global_size_ && "Wrong size of output homogeneous point.");
_h_plus_delta = (exp_q(_delta) * Eigen::Quaterniond(_h.data())).coeffs();
return true;
}
inline bool LocalParametrizationHomogeneous::computeJacobian (Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
{
assert(_h.size() == global_size_ && "Wrong size of input quaternion.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
Eigen::Vector4d hh = _h / 2;
_jacobian << hh(3), hh(2), -hh(1),
-hh(2), hh(3), hh(0),
hh(1), -hh(0), hh(3),
-hh(0), -hh(1), -hh(2);
return true;
}
inline bool LocalParametrizationHomogeneous::minus (Eigen::Map<const Eigen::VectorXd>& _h1,
Eigen::Map<const Eigen::VectorXd>& _h2,
Eigen::Map<Eigen::VectorXd>& _h2_minus_h1)
{
using Eigen::Quaterniond;
_h2_minus_h1 = log_q(Quaterniond(_h2.data()) * Quaterniond(_h1.data()).conjugate());
return true;
}
} // namespace wolf } // namespace wolf
#endif /* LOCALPARAMETRIZATIONHOMOGENEOUS_H_ */ #endif /* LOCALPARAMETRIZATIONHOMOGENEOUS_H_ */
...@@ -78,125 +78,6 @@ public: ...@@ -78,125 +78,6 @@ public:
typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal; typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal;
typedef LocalParametrizationQuaternion<DQ_LOCAL> LocalParametrizationQuaternionLocal; typedef LocalParametrizationQuaternion<DQ_LOCAL> LocalParametrizationQuaternionLocal;
}
///////////////////// IMPLEMENTATION //////////////////////
#include "core/math/rotations.h"
namespace wolf
{
////////// LOCAL PERTURBATION //////////////
template <>
inline bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<const Eigen::VectorXd>& _delta_theta,
Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
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;
_q_plus_delta_theta = ( Quaterniond(_q.data()) * exp_q(_delta_theta) ).coeffs();
return true;
}
template <>
inline bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<Eigen::MatrixRowXd>& _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.");
Eigen::Vector4d qq = _q/2;
_jacobian << qq(3), -qq(2), qq(1),
qq(2), qq(3), -qq(0),
-qq(1), qq(0), qq(3),
-qq(0), -qq(1), -qq(2) ;
return true;
}
template <>
inline bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1,
Eigen::Map<const Eigen::VectorXd>& _q2,
Eigen::Map<Eigen::VectorXd>& _q2_minus_q1)
{
assert(_q1.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2_minus_q1.size() == local_size_ && "Wrong size of output quaternion difference.");
using Eigen::Quaterniond;
_q2_minus_q1 = log_q(Quaterniond(_q1.data()).conjugate() * Quaterniond(_q2.data()));
return true;
}
////////// GLOBAL PERTURBATION //////////////
template <>
inline bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<const Eigen::VectorXd>& _delta_theta,
Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
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;
_q_plus_delta_theta = ( exp_q(_delta_theta) * Quaterniond(_q.data()) ).coeffs();
return true;
}
template <>
inline bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<Eigen::MatrixRowXd>& _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.");
Eigen::Vector4d qq = _q/2;
_jacobian << qq(3), qq(2), -qq(1),
-qq(2), qq(3), qq(0),
qq(1), -qq(0), qq(3),
-qq(0), -qq(1), -qq(2);
return true;
}
template <>
inline bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1,
Eigen::Map<const Eigen::VectorXd>& _q2,
Eigen::Map<Eigen::VectorXd>& _q2_minus_q1)
{
assert(_q1.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2_minus_q1.size() == local_size_ && "Wrong size of output quaternion difference.");
using Eigen::Quaterniond;
_q2_minus_q1 = log_q(Quaterniond(_q2.data()) * Quaterniond(_q1.data()).conjugate());
return true;
}
} // namespace wolf } // namespace wolf
#endif /* LOCAL_PARAMETRIZATION_QUATERNION_H_ */ #endif /* LOCAL_PARAMETRIZATION_QUATERNION_H_ */
#include "core/state_block/local_parametrization_base.h"
namespace wolf {
LocalParametrizationBase::LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size) :
global_size_(_global_size), local_size_(_local_size)
{
}
LocalParametrizationBase::~LocalParametrizationBase()
{
}
unsigned int LocalParametrizationBase::getLocalSize() const
{
return local_size_;
}
unsigned int LocalParametrizationBase::getGlobalSize() const
{
return global_size_;
}
} // namespace wolf
/*
* \file local_parametrization_homogeneous.cpp
*
* Created on: 24/02/2016
* Author: jsola
*/
#include "core/state_block/local_parametrization_homogeneous.h"
#include "iostream"
#include "core/math/rotations.h" // we use quaternion algebra here
namespace wolf {
LocalParametrizationHomogeneous::LocalParametrizationHomogeneous() :
LocalParametrizationBase(4, 3)
{
//
}
LocalParametrizationHomogeneous::~LocalParametrizationHomogeneous()
{
//
}
bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<const Eigen::VectorXd>& _delta,
Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const
{
assert(_h.size() == global_size_ && "Wrong size of input homogeneous point.");
assert(_delta.size() == local_size_ && "Wrong size of input delta.");
assert(_h_plus_delta.size() == global_size_ && "Wrong size of output homogeneous point.");
using namespace Eigen;
_h_plus_delta = ( exp_q(_delta) * Quaterniond(_h.data()) ).coeffs();
return true;
}
bool LocalParametrizationHomogeneous::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
{
assert(_h.size() == global_size_ && "Wrong size of input quaternion.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
Eigen::Vector4d hh = _h/2;
_jacobian << hh(3), hh(2), -hh(1),
-hh(2), hh(3), hh(0),
hh(1), -hh(0), hh(3),
-hh(0), -hh(1), -hh(2) ;
return true;
}
bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXd>& _h1,
Eigen::Map<const Eigen::VectorXd>& _h2,
Eigen::Map<Eigen::VectorXd>& _h2_minus_h1)
{
using Eigen::Quaterniond;
_h2_minus_h1 = log_q(Quaterniond(_h2.data()) * Quaterniond(_h1.data()).conjugate());
return true;
}
} // namespace wolf
#include "core/state_block/local_parametrization_quaternion.h"
#include "core/math/rotations.h"
#include <iostream>
namespace wolf {
////////// LOCAL PERTURBATION //////////////
template <>
bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<const Eigen::VectorXd>& _delta_theta,
Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
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;
_q_plus_delta_theta = ( Quaterniond(_q.data()) * exp_q(_delta_theta) ).coeffs();
return true;
}
template <>
bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<Eigen::MatrixRowXd>& _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.");
Eigen::Vector4d qq = _q/2;
_jacobian << qq(3), -qq(2), qq(1),
qq(2), qq(3), -qq(0),
-qq(1), qq(0), qq(3),
-qq(0), -qq(1), -qq(2) ;
return true;
}
template <>
bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1,
Eigen::Map<const Eigen::VectorXd>& _q2,
Eigen::Map<Eigen::VectorXd>& _q2_minus_q1)
{
assert(_q1.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2_minus_q1.size() == local_size_ && "Wrong size of output quaternion difference.");
using Eigen::Quaterniond;
_q2_minus_q1 = log_q(Quaterniond(_q1.data()).conjugate() * Quaterniond(_q2.data()));
return true;
}
////////// GLOBAL PERTURBATION //////////////
template <>
bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<const Eigen::VectorXd>& _delta_theta,
Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
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;
_q_plus_delta_theta = ( exp_q(_delta_theta) * Quaterniond(_q.data()) ).coeffs();
return true;
}
template <>
bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<Eigen::MatrixRowXd>& _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.");
Eigen::Vector4d qq = _q/2;
_jacobian << qq(3), qq(2), -qq(1),
-qq(2), qq(3), qq(0),
qq(1), -qq(0), qq(3),
-qq(0), -qq(1), -qq(2);
return true;
}
template <>
bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1,
Eigen::Map<const Eigen::VectorXd>& _q2,
Eigen::Map<Eigen::VectorXd>& _q2_minus_q1)
{
assert(_q1.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2_minus_q1.size() == local_size_ && "Wrong size of output quaternion difference.");
using Eigen::Quaterniond;
_q2_minus_q1 = log_q(Quaterniond(_q2.data()) * Quaterniond(_q1.data()).conjugate());
return true;
}
} // namespace wolf
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment