Skip to content
Snippets Groups Projects
Commit c2f650c3 authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

plus diff jacobians WIP, weird compilation error on plus

parent 5f72621e
No related branches found
No related tags found
No related merge requests found
...@@ -45,6 +45,89 @@ namespace wolf ...@@ -45,6 +45,89 @@ namespace wolf
namespace imu_with_dt { namespace imu_with_dt {
using namespace Eigen; using namespace Eigen;
template<typename D>
inline Matrix<typename D::Scalar, 5, 5> hat(const MatrixBase<D>& tau)
{
// tagent space element vector form to matrix form
MatrixSizeCheck<10,1>::check(tau);
typedef typename D::Scalar T;
Map<const Matrix<T, 3, 1> > dp ( & tau( 0 ) );
Map<const Matrix<T, 3, 1> > doo( & tau( 3 ) );
Map<const Matrix<T, 3, 1> > dv ( & tau( 6 ) );
T& dt = tau( 9 );
Matrix<T, 5, 5> ret;
ret.setZero();
ret.block(0,0,3,3) = skew(doo);
ret.block(0,3,3,1) = dv;
ret.block(0,4,3,1) = dp;
ret.block(1,4,3,1) = dt;
return ret;
}
template<typename D1, typename D2>
inline void adjoint(const MatrixBase<D1>& delta, MatrixBase<D2>& adjd)
{
MatrixSizeCheck<11, 1>::check(delta);
MatrixSizeCheck<10, 10>::check(adjd);
Map<const Matrix<typename D1::Scalar, 3, 1> > deltap ( & delta( 0 ) );
Map<const Quaternion<typename D1::Scalar> > deltaq ( & delta(3) );
Matrix<typename D1::Scalar, 3, 3> deltaR = q2R(deltaq);
Map<const Matrix<typename D1::Scalar, 3, 1> > deltav ( & delta( 7 ) );
const typename D1::Scalar& deltat = delta(10);
// pqvt impl (in paper: pvqt)
adjd.setIdentity();
adjd.block(0,0,3,3) = deltaR;
adjd.block(0,3,3,3) = skew(deltap - deltav * deltat) * deltaR;
adjd.block(0,6,3,3) = -deltaR * deltat;
adjd.block(0,9,3,1) = deltav;
adjd.block(3,3,3,3) = deltaR;
adjd.block(6,3,3,3) = skew(deltav) * deltaR;
adjd.block(6,6,3,3) = deltaR;
}
template<typename D>
inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){
Matrix<typename D::Scalar, 10, 10> adjd;
adjoint(delta, adjd);
return adjd;
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 11, 1> smalladjoint(const MatrixBase<D1> d, MatrixBase<D1> sadjd)
{
MatrixSizeCheck<10, 1>::check(d);
MatrixSizeCheck<10, 10>::check(sadjd);
Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
Map<const Quaternion<typename D1::Scalar> > doo ( &d( 3 ) );
Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( & d( 6 ) );
const typename D1::Scalar& dt = d( 9 );
const Matrix<typename D1::Scalar, 3, 3> Id3 = Matrix<typename D1::Scalar, 3, 3>::identity(3,3);
const Matrix<typename D1::Scalar, 3, 3> dox = skew(doo);
// pqvt impl (in paper: pvqt)
sadjd.setIdentity();
sadjd.block(0,0,3,3) = dox;
sadjd.block(0,3,3,3) = skew(dp);
sadjd.block(0,6,3,3) = -Id3*dt;
sadjd.block(0,9,3,1) = dv;
sadjd.block(3,3,3,3) = dox;
sadjd.block(6,3,3,3) = skew(dv);
sadjd.block(6,6,3,3) = dox;
return sadjd;
}
template<typename D1, typename D2, typename D3, typename T> template<typename D1, typename D2, typename D3, typename T>
inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q, MatrixBase<D3>& v, T& dt) inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q, MatrixBase<D3>& v, T& dt)
{ {
...@@ -182,65 +265,6 @@ inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& delta1, ...@@ -182,65 +265,6 @@ inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& delta1,
return ret; return ret;
} }
template<typename D1, typename D2>
inline void adjoint(const MatrixBase<D1>& delta, MatrixBase<D2>& adjd)
{
MatrixSizeCheck<11, 1>::check(delta);
MatrixSizeCheck<10, 10>::check(adjd);
Map<const Matrix<typename D1::Scalar, 3, 1> > deltap ( & delta( 0 ) );
Map<const Quaternion<typename D1::Scalar> > deltaq ( & delta(3) );
Matrix<typename D1::Scalar, 3, 3> deltaR = q2R(deltaq);
Map<const Matrix<typename D1::Scalar, 3, 1> > deltav ( & delta( 7 ) );
const typename D1::Scalar& deltat = delta(10);
// pqvt impl (in paper: pvqt)
adjd.setIdentity();
adjd.block(0,0,3,3) = deltaR;
adjd.block(0,3,3,3) = skew(deltap - deltav * deltat) * deltaR;
adjd.block(0,6,3,3) = -deltaR * deltat;
adjd.block(0,9,3,1) = deltav;
adjd.block(3,3,3,3) = deltaR;
adjd.block(6,3,3,3) = skew(deltav) * deltaR;
adjd.block(6,6,3,3) = deltaR;
}
template<typename D>
inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){
Matrix<typename D::Scalar, 10, 10> adjd;
adjoint(delta, adjd);
return adjd;
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 11, 1> smalladjoint(const MatrixBase<D1> d, MatrixBase<D1> sadjd)
{
MatrixSizeCheck<10, 1>::check(d);
MatrixSizeCheck<10, 10>::check(sadjd);
Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
Map<const Quaternion<typename D1::Scalar> > doo ( &d( 3 ) );
Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( & d( 6 ) );
const typename D1::Scalar& dt = d( 9 );
const Matrix<typename D1::Scalar, 3, 3> Id3 = Matrix<typename D1::Scalar, 3, 3>::identity(3,3);
const Matrix<typename D1::Scalar, 3, 3> dox = skew(doo);
// pqvt impl (in paper: pvqt)
sadjd.setIdentity();
sadjd.block(0,0,3,3) = dox;
sadjd.block(0,3,3,3) = skew(dp);
sadjd.block(0,6,3,3) = -Id3*dt;
sadjd.block(0,9,3,1) = dv;
sadjd.block(3,3,3,3) = dox;
sadjd.block(6,3,3,3) = skew(dv);
sadjd.block(6,6,3,3) = dox;
return sadjd;
}
template<typename D1, typename D2, typename D3, typename D4, typename D5> template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const MatrixBase<D1>& delta1, inline void compose(const MatrixBase<D1>& delta1,
const MatrixBase<D2>& delta2, const MatrixBase<D2>& delta2,
...@@ -465,6 +489,7 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in) ...@@ -465,6 +489,7 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in)
Matrix<T, 11, 1> ret; Matrix<T, 11, 1> ret;
// TODO: solve strange compilation error
Map<const Matrix<T, 3, 1> > dp_in ( & d_in(0) ); Map<const Matrix<T, 3, 1> > dp_in ( & d_in(0) );
Map<const Matrix<T, 3, 1> > do_in ( & d_in(3) ); Map<const Matrix<T, 3, 1> > do_in ( & d_in(3) );
Map<const Matrix<T, 3, 1> > dv_in ( & d_in(6) ); Map<const Matrix<T, 3, 1> > dv_in ( & d_in(6) );
......
...@@ -153,7 +153,7 @@ TEST(IMU_tools, lift_retract) ...@@ -153,7 +153,7 @@ TEST(IMU_tools, lift_retract)
ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10); ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
} }
TEST(IMU_tools, plus_minus) TEST(IMU_tools, plus_diff)
{ {
VectorXs delta1(11), delta2(11), delta2_eq(11); VectorXs delta1(11), delta2(11), delta2_eq(11);
VectorXs err(10), err_0(10); VectorXs err(10), err_0(10);
...@@ -174,10 +174,24 @@ TEST(IMU_tools, adjoint) ...@@ -174,10 +174,24 @@ TEST(IMU_tools, adjoint)
delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1; delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1;
delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3; delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3;
// From the definition
// From direct properties
ASSERT_MATRIX_APPROX(adjoint(delta1).inverse(), adjoint(inverse(delta1)), 1e-10); ASSERT_MATRIX_APPROX(adjoint(delta1).inverse(), adjoint(inverse(delta1)), 1e-10);
ASSERT_MATRIX_APPROX(adjoint(compose(delta1, delta2)), adjoint(delta1)*adjoint(delta2), 1e-10); ASSERT_MATRIX_APPROX(adjoint(compose(delta1, delta2)), adjoint(delta1)*adjoint(delta2), 1e-10);
} }
TEST(IMU_tools, smalladjoint)
{
VectorXs delta1(11), delta2(11);
Vector4s qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
Vector4s qv2 = (Vector4s() << 6, 5, 4, 3).finished().normalized();
delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1;
delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3;
// From the definition
}
// TEST(IMU_tools, plus) // TEST(IMU_tools, plus)
// { // {
...@@ -218,9 +232,6 @@ TEST(IMU_tools, adjoint) ...@@ -218,9 +232,6 @@ TEST(IMU_tools, adjoint)
// ASSERT_MATRIX_APPROX(err, diff(delta1, delta3), 1e-10); // ASSERT_MATRIX_APPROX(err, diff(delta1, delta3), 1e-10);
// } // }
TEST(IMU_tools, compose_jacobians) TEST(IMU_tools, compose_jacobians)
{ {
VectorXs delta1(11), delta2(11), delta3(11); // deltas VectorXs delta1(11), delta2(11), delta3(11); // deltas
...@@ -238,6 +249,19 @@ TEST(IMU_tools, compose_jacobians) ...@@ -238,6 +249,19 @@ TEST(IMU_tools, compose_jacobians)
// analytical jacobians // analytical jacobians
compose(delta1, delta2, delta3, J_comp_delta1, J_comp_delta2); compose(delta1, delta2, delta3, J_comp_delta1, J_comp_delta2);
J_comp_delta1.setIdentity();
J_comp_delta2.setIdentity();
// std::cout << plus(delta2, tau) << std::endl;
auto tautau = J_comp_delta1*tau;
std::cout << tau << std::endl;
std::cout << tautau << std::endl;
std::cout << plus(delta2, tau) << std::endl; // No compilation error
// std::cout << plus(delta2, tautau) << std::endl; // COMPILATION ERROR -> TODO
// std::cout << plus(delta1, J_comp_delta1*tau) << std::endl;
// That first order approximation holds TODO -> comp problem! // That first order approximation holds TODO -> comp problem!
// ASSERT_MATRIX_APPROX(compose(plus(delta1, tau), delta2), plus(compose(delta1, delta2), J_comp_delta1*tau), 1e-4); // ASSERT_MATRIX_APPROX(compose(plus(delta1, tau), delta2), plus(compose(delta1, delta2), J_comp_delta1*tau), 1e-4);
// ASSERT_MATRIX_APPROX(compose(delta1, plus(delta2, tau)), plus(compose(delta1, delta2), J_comp_delta2*tau), 1e-4); // ASSERT_MATRIX_APPROX(compose(delta1, plus(delta2, tau)), plus(compose(delta1, delta2), J_comp_delta2*tau), 1e-4);
...@@ -245,6 +269,7 @@ TEST(IMU_tools, compose_jacobians) ...@@ -245,6 +269,7 @@ TEST(IMU_tools, compose_jacobians)
TEST(IMU_tools, diff_jacobians) TEST(IMU_tools, diff_jacobians)
{ {
// !!! WRONG JACOBIANS but still works... Bad test?
VectorXs delta1(11), delta2(11), err(10); // deltas and err VectorXs delta1(11), delta2(11), err(10); // deltas and err
VectorXs tau(10); // small tangent space element VectorXs tau(10); // small tangent space element
tau.setOnes(); tau.setOnes();
...@@ -261,9 +286,9 @@ TEST(IMU_tools, diff_jacobians) ...@@ -261,9 +286,9 @@ TEST(IMU_tools, diff_jacobians)
// analytical jacobians // analytical jacobians
diff(delta1, delta2, err, J_diff_delta1, J_diff_delta2); diff(delta1, delta2, err, J_diff_delta1, J_diff_delta2);
// check that numerical and analytical match // check that numerical and analytical match -> WRONG Checksize
ASSERT_MATRIX_APPROX(diff(plus(delta1, tau), delta2), plus(diff(delta1, delta2), J_diff_delta1*tau), 1e-4); ASSERT_MATRIX_APPROX(diff(plus(delta1, tau), delta2), diff(delta1, delta2) + J_diff_delta1*tau, 1e-4);
ASSERT_MATRIX_APPROX(diff(delta1, plus(delta2, tau)), plus(diff(delta1, delta2), J_diff_delta2*tau), 1e-4); ASSERT_MATRIX_APPROX(diff(delta1, plus(delta2, tau)), diff(delta1, delta2) + J_diff_delta2*tau, 1e-4);
} }
// TEST(IMU_tools, body2delta_jacobians) // TEST(IMU_tools, body2delta_jacobians)
......
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