From 0a88e42e84577554221361662ec9ecb27282d370 Mon Sep 17 00:00:00 2001 From: Aleksei Panchenko Date: Wed, 30 Jun 2021 15:17:00 +0300 Subject: [PATCH 1/7] Initial commit for SE3vel class, the extended SE3 group with velocity coordinates --- .gitignore | 1 + src/common/mrob/matrix_base.hpp | 3 + src/geometry/CMakeLists.txt | 2 + src/geometry/SE3vel.cpp | 200 ++++++++++++++++++++++++++++++++ src/geometry/mrob/SE3vel.hpp | 48 ++++++++ 5 files changed, 254 insertions(+) create mode 100644 src/geometry/SE3vel.cpp create mode 100644 src/geometry/mrob/SE3vel.hpp diff --git a/.gitignore b/.gitignore index 3c463593..7c7beb5f 100644 --- a/.gitignore +++ b/.gitignore @@ -18,6 +18,7 @@ bin/ *.png *.pdf *.pkl +.vscode mrob dist/ diff --git a/src/common/mrob/matrix_base.hpp b/src/common/mrob/matrix_base.hpp index 1bcdefc2..bfd82416 100644 --- a/src/common/mrob/matrix_base.hpp +++ b/src/common/mrob/matrix_base.hpp @@ -43,6 +43,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 @@ -56,6 +57,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) @@ -64,6 +66,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 723fe35d..dad34aa4 100644 --- a/src/geometry/CMakeLists.txt +++ b/src/geometry/CMakeLists.txt @@ -5,12 +5,14 @@ SET(sources SO3.cpp SE3.cpp + SE3vel.cpp ) # extra header files SET(headers mrob/SO3.hpp mrob/SE3.hpp + mrob/SE3vel.hpp ) # create the shared library diff --git a/src/geometry/SE3vel.cpp b/src/geometry/SE3vel.cpp new file mode 100644 index 00000000..7afd4f68 --- /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); +} \ No newline at end of file diff --git a/src/geometry/mrob/SE3vel.hpp b/src/geometry/mrob/SE3vel.hpp new file mode 100644 index 00000000..75039f35 --- /dev/null +++ b/src/geometry/mrob/SE3vel.hpp @@ -0,0 +1,48 @@ +#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_ */ From 577713f4a0ac7ca19824ec06d4df055e3b9f5fa2 Mon Sep 17 00:00:00 2001 From: Aleksei Panchenko Date: Thu, 1 Jul 2021 19:16:08 +0300 Subject: [PATCH 2/7] SE3velCov class for compounding initial commit #72 --- src/geometry/CMakeLists.txt | 2 + src/geometry/SE3velCov.cpp | 122 ++++++++++++++++++++++++++++++++ src/geometry/mrob/SE3velCov.hpp | 33 +++++++++ 3 files changed, 157 insertions(+) create mode 100644 src/geometry/SE3velCov.cpp create mode 100644 src/geometry/mrob/SE3velCov.hpp diff --git a/src/geometry/CMakeLists.txt b/src/geometry/CMakeLists.txt index dad34aa4..f0bc966a 100644 --- a/src/geometry/CMakeLists.txt +++ b/src/geometry/CMakeLists.txt @@ -6,6 +6,7 @@ SET(sources SO3.cpp SE3.cpp SE3vel.cpp + SE3velCov.cpp ) # extra header files @@ -13,6 +14,7 @@ SET(headers mrob/SO3.hpp mrob/SE3.hpp mrob/SE3vel.hpp + mrob/SE3velCov.hpp ) # create the shared library diff --git a/src/geometry/SE3velCov.cpp b/src/geometry/SE3velCov.cpp new file mode 100644 index 00000000..13b39b11 --- /dev/null +++ b/src/geometry/SE3velCov.cpp @@ -0,0 +1,122 @@ +#include "mrob/SE3velCov.hpp" +#include + +using namespace mrob; + +Mat3 brackets(const Mat3 &A) +{ + return -A.trace() * Mat3::Identity() + A; +} + +Mat3 brackets(const Mat3 &A, const Mat3 &B) +{ + return brackets(A) * brackets(B) + brackets(B * A); +} + +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; +} \ No newline at end of file diff --git a/src/geometry/mrob/SE3velCov.hpp b/src/geometry/mrob/SE3velCov.hpp new file mode 100644 index 00000000..6bd0de31 --- /dev/null +++ b/src/geometry/mrob/SE3velCov.hpp @@ -0,0 +1,33 @@ +#ifndef SE3VELCOV_HPP_ +#define SE3VELCOV_HPP_ + +#include "mrob/SE3vel.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; + }; + + // Mat3 brackets(const Mat3 &A); + // Mat3 brackets(const Mat3 &A, const Mat3 &B); + +} // end namespace + +#endif //SE3VELCOV_HPP_ \ No newline at end of file From 9ac8a214ae231b376f2c86a9f3ccf0e7dad89d6f Mon Sep 17 00:00:00 2001 From: Aleksei Panchenko Date: Wed, 30 Jun 2021 15:17:00 +0300 Subject: [PATCH 3/7] Initial commit for SE3vel class, the extended SE3 group with velocity coordinates --- src/common/mrob/matrix_base.hpp | 3 + src/geometry/CMakeLists.txt | 2 + src/geometry/SE3vel.cpp | 200 ++++++++++++++++++++++++++++++++ src/geometry/mrob/SE3vel.hpp | 48 ++++++++ 4 files changed, 253 insertions(+) create mode 100644 src/geometry/SE3vel.cpp create mode 100644 src/geometry/mrob/SE3vel.hpp 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..53af4e7b 100644 --- a/src/geometry/CMakeLists.txt +++ b/src/geometry/CMakeLists.txt @@ -6,6 +6,7 @@ SET(sources SO3.cpp SE3.cpp SE3cov.cpp + SE3vel.cpp ) # extra header files @@ -13,6 +14,7 @@ SET(headers mrob/SO3.hpp mrob/SE3.hpp mrob/SE3cov.hpp + mrob/SE3vel.hpp ) # create the shared library diff --git a/src/geometry/SE3vel.cpp b/src/geometry/SE3vel.cpp new file mode 100644 index 00000000..7afd4f68 --- /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); +} \ No newline at end of file diff --git a/src/geometry/mrob/SE3vel.hpp b/src/geometry/mrob/SE3vel.hpp new file mode 100644 index 00000000..75039f35 --- /dev/null +++ b/src/geometry/mrob/SE3vel.hpp @@ -0,0 +1,48 @@ +#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_ */ From 7ac2f21c810cdb5bc3eceec7626374e52060c99c Mon Sep 17 00:00:00 2001 From: Aleksei Panchenko Date: Thu, 1 Jul 2021 19:16:08 +0300 Subject: [PATCH 4/7] SE3velCov class for compounding initial commit #72 --- src/geometry/CMakeLists.txt | 2 + src/geometry/SE3velCov.cpp | 122 ++++++++++++++++++++++++++++++++ src/geometry/mrob/SE3velCov.hpp | 33 +++++++++ 3 files changed, 157 insertions(+) create mode 100644 src/geometry/SE3velCov.cpp create mode 100644 src/geometry/mrob/SE3velCov.hpp diff --git a/src/geometry/CMakeLists.txt b/src/geometry/CMakeLists.txt index 53af4e7b..9e12ba9b 100644 --- a/src/geometry/CMakeLists.txt +++ b/src/geometry/CMakeLists.txt @@ -7,6 +7,7 @@ SET(sources SE3.cpp SE3cov.cpp SE3vel.cpp + SE3velCov.cpp ) # extra header files @@ -15,6 +16,7 @@ SET(headers mrob/SE3.hpp mrob/SE3cov.hpp mrob/SE3vel.hpp + mrob/SE3velCov.hpp ) # create the shared library diff --git a/src/geometry/SE3velCov.cpp b/src/geometry/SE3velCov.cpp new file mode 100644 index 00000000..13b39b11 --- /dev/null +++ b/src/geometry/SE3velCov.cpp @@ -0,0 +1,122 @@ +#include "mrob/SE3velCov.hpp" +#include + +using namespace mrob; + +Mat3 brackets(const Mat3 &A) +{ + return -A.trace() * Mat3::Identity() + A; +} + +Mat3 brackets(const Mat3 &A, const Mat3 &B) +{ + return brackets(A) * brackets(B) + brackets(B * A); +} + +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; +} \ No newline at end of file diff --git a/src/geometry/mrob/SE3velCov.hpp b/src/geometry/mrob/SE3velCov.hpp new file mode 100644 index 00000000..6bd0de31 --- /dev/null +++ b/src/geometry/mrob/SE3velCov.hpp @@ -0,0 +1,33 @@ +#ifndef SE3VELCOV_HPP_ +#define SE3VELCOV_HPP_ + +#include "mrob/SE3vel.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; + }; + + // Mat3 brackets(const Mat3 &A); + // Mat3 brackets(const Mat3 &A, const Mat3 &B); + +} // end namespace + +#endif //SE3VELCOV_HPP_ \ No newline at end of file From f96c199b8d5695aa43e0635c23cce51dd8088446 Mon Sep 17 00:00:00 2001 From: Aleksei Panchenko Date: Fri, 8 Jul 2022 01:13:07 +0300 Subject: [PATCH 5/7] new line at the EOF --- src/geometry/SE3vel.cpp | 2 +- src/geometry/SE3velCov.cpp | 2 +- src/geometry/mrob/SE3velCov.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/geometry/SE3vel.cpp b/src/geometry/SE3vel.cpp index 7afd4f68..1fe7b8da 100644 --- a/src/geometry/SE3vel.cpp +++ b/src/geometry/SE3vel.cpp @@ -197,4 +197,4 @@ void SE3vel::regenerate() { Mat91 xi = this->Ln(); this->Exp(xi); -} \ No newline at end of file +} diff --git a/src/geometry/SE3velCov.cpp b/src/geometry/SE3velCov.cpp index 13b39b11..c7018a76 100644 --- a/src/geometry/SE3velCov.cpp +++ b/src/geometry/SE3velCov.cpp @@ -119,4 +119,4 @@ Mat9 SE3velCov::get_S_4th(const Mat9 &Q) const S_4th = 1./12.*(A_sigma*Q + Q*A_sigma.transpose() + A_q*sigma + sigma*A_q.transpose()) + 1./4.*B; return S_4th; -} \ No newline at end of file +} diff --git a/src/geometry/mrob/SE3velCov.hpp b/src/geometry/mrob/SE3velCov.hpp index 6bd0de31..9f33cdb7 100644 --- a/src/geometry/mrob/SE3velCov.hpp +++ b/src/geometry/mrob/SE3velCov.hpp @@ -30,4 +30,4 @@ namespace mrob } // end namespace -#endif //SE3VELCOV_HPP_ \ No newline at end of file +#endif //SE3VELCOV_HPP_ From 14e397c86bc09b32a761600fe201dd893292a37f Mon Sep 17 00:00:00 2001 From: Gonzalo Ferrer Date: Mon, 29 Aug 2022 20:01:45 +0300 Subject: [PATCH 6/7] brackets function in namespace mrob, visible for all other files including SE3cov --- src/geometry/SE3velCov.cpp | 4 ++-- src/geometry/mrob/SE3velCov.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/geometry/SE3velCov.cpp b/src/geometry/SE3velCov.cpp index c7018a76..b982c19b 100644 --- a/src/geometry/SE3velCov.cpp +++ b/src/geometry/SE3velCov.cpp @@ -3,12 +3,12 @@ using namespace mrob; -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/mrob/SE3velCov.hpp b/src/geometry/mrob/SE3velCov.hpp index 9f33cdb7..339da114 100644 --- a/src/geometry/mrob/SE3velCov.hpp +++ b/src/geometry/mrob/SE3velCov.hpp @@ -25,8 +25,8 @@ namespace mrob Mat9 transform(const Mat9 &covariance) const; }; - // Mat3 brackets(const Mat3 &A); - // Mat3 brackets(const Mat3 &A, const Mat3 &B); + Mat3 brackets(const Mat3 &A); + Mat3 brackets(const Mat3 &A, const Mat3 &B); } // end namespace From 9cc6362c570bf934d506bac7961a30424bff3009 Mon Sep 17 00:00:00 2001 From: Gonzalo Ferrer Date: Mon, 29 Aug 2022 20:17:24 +0300 Subject: [PATCH 7/7] brackets function now defined in SE3cov and used in other files, such as SE3velCov --- src/geometry/SE3cov.cpp | 4 ++-- src/geometry/SE3velCov.cpp | 10 ---------- src/geometry/mrob/SE3cov.hpp | 2 ++ src/geometry/mrob/SE3vel.hpp | 3 ++- src/geometry/mrob/SE3velCov.hpp | 4 +--- 5 files changed, 7 insertions(+), 16 deletions(-) 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/SE3velCov.cpp b/src/geometry/SE3velCov.cpp index b982c19b..62aae0a3 100644 --- a/src/geometry/SE3velCov.cpp +++ b/src/geometry/SE3velCov.cpp @@ -3,16 +3,6 @@ using namespace mrob; -Mat3 mrob::brackets(const Mat3 &A) -{ - return -A.trace() * Mat3::Identity() + A; -} - -Mat3 mrob::brackets(const Mat3 &A, const Mat3 &B) -{ - return brackets(A) * brackets(B) + brackets(B * A); -} - SE3velCov::SE3velCov(void) { this->T_ = Mat5::Identity(); 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 index 75039f35..bdd32345 100644 --- a/src/geometry/mrob/SE3vel.hpp +++ b/src/geometry/mrob/SE3vel.hpp @@ -31,7 +31,8 @@ class SE3vel{ void Exp(const Mat91 &xi); Mat91 Ln(void) const; - void regenerate(); + void regenerate(); + protected: Mat5 T_; }; diff --git a/src/geometry/mrob/SE3velCov.hpp b/src/geometry/mrob/SE3velCov.hpp index 339da114..59a5a58a 100644 --- a/src/geometry/mrob/SE3velCov.hpp +++ b/src/geometry/mrob/SE3velCov.hpp @@ -2,6 +2,7 @@ #define SE3VELCOV_HPP_ #include "mrob/SE3vel.hpp" +#include "mrob/SE3cov.hpp" namespace mrob { @@ -25,9 +26,6 @@ namespace mrob Mat9 transform(const Mat9 &covariance) const; }; - Mat3 brackets(const Mat3 &A); - Mat3 brackets(const Mat3 &A, const Mat3 &B); - } // end namespace #endif //SE3VELCOV_HPP_