Feature/se3vel

src/common/mrob/matrix_base.hpp

@@ -47,6 +47,7 @@ typedef Eigen::Matrix<matData_t, 3,3, Eigen::RowMajor> Mat3;
 typedef Eigen::Matrix<matData_t, 4,4, Eigen::RowMajor> Mat4;
 typedef Eigen::Matrix<matData_t, 5,5, Eigen::RowMajor> Mat5;
 typedef Eigen::Matrix<matData_t, 6,6, Eigen::RowMajor> Mat6;
+typedef Eigen::Matrix<matData_t, 9,9, Eigen::RowMajor> Mat9;
 typedef Eigen::Matrix<matData_t, Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> MatX;
 
 //Sparse Matrices
@@ -60,6 +61,7 @@ typedef Eigen::Matrix<matData_t, 3,1> Mat31;
 typedef Eigen::Matrix<matData_t, 4,1> Mat41;
 typedef Eigen::Matrix<matData_t, 5,1> Mat51;
 typedef Eigen::Matrix<matData_t, 6,1> Mat61;
+typedef Eigen::Matrix<matData_t, 9,1> Mat91;
 typedef Eigen::Matrix<matData_t, Eigen::Dynamic,1> MatX1;
 
 // Definition of row matrices (vectors)
@@ -68,6 +70,7 @@ typedef Eigen::Matrix<matData_t, 1,3> Mat13;
 typedef Eigen::Matrix<matData_t, 1,4> Mat14;
 typedef Eigen::Matrix<matData_t, 1,5> Mat15;
 typedef Eigen::Matrix<matData_t, 1,6> Mat16;
+typedef Eigen::Matrix<matData_t, 1,9> Mat19;
 typedef Eigen::Matrix<matData_t, 1,Eigen::Dynamic> Mat1X;
 
 

src/geometry/CMakeLists.txt

@@ -6,13 +6,17 @@ SET(sources
     SO3.cpp
     SE3.cpp
     SE3cov.cpp
+    SE3vel.cpp
+    SE3velCov.cpp
 )
 
 # extra header files
 SET(headers
 	mrob/SO3.hpp
 	mrob/SE3.hpp
     mrob/SE3cov.hpp
+    mrob/SE3vel.hpp
+    mrob/SE3velCov.hpp
 )
 
 # create the shared library

src/geometry/SE3cov.cpp

@@ -9,12 +9,12 @@ SE3Cov::SE3Cov(const SE3& T, const Mat6 &cov): SE3(T), covariance_(cov){}
 
 SE3Cov::SE3Cov(const SE3Cov& pose):SE3(Mat4(pose.T())), covariance_(pose.cov()){}
 
-Mat3 brackets(const Mat3& A)
+Mat3 mrob::brackets(const Mat3& A)
 {
     return -A.trace()*Mat3::Identity() + A;
 }
 
-Mat3 brackets(const Mat3& A, const Mat3& B)
+Mat3 mrob::brackets(const Mat3& A, const Mat3& B)
 {
     return brackets(A)*brackets(B) + brackets(B*A);
 }

src/geometry/SE3vel.cpp

@@ -0,0 +1,200 @@
+#include "mrob/SE3vel.hpp"
+
+using namespace mrob;
+
+SE3vel::SE3vel(const Mat5 &T) : T_(T) {}
+
+SE3vel::SE3vel(const SE3vel &T) : T_(T.T()){}
+
+SE3vel::SE3vel(const Mat91 &xi)
+{
+    this->Exp(xi);
+}
+
+Mat31 SE3vel::t() const
+{
+    return T_.topRightCorner<3,1>();
+}
+
+Mat31 SE3vel::v() const
+{
+    return T_.block<3,1>(0,3);
+}
+
+Mat3 SE3vel::R() const
+{
+    return T_.topLeftCorner<3,3>();
+}
+
+Mat5 SE3vel::T(void) const
+{
+    return this->T_;
+}
+
+SE3vel SE3vel::inv(void) const
+{
+    Mat5 inv(Mat5::Zero());
+    Mat3 R = this->R();
+    R.transposeInPlace();
+
+    inv << R, -R*this->v(), -R*this->t(),
+            0,0,0,1,0,
+            0,0,0,0,1;
+
+    return SE3vel(inv);
+}
+
+SE3vel operator*(const SE3vel& lhs, const SE3vel& rhs)
+{
+    return SE3vel(Mat5(lhs.T()*rhs.T()));
+}
+
+Mat9 SE3vel::adj() const
+{
+    Mat9 res(Mat9::Zero());
+    Mat3 R = this->R();
+    Mat31 v = this->v();
+    Mat31 t = this->t();
+
+    res.block<3,3>(0,0) = R;
+    res.block<3,3>(3,3) = R;
+    res.block<3,3>(6,6) = R;
+
+    res.block<3,3>(3,0) = hat3(v)*R;
+    res.block<3,3>(6,0) = hat3(t)*R;
+
+    return res;
+}
+
+std::ostream& operator<<(std::ostream &os, const SE3vel& obj)
+    {
+        os << obj.T();
+        return os;
+    }
+
+
+Mat5 hat9(const Mat91 &xi)
+{
+    Mat5 result(Mat5::Zero());
+
+    result <<    0, -xi(2),  xi(1), xi(3), xi(6),
+             xi(2),      0, -xi(0), xi(4), xi(7),
+            -xi(1),  xi(0),      0, xi(5), xi(8),
+                 0,      0,      0,     0,     0,
+                 0,      0,      0,     0,     0;
+
+    return result;   
+}
+
+
+/**
+ * Vee operator (v), the inverse of hat
+ */
+Mat91 vee9(const Mat4 &xi_hat)
+{
+    Mat91 result(Mat91::Zero());
+
+    result << xi_hat(2,1), xi_hat(0,2), xi_hat(1,0),
+              xi_hat(0,3), xi_hat(1,3), xi_hat(2,3),
+              xi_hat(0,4), xi_hat(1,4), xi_hat(2,4);
+
+    return result;
+}
+
+
+Mat3 SE3vel::left_jacobian(const Mat31& phi)
+{
+    Mat3 V = Mat3::Identity();
+    Mat3 phi_hat = hat3(phi);
+    double o = phi.norm();
+    double o2 = phi.squaredNorm();
+    // If rotation is not zero
+    matData_t c2, c3;
+    if ( o > 1e-3){ // c2 and c3 become numerically imprecise for o < 1-5, so we choose a conservative threshold 1e-3
+        c2 = (1 - std::cos(o))/o2;
+        c3 = (o - std::sin(o))/o2/o;
+    }
+    else
+    {
+        // second order Taylor (first order is zero since this is an even function)
+        c2 = 0.5 - o2/24;
+        // Second order Taylor
+        c3 = 1.0/6.0 - o2/120;
+    }
+    V += c2*phi_hat + c3*phi_hat*phi_hat;
+
+    return V;
+}
+
+
+Mat3 SE3vel::inv_left_jacobian(const Mat31 &phi)
+{
+    Mat3 Vinv = Mat3::Identity();
+    double k1;
+    double o = phi.norm();
+    double o2 = phi.squaredNorm();
+    Mat3 phi_hat = hat3(phi);
+    // 5e-3 bound provided on numerical_test.cpp, smaller than this k1 becomes degradated
+    if (o > 5e-3)
+    {
+        double c1 = std::sin(o); //sin(o)/o, we remove the o in both coeficients
+        double c2 = (1 - std::cos(o))/o; // (1 - std::cos(o))/o/o
+        k1 = 1/o2*(1 - 0.5*c1/c2);
+    }
+    //Taylor expansion for small o.
+    else
+    {
+        // f(o) = 1/12 + 1/2*f''*o^2
+        // f'' = 1/360
+        k1 = 1.0/12 + o2/720;
+    }
+    Vinv += -0.5*phi_hat + k1* phi_hat*phi_hat;
+
+    // v = V^-1 t
+    return Vinv;
+}
+
+void SE3vel::Exp(const Mat91& xi)  
+{
+    Mat5 result(Mat5::Identity());
+
+    Mat31 phi = xi.head(3);
+    Mat31 v = xi.segment<3>(3);
+    Mat31 t = xi.tail(3);
+
+    SO3 tmp(phi);
+
+    result.topLeftCorner<3,3>() << tmp.R();
+
+    Mat3 jac = this->left_jacobian(phi);
+
+    result.block<3,1>(0,3) << jac*v;
+    result.block<3,1>(0,4) << jac*t;
+
+    this->T_ = result;
+}
+
+Mat91 SE3vel::Ln() const
+{
+    Mat91 result;
+
+    Mat3 R = this->R();
+    Mat31 v = this->v();
+    Mat31 t = this->t();
+
+    SO3 tmp(R);
+    Mat31 log_R_vee = tmp.ln_vee();
+    Mat3 jac = SE3vel::inv_left_jacobian(log_R_vee);
+
+    result.head(3) << log_R_vee;
+    result.segment<3>(3) << jac*v;
+    result.tail(3) << jac*t;
+
+    return result;
+}
+
+void SE3vel::regenerate()
+{
+    Mat91 xi = this->Ln();
+    this->Exp(xi);
+}

src/geometry/SE3velCov.cpp

@@ -0,0 +1,112 @@
+#include "mrob/SE3velCov.hpp"
+#include <iostream>
+
+using namespace mrob;
+
+SE3velCov::SE3velCov(void)
+{
+    this->T_ = Mat5::Identity();
+    this->covariance = Mat9::Identity();
+}
+
+SE3velCov::SE3velCov(const SE3vel &pose, const Mat9 &covariance)
+{
+    this->T_ = pose.T();
+    this->covariance = covariance;
+}
+
+Mat9 SE3velCov::getQ(const Mat3 &cov_a, const Mat3 &cov_w, const double dt) const
+{
+    Mat9 Q(Mat9::Identity());
+
+    Q.topLeftCorner<3, 3>() = cov_w;
+    Q.block<3, 3>(3, 3) = cov_a;
+    Q.block<3, 3>(6, 3) = 1. / 2. * cov_a * dt;
+    Q.block<3, 3>(3, 6) = 1. / 2. * cov_a * dt;
+    Q.bottomRightCorner<3, 3>() = 1. / 4. * cov_a * dt * dt;
+    Q *= dt * dt;
+
+    return Q;
+}
+
+void SE3velCov::compound_2nd_order(const SE3vel &pose_increment, const Mat9 &Q, const double dt)
+{
+    Mat9 F(Mat9::Identity());
+    F.block<3, 3>(6, 3) = dt * Mat3::Identity();
+
+    Mat9 Gamma_inv_adj = pose_increment.inv().adj();
+
+    Mat9 tmp = Gamma_inv_adj * F;
+
+    this->covariance = tmp * this->covariance * tmp.transpose() + Q;
+
+    this->T_ = this->T() * pose_increment.T();
+}
+
+void SE3velCov::compound_4th_order(const SE3vel &pose_increment, const Mat9 &Q, const double dt)
+{
+    Mat9 S_4th = this->get_S_4th(Q); // get_s_4th mus be called exactly before calling compound_2nd
+    compound_2nd_order(pose_increment, Q, dt);
+
+    this->covariance += S_4th;
+}
+
+Mat9 SE3velCov::get_S_4th(const Mat9 &Q) const
+{
+    Mat9 S_4th(Mat9::Zero());
+
+    Mat9 A_sigma(Mat9::Zero());
+
+    Mat9 sigma = this->covariance;
+    Mat3 sigma_phi_phi = sigma.topLeftCorner<3, 3>();
+    Mat3 sigma_phi_nu = sigma.block<3, 3>(0, 3);
+    Mat3 sigma_nu_phi = sigma_phi_nu.transpose();
+    Mat3 sigma_phi_rho = sigma.topRightCorner<3, 3>();
+    Mat3 sigma_rho_phi = sigma_phi_rho.transpose();
+    Mat3 sigma_nu_nu = sigma.block<3, 3>(3, 3);
+    Mat3 sigma_nu_rho = sigma.block<3, 3>(3, 6);
+    Mat3 sigma_rho_rho = sigma.bottomRightCorner<3, 3>();
+
+    A_sigma.topLeftCorner<3, 3>() = brackets(sigma_phi_phi);
+    A_sigma.block<3, 3>(3, 0) = brackets(sigma_phi_nu.transpose() + sigma_phi_nu);
+    A_sigma.block<3, 3>(3, 3) = brackets(sigma_phi_phi);
+    A_sigma.bottomLeftCorner<3, 3>() = brackets(sigma_phi_rho.transpose() + sigma_phi_rho);
+    A_sigma.bottomRightCorner<3, 3>() = brackets(sigma_phi_phi);
+
+    Mat9 A_q(Mat9::Zero());
+
+    Mat3 q_phi_phi = Q.topLeftCorner<3, 3>();
+    Mat3 q_phi_nu = Q.block<3, 3>(0, 3);
+    Mat3 q_nu_phi = q_phi_nu.transpose();
+    Mat3 q_phi_rho = Q.topRightCorner<3, 3>();
+    Mat3 q_rho_phi = q_phi_rho.transpose();
+    Mat3 q_nu_nu = Q.block<3, 3>(3, 3);
+    Mat3 q_nu_rho = Q.block<3, 3>(3, 6);
+    Mat3 q_rho_rho = Q.bottomRightCorner<3, 3>();
+
+    A_q.topLeftCorner<3, 3>() = brackets(q_phi_phi);
+    A_q.block<3, 3>(3, 0) = brackets(q_phi_nu + q_phi_nu.transpose());
+    A_q.block<3, 3>(3, 3) = brackets(q_phi_phi);
+    A_q.bottomLeftCorner<3, 3>() = brackets(q_phi_rho + q_phi_rho.transpose());
+    A_q.bottomRightCorner<3, 3>() = brackets(q_phi_phi);
+
+    Mat9 B(Mat9::Zero());
+
+    B.topLeftCorner<3, 3>() = brackets(sigma_phi_phi, q_phi_phi);
+    B.block<3, 3>(3, 0) = brackets(sigma_phi_phi, q_phi_nu) + brackets(sigma_nu_phi, q_phi_phi);
+    B.block<3, 3>(0, 3) = B.block<3, 3>(3, 0).transpose();
+    B.block<3, 3>(6, 0) = brackets(sigma_phi_phi, q_phi_rho) + brackets(sigma_rho_phi, q_phi_phi);
+    B.block<3, 3>(0, 6) = B.block<3, 3>(6, 0).transpose();
+    B.block<3,3>(3,3) = brackets(sigma_phi_phi,q_nu_nu) + brackets(sigma_phi_nu,q_nu_phi) +
+                        brackets(sigma_nu_phi, q_nu_phi) + brackets(sigma_nu_nu,q_phi_phi);
+
+    B.block<3,3>(3,6) = brackets(sigma_nu_rho,q_phi_phi) + brackets(sigma_phi_rho,q_nu_phi) + 
+                        brackets(sigma_nu_phi,q_rho_phi) + brackets(sigma_phi_phi, q_nu_rho);
+    B.block<3,3>(6,3) = B.block<3,3>(3,6).transpose();
+    B.bottomRightCorner<3, 3>() = brackets(sigma_phi_phi,q_rho_rho) + brackets(sigma_phi_rho,q_rho_phi) + 
+                        brackets(sigma_rho_phi,q_phi_rho) + brackets(sigma_rho_rho,q_phi_phi);
+
+    S_4th = 1./12.*(A_sigma*Q + Q*A_sigma.transpose() + A_q*sigma + sigma*A_q.transpose()) + 1./4.*B;
+
+    return S_4th;
+}

src/geometry/mrob/SE3cov.hpp

@@ -135,6 +135,8 @@ namespace mrob
      * @return Mat6
      */
     Mat6 curly_wedge(const Mat61& xi);
+    Mat3 brackets(const Mat3 &A);
+    Mat3 brackets(const Mat3 &A, const Mat3 &B);
 
 } // end namespace