diff --git a/src/common/mrob/matrix_base.hpp b/src/common/mrob/matrix_base.hpp index c1fe2c49..4c85f97d 100644 --- a/src/common/mrob/matrix_base.hpp +++ b/src/common/mrob/matrix_base.hpp @@ -47,6 +47,7 @@ typedef Eigen::Matrix Mat3; typedef Eigen::Matrix Mat4; typedef Eigen::Matrix Mat5; typedef Eigen::Matrix Mat6; +typedef Eigen::Matrix Mat9; typedef Eigen::Matrix MatX; //Sparse Matrices @@ -60,6 +61,7 @@ typedef Eigen::Matrix Mat31; typedef Eigen::Matrix Mat41; typedef Eigen::Matrix Mat51; typedef Eigen::Matrix Mat61; +typedef Eigen::Matrix Mat91; typedef Eigen::Matrix MatX1; // Definition of row matrices (vectors) @@ -68,6 +70,7 @@ typedef Eigen::Matrix Mat13; typedef Eigen::Matrix Mat14; typedef Eigen::Matrix Mat15; typedef Eigen::Matrix Mat16; +typedef Eigen::Matrix Mat19; typedef Eigen::Matrix Mat1X; diff --git a/src/geometry/CMakeLists.txt b/src/geometry/CMakeLists.txt index 1309250d..9e12ba9b 100644 --- a/src/geometry/CMakeLists.txt +++ b/src/geometry/CMakeLists.txt @@ -6,6 +6,8 @@ SET(sources SO3.cpp SE3.cpp SE3cov.cpp + SE3vel.cpp + SE3velCov.cpp ) # extra header files @@ -13,6 +15,8 @@ SET(headers mrob/SO3.hpp mrob/SE3.hpp mrob/SE3cov.hpp + mrob/SE3vel.hpp + mrob/SE3velCov.hpp ) # create the shared library diff --git a/src/geometry/SE3cov.cpp b/src/geometry/SE3cov.cpp index 71e35d69..47996d80 100644 --- a/src/geometry/SE3cov.cpp +++ b/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); } diff --git a/src/geometry/SE3vel.cpp b/src/geometry/SE3vel.cpp new file mode 100644 index 00000000..1fe7b8da --- /dev/null +++ b/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); +} diff --git a/src/geometry/SE3velCov.cpp b/src/geometry/SE3velCov.cpp new file mode 100644 index 00000000..62aae0a3 --- /dev/null +++ b/src/geometry/SE3velCov.cpp @@ -0,0 +1,112 @@ +#include "mrob/SE3velCov.hpp" +#include + +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; +} diff --git a/src/geometry/mrob/SE3cov.hpp b/src/geometry/mrob/SE3cov.hpp index 1525845f..cae1c5a0 100644 --- a/src/geometry/mrob/SE3cov.hpp +++ b/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 diff --git a/src/geometry/mrob/SE3vel.hpp b/src/geometry/mrob/SE3vel.hpp new file mode 100644 index 00000000..bdd32345 --- /dev/null +++ b/src/geometry/mrob/SE3vel.hpp @@ -0,0 +1,49 @@ +#ifndef SE3VEL_HPP_ +#define SE3VEL_HPP_ + +#include + +#include "mrob/matrix_base.hpp" +#include "mrob/SO3.hpp" + +namespace mrob{ + +class SE3vel{ + public: + SE3vel(const Mat5 &T = Mat5::Identity()); + + SE3vel(const SE3vel &T); + + SE3vel(const Mat91 &xi); + + SE3vel inv(void) const; + + Mat31 t() const; + Mat31 v() const; + Mat3 R() const; + Mat5 T() const; + + Mat9 adj() const; + + static Mat3 left_jacobian(const Mat31 &phi); + static Mat3 inv_left_jacobian(const Mat31 &phi); + + void Exp(const Mat91 &xi); + Mat91 Ln(void) const; + + void regenerate(); + + protected: + Mat5 T_; +}; + +Mat5 hat9(const Mat91 &xi); + +Mat91 vee9(const Mat5 &xi_hat); + +}// end namespace + +mrob::SE3vel operator*(const mrob::SE3vel& lhs, const mrob::SE3vel& rhs); +std::ostream& operator<<(std::ostream &os, const mrob::SE3vel &obj); + +#endif /* SE3VEL_HPP_ */ diff --git a/src/geometry/mrob/SE3velCov.hpp b/src/geometry/mrob/SE3velCov.hpp new file mode 100644 index 00000000..59a5a58a --- /dev/null +++ b/src/geometry/mrob/SE3velCov.hpp @@ -0,0 +1,31 @@ +#ifndef SE3VELCOV_HPP_ +#define SE3VELCOV_HPP_ + +#include "mrob/SE3vel.hpp" +#include "mrob/SE3cov.hpp" + +namespace mrob +{ + class SE3velCov : public mrob::SE3vel + { + public: + SE3velCov(void); + SE3velCov(const SE3vel &pose, const Mat9 &covariance); + Mat9 covariance; + void compound_2nd_order(const SE3vel &pose_increment, const Mat9 &increment_covariance, const double dt); + void compound_4th_order(const SE3vel &pose_increment, const Mat9 &increment_covariance, const double dt); + + Mat9 getQ(const Mat3& cov_a, const Mat3& cov_w, const double dt) const; + + Mat9 get_S_4th(const Mat9 &Q) const; + + void print(); + + // transforms covariance matrix to notation from Barfoot's papers + // transform(transform(cov)) = cov - self-inverse + Mat9 transform(const Mat9 &covariance) const; + }; + +} // end namespace + +#endif //SE3VELCOV_HPP_