From cf2d7ca27cb7eaf8c25baf275a8ec6d63f69b08d Mon Sep 17 00:00:00 2001 From: GAECHTER TOYA Stefan Date: Mon, 25 Nov 2024 18:04:28 +0100 Subject: [PATCH 1/8] Add options for lie group preintegration --- cmake/HandleGeneralOptions.cmake | 2 ++ cmake/HandlePrintConfiguration.cmake | 2 ++ gtsam/config.h.in | 8 +++++++- 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 8c56ae242e..cdc9de999c 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -31,6 +31,7 @@ option(GTSAM_FORCE_STATIC_LIB "Force gtsam to be a static library, option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) +option(GTSAM_EXTENDED_POSE_RETRACT "Enable/Disable using ExtendedPose3::Retract as the default for NavState" OFF) option(GTSAM_DT_MERGING "Enable/Disable merging of equal leaf nodes in DecisionTrees. This leads to significant speed up and memory savings." ON) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_ENABLE_MEMORY_SANITIZER "Enable/Disable memory sanitizer" OFF) @@ -43,6 +44,7 @@ option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matl option(GTSAM_ALLOW_DEPRECATED_SINCE_V43 "Allow use of methods/functions deprecated in GTSAM 4.3" ON) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) +option(GTSAM_LIEGROUP_PREINTEGRATION "Use IMU pre-integration based on Lie groups" OFF) option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) option(GTSAM_SLOW_BUT_CORRECT_EXPMAP "Use slower but correct expmap for Pose2" OFF) diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake index 42fae90f77..a962bb4be9 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -90,10 +90,12 @@ print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency c print_enabled_config(${GTSAM_ENABLE_MEMORY_SANITIZER} "Build with Memory Sanitizer ") print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") +print_enabled_config(${GTSAM_EXTENDED_POSE_RETRACT} "NavState uses ExtendedPose3 retract") print_enabled_config(${GTSAM_DT_MERGING} "Enable branch merging in DecisionTree") print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V43} "Allow features deprecated in GTSAM 4.3") print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") +print_enabled_config(${GTSAM_LIEGROUP_PREINTEGRATION} "Use Lie group based preintegration") message(STATUS "MATLAB toolbox flags") print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ") diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 719cbc6cb0..8ffdbd0d8f 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -39,6 +39,9 @@ #cmakedefine GTSAM_ROT3_EXPMAP #endif +// Whether GTSAM is compiled to use ExtendedPose3::Retract as the default for NavState +#cmakedefine GTSAM_EXTENDED_POSE_RETRACT + // Whether to enable merging of equal leaf nodes in the Discrete Decision Tree. #cmakedefine GTSAM_DT_MERGING @@ -78,9 +81,12 @@ // Support Metis-based nested dissection #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION -// Support Metis-based nested dissection +// Whether to use IMU pre-integration on tangent space #cmakedefine GTSAM_TANGENT_PREINTEGRATION +// Whether to use IMU pre-integration based on Lie groups +#cmakedefine GTSAM_LIEGROUP_PREINTEGRATION + // Whether to use the system installed Metis instead of the provided one #cmakedefine GTSAM_USE_SYSTEM_METIS From 41fd7b5200825980c22ca1344e82679f27b7dc49 Mon Sep 17 00:00:00 2001 From: GAECHTER TOYA Stefan Date: Mon, 25 Nov 2024 18:05:05 +0100 Subject: [PATCH 2/8] Add extended pose --- gtsam/geometry/ExtendedPose3.cpp | 475 ++++++++++++++++++ gtsam/geometry/ExtendedPose3.h | 381 +++++++++++++++ gtsam/geometry/tests/testExtendedPose3.cpp | 531 +++++++++++++++++++++ 3 files changed, 1387 insertions(+) create mode 100644 gtsam/geometry/ExtendedPose3.cpp create mode 100644 gtsam/geometry/ExtendedPose3.h create mode 100644 gtsam/geometry/tests/testExtendedPose3.cpp diff --git a/gtsam/geometry/ExtendedPose3.cpp b/gtsam/geometry/ExtendedPose3.cpp new file mode 100644 index 0000000000..89dfd84efc --- /dev/null +++ b/gtsam/geometry/ExtendedPose3.cpp @@ -0,0 +1,475 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ExtendedPose3.cpp + * @brief 3D ExtendedPose3 + */ + +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/** instantiate concept checks */ +GTSAM_CONCEPT_POSE_INST(ExtendedPose3); + +/* ************************************************************************* */ +ExtendedPose3 ExtendedPose3::Create(const Rot3& R, const Point3& v, const Point3& p, OptionalJacobian<9, 3> HR, OptionalJacobian<9, 3> Hv, + OptionalJacobian<9, 3> Hp) { + if (HR) *HR << I_3x3, Z_3x3, Z_3x3; + if (Hv) *Hv << Z_3x3, R.transpose(), Z_3x3; + if (Hp) *Hp << Z_3x3, Z_3x3, R.transpose(); + return ExtendedPose3(R, v, p); +} + +/* ************************************************************************* */ +ExtendedPose3 ExtendedPose3::inverse() const { + Rot3 Rt = R_.inverse(); + return ExtendedPose3(Rt, Rt * (-v_), Rt * (-p_)); +} + +/* ************************************************************************* */ +// Calculate Adjoint map +// Ad_pose is 9*9 matrix that when applied to twist xi, returns Ad_pose(xi) +Matrix9 ExtendedPose3::AdjointMap() const { + const Matrix3 R = R_.matrix(); + Matrix3 A = skewSymmetric(v_.x(), v_.y(), v_.z()) * R; + Matrix3 B = skewSymmetric(p_.x(), p_.y(), p_.z()) * R; + Matrix9 adj; + adj << R, Z_3x3, Z_3x3, + A, R, Z_3x3, + B, Z_3x3, R; + return adj; +} + +/* ************************************************************************* */ +Matrix9 ExtendedPose3::adjointMap(const Vector9& xi) { + Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); + Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8)); + Matrix9 adj; + adj << w_hat, Z_3x3, Z_3x3, + v_hat, w_hat, Z_3x3, + a_hat, Z_3x3, w_hat; + return adj; +} + +/* ************************************************************************* */ +Vector9 ExtendedPose3::adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 Gi = adjointMap(dxi); + Hxi->col(i) = Gi * y; + } + } + return adjointMap(xi) * y; +} + +/* ************************************************************************* */ +Vector9 ExtendedPose3::adjointTranspose(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 GTi = adjointMap(dxi).transpose(); + Hxi->col(i) = GTi * y; + } + } + return adjointMap(xi).transpose() * y; +} + + +/* ************************************************************************* */ +void ExtendedPose3::print(const string& s) const { + cout << s; + R_.print("R:\n"); + cout << "v:" << v_ << ";" << endl; + cout << "p:" << p_ << ";" << endl; +} + +/* ************************************************************************* */ +bool ExtendedPose3::equals(const ExtendedPose3& pose, double tol) const { + return R_.equals(pose.R_, tol) && traits::Equals(v_, pose.v_, tol) && traits::Equals(p_, pose.p_, tol); +} + +/* ************************************************************************* */ +ExtendedPose3 ExtendedPose3::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { + if (Hxi) *Hxi = ExpmapDerivative(xi); + + Vector3 phi(xi(0), xi(1), xi(2)), nu(xi(3), xi(4), xi(5)), rho(xi(6), xi(7), xi(8)); + + Rot3 R = Rot3::Expmap(phi); + + double phi_norm = phi.norm(); + if (phi_norm < 1e-8) + return ExtendedPose3(Rot3(), Point3(nu), Point3(rho)); + else { + Matrix W = skewSymmetric(phi/phi_norm); + Matrix A = I_3x3 + ((1 - cos(phi_norm)) / phi_norm) * W + ((phi_norm - sin(phi_norm)) / phi_norm) * (W * W); + return ExtendedPose3(Rot3::Expmap(phi), Point3(A * nu), Point3(A * rho)); + } + +} + +/* ************************************************************************* */ +Vector9 ExtendedPose3::Logmap(const ExtendedPose3& pose, OptionalJacobian<9, 9> Hpose) { + if (Hpose) *Hpose = LogmapDerivative(pose); + const Vector3 phi = Rot3::Logmap(pose.rotation()); + const Vector3 v = pose.velocity(); + const Vector3 p = pose.position(); + const double t = phi.norm(); + if (t < 1e-8) { + Vector9 log; + log << phi, v, p; + return log; + } else { + const Matrix3 W = skewSymmetric(phi / t); + + const double Tan = tan(0.5 * t); + const Vector3 Wp = W * p; + const Vector3 Wv = W * v; + const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); + const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); + Vector9 log; + log << phi, nu, rho; + return log; + } +} + + +/* ************************************************************************* */ +ExtendedPose3 ExtendedPose3::ChartAtOrigin::Retract(const Vector9& xi, ChartJacobian Hxi) { +#ifdef GTSAM_POSE3_EXPMAP + return Expmap(xi, Hxi); +#else + +#endif +} + +/* ************************************************************************* */ +Vector9 ExtendedPose3::ChartAtOrigin::Local(const ExtendedPose3& pose, ChartJacobian Hpose) { +#ifdef GTSAM_POSE3_EXPMAP + return Logmap(pose, Hpose); +#else + +#endif +} + +/* ************************************************************************* */ + Vector9 ExtendedPose3::boxminus(const ExtendedPose3& g) const { + // Matrix3 D_dR_R, D_dt_R, D_dv_R; + // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); + // const Point3 dt = R_.unrotate(g.p_ - p_, H1 ? &D_dt_R : 0); + // const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + + + // Vector9 xi; + // Matrix3 D_xi_R; + // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv; + // if (H1) { + // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + // D_dt_R, -I_3x3, Z_3x3, // + // D_dv_R, Z_3x3, -I_3x3; + // } + // if (H2) { + // *H2 << D_xi_R, Z_3x3, Z_3x3, // + // Z_3x3, dR.matrix(), Z_3x3, // + // Z_3x3, Z_3x3, dR.matrix(); + // } + + Matrix3 D_dR_R, D_dt_R, D_dv_R; + const Rot3 dR = g.R_; + const Point3 dt = g.p_; + const Vector dv = g.v_; + + Vector9 xi; + xi << Rot3::Logmap(dR), dt, dv; + return xi; +} + +/* ************************************************************************* */ +/** + * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative matrix + */ +static Matrix63 computeQforExpmapDerivative(const Vector9& xi) { + const auto omega = xi.head<3>(); + const auto nu = xi.segment<3>(3); + const auto rho = xi.tail<3>(); + const Matrix3 V = skewSymmetric(nu); + const Matrix3 P = skewSymmetric(rho); + const Matrix3 W = skewSymmetric(omega); + + Matrix3 Qv, Qp; + Matrix63 Q; + +#ifdef NUMERICAL_EXPMAP_DERIV + + +#else + // The closed-form formula in Barfoot14tro eq. (102) + double phi = omega.norm(); + if (std::abs(phi)>1e-5) { + const double sinPhi = sin(phi), cosPhi = cos(phi); + const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Qv = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); + Qp = -0.5*P + (phi-sinPhi)/phi3*(W*P + P*W - W*P*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*P + P*W*W - 3*W*P*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*P*W*W + W*W*P*W); + } + else { + Qv = -0.5*V + 1./6.*(W*V + V*W - W*V*W) + + 1./24.*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + Qp = -0.5*P + 1./6.*(W*P + P*W - W*P*W) + + 1./24.*(W*W*P + P*W*W - 3*W*P*W) + - 0.5*(1./24. + 3./120.)*(W*P*W*W + W*W*P*W); + } +#endif + Q << Qv, Qp; + return Q; +} + +/* ************************************************************************* */ +Matrix9 ExtendedPose3::ExpmapDerivative(const Vector9& xi) { + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + const Matrix63 Q = computeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, + Qv, Jw, Z_3x3, + Qp, Z_3x3, Jw; + return J; +} + +/* ************************************************************************* */ +Matrix9 ExtendedPose3::LogmapDerivative(const ExtendedPose3& pose) { + const Vector9 xi = Logmap(pose); + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + const Matrix63 Q = computeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + const Matrix3 Qv2 = -Jw*Qv*Jw; + const Matrix3 Qp2 = -Jw*Qp*Jw; + Matrix9 J; + + J << Jw, Z_3x3, Z_3x3, + Qv2, Jw, Z_3x3, + Qp2, Z_3x3, Jw; + return J; +} + +/* ************************************************************************* */ +const Point3& ExtendedPose3::position(OptionalJacobian<3, 9> Hself) const { + if (Hself) *Hself << Z_3x3, Z_3x3, rotation().matrix(); + return p_; +} + +const Point3& ExtendedPose3::translation(OptionalJacobian<3, 9> Hself) const { + if (Hself) *Hself << Z_3x3, Z_3x3, rotation().matrix(); + return p_; +} + +/* ************************************************************************* */ + +const Point3& ExtendedPose3::velocity(OptionalJacobian<3, 9> Hself) const { + if (Hself) { + *Hself << Z_3x3, rotation().matrix(), Z_3x3; + } + return v_; +} + +/* ************************************************************************* */ + +const Rot3& ExtendedPose3::rotation(OptionalJacobian<3, 9> Hself) const { + if (Hself) { + *Hself << I_3x3, Z_3x3, Z_3x3; + } + return R_; +} + +/* ************************************************************************* */ +Matrix5 ExtendedPose3::matrix() const { + Matrix5 mat; + mat << R_.matrix(), v_, p_, + 0.0, 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0; + return mat; +} + +/* ************************************************************************* */ +ExtendedPose3 ExtendedPose3::transformPoseFrom(const ExtendedPose3& aTb, OptionalJacobian<9, 9> Hself, OptionalJacobian<9, 9> HaTb) const { + const ExtendedPose3& wTa = *this; + return wTa.compose(aTb, Hself, HaTb); +} + +/* ************************************************************************* */ +ExtendedPose3 ExtendedPose3::transformPoseTo(const ExtendedPose3& wTb, OptionalJacobian<9, 9> Hself, OptionalJacobian<9, 9> HwTb) const { + if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap(); + if (HwTb) *HwTb = I_9x9; + const ExtendedPose3& wTa = *this; + return wTa.inverse() * wTb; +} + +/* ************************************************************************* */ +Point3 ExtendedPose3::transformFrom(const Point3& point, OptionalJacobian<3, 9> Hself, + OptionalJacobian<3, 3> Hpoint) const { + // Only get matrix once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + const Matrix3 R = R_.matrix(); + if (Hself) { + Hself->leftCols<3>() = R * skewSymmetric(-point.x(), -point.y(), -point.z()); + Hself->rightCols<3>() = R; + } + if (Hpoint) { + *Hpoint = R; + } + return R_ * point + p_; +} + +/* ************************************************************************* */ +Point3 ExtendedPose3::transformTo(const Point3& point, OptionalJacobian<3, 9> Hself, + OptionalJacobian<3, 3> Hpoint) const { + // Only get transpose once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + const Matrix3 Rt = R_.transpose(); + const Point3 q(Rt*(point - p_)); + if (Hself) { + const double wx = q.x(), wy = q.y(), wz = q.z(); + (*Hself) << + 0.0, -wz, +wy,0.0,0.0,0.0,-1.0, 0.0, 0.0, + +wz, 0.0, -wx,0.0,0.0,0.0, 0.0,-1.0, 0.0, + -wy, +wx, 0.0,0.0,0.0,0.0, 0.0, 0.0,-1.0; + } + if (Hpoint) { + *Hpoint = Rt; + } + return q; +} + +/* ************************************************************************* */ +double ExtendedPose3::range(const Point3& point, OptionalJacobian<1, 9> Hself, + OptionalJacobian<1, 3> Hpoint) const { + Matrix39 D_local_pose; + Matrix3 D_local_point; + Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0); + if (!Hself && !Hpoint) { + return local.norm(); + } else { + Matrix13 D_r_local; + const double r = norm3(local, D_r_local); + if (Hself) *Hself = D_r_local * D_local_pose; + if (Hpoint) *Hpoint = D_r_local * D_local_point; + return r; + } +} + +/* ************************************************************************* */ +double ExtendedPose3::range(const ExtendedPose3& pose, OptionalJacobian<1, 9> Hself, + OptionalJacobian<1, 9> Hpose) const { + Matrix13 D_local_point; + double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0); + if (Hpose) *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); + return r; +} + +/* ************************************************************************* */ +Unit3 ExtendedPose3::bearing(const Point3& point, OptionalJacobian<2, 9> Hself, + OptionalJacobian<2, 3> Hpoint) const { + Matrix39 D_local_pose; + Matrix3 D_local_point; + Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0); + if (!Hself && !Hpoint) { + return Unit3(local); + } else { + Matrix23 D_b_local; + Unit3 b = Unit3::FromPoint3(local, D_b_local); + if (Hself) *Hself = D_b_local * D_local_pose; + if (Hpoint) *Hpoint = D_b_local * D_local_point; + return b; + } +} + +/* ************************************************************************* */ +Unit3 ExtendedPose3::bearing(const ExtendedPose3& pose, OptionalJacobian<2, 9> Hself, + OptionalJacobian<2, 9> Hpose) const { + if (Hpose) { + Hpose->setZero(); + return bearing(pose.translation(), Hself, Hpose.cols<3>(3)); + } + return bearing(pose.translation(), Hself, boost::none); +} + +/* ************************************************************************* */ +boost::optional ExtendedPose3::Align(const std::vector& abPointPairs) { + const size_t n = abPointPairs.size(); + if (n < 3) + return boost::none; // we need at least three pairs + + // calculate centroids + Point3 aCentroid(0,0,0), bCentroid(0,0,0); + for(const Point3Pair& abPair: abPointPairs) { + aCentroid += abPair.first; + bCentroid += abPair.second; + } + double f = 1.0 / n; + aCentroid *= f; + bCentroid *= f; + + // Add to form H matrix + Matrix3 H = Z_3x3; + for(const Point3Pair& abPair: abPointPairs) { + Point3 da = abPair.first - aCentroid; + Point3 db = abPair.second - bCentroid; + H += da * db.transpose(); + } + + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot3 aRb = Rot3::ClosestTo(H); + Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); + Point3 v; + return ExtendedPose3(aRb, v, aTb); +} + + +/* ************************************************************************* */ +std::ostream &operator<<(std::ostream &os, const ExtendedPose3& pose) { + os << pose.rotation(); + const Point3& v = pose.velocity(); + const Point3& p = pose.position(); + os << "v:[" << v.x() << ", " << v.y() << ", " << v.z() << "];\n"; + os << "p:[" << p.x() << ", " << p.y() << ", " << p.z() << "];\n"; + return os; +} + +} // namespace gtsam diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h new file mode 100644 index 0000000000..20c0eba4f0 --- /dev/null +++ b/gtsam/geometry/ExtendedPose3.h @@ -0,0 +1,381 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + *@file ExtendedPose3.h + *@brief 3D Extended Pose + */ + +// \callgraph +#pragma once + +#include + +#include +#include +#include +#include + +namespace gtsam { + +/** + * A 3D extended pose (R,v,p) : (Rot3,Point3,Point3) + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT ExtendedPose3: public LieGroup { +public: + + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Velocity; + typedef Point3 Position; + typedef Position Translation; + +private: + + Rot3 R_; ///< Rotation gRp, between global and pose frame + Point3 v_; ///< Velocity gVp, from global origin to pose frame origin + Point3 p_; ///< Position gPp, from global origin to pose frame origin + +public: + + /// @name Standard Constructors + /// @{ + + /** Default constructor is origin */ + ExtendedPose3() : R_(traits::Identity()), v_(traits::Identity()), p_(traits::Identity()) {} + + /** Copy constructor */ + ExtendedPose3(const ExtendedPose3& pose) : + R_(pose.R_), v_(pose.v_), p_(pose.p_) { + } + + /** Construct from R,v,p */ + ExtendedPose3(const Rot3& R, const Point3& v, const Point3& p) : + R_(R), v_(v), p_(p) { + } + + //explicit Pose3(const Pose2& pose2); + + /** Constructor from 5*5 matrix */ + ExtendedPose3(const Matrix &T) : + R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1), + T(2, 2)), v_(T(0, 3), T(1, 3), T(2, 3)), p_(T(0, 4), T(1, 4), T(2, 4)) { + } + + /// Named constructor with derivatives + static ExtendedPose3 Create(const Rot3& R, const Point3& v, const Point3& p, + OptionalJacobian<9, 3> HR = boost::none, + OptionalJacobian<9, 3> Hv = boost::none, + OptionalJacobian<9, 3> Hp = boost::none); + + /** + * Create Pose3 by aligning two point pairs + * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point + * Note this allows for noise on the points but in that case the mapping will not be exact. + */ + static boost::optional Align(const std::vector& abPointPairs); + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const; + + /// assert equality up to a tolerance + bool equals(const ExtendedPose3& pose, double tol = 1e-9) const; + + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static ExtendedPose3 identity() { + return ExtendedPose3(); + } + + /// inverse transformation with derivatives + ExtendedPose3 inverse() const; + + /// compose syntactic sugar + ExtendedPose3 operator*(const ExtendedPose3& T) const { + return ExtendedPose3(R_ * T.R_, v_ + R_ * T.v_, p_ + R_ * T.p_); + } + + /// @} + /// @name Lie Group + /// @{ + + /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,V_x,V_y,V_z,P_x,P_y,P_z] \f$ + static ExtendedPose3 Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = boost::none); + + /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,V_x,V_y,V_z,P_x,P_y,P_z] \f$ of this rotation + static Vector9 Logmap(const ExtendedPose3& pose, OptionalJacobian<9, 9> Hpose = boost::none); + + /** + * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame + * Ad_pose is 9*9 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,V_x,V_y,V_z,P_x,P_y,P_z] \f$, returns Ad_pose(xi) + */ + Matrix9 AdjointMap() const; /// FIXME Not tested - marked as incorrect + + /** + * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame + * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + */ + Vector9 Adjoint(const Vector9& xi_b) const { + return AdjointMap() * xi_b; + } /// FIXME Not tested - marked as incorrect + + /** + * Compute the ad operator + */ + static Matrix9 adjointMap(const Vector9 &xi); + + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + static Vector9 adjoint(const Vector9 &xi, const Vector9 &y, + OptionalJacobian<9, 9> Hxi = boost::none); + + // temporary fix for wrappers until case issue is resolved + static Matrix9 adjointMap_(const Vector9 &xi) { return adjointMap(xi);} + static Vector9 adjoint_(const Vector9 &xi, const Vector9 &y) { return adjoint(xi, y);} + + /** + * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. + */ + static Vector9 adjointTranspose(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi = boost::none); + + /// Derivative of Expmap + static Matrix9 ExpmapDerivative(const Vector9& xi); + + /// Derivative of Logmap + static Matrix9 LogmapDerivative(const ExtendedPose3& xi); + + // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP + struct ChartAtOrigin { + static ExtendedPose3 Retract(const Vector9& xi, ChartJacobian Hxi = boost::none); + static Vector9 Local(const ExtendedPose3& pose, ChartJacobian Hpose = boost::none); + }; + + Vector9 boxminus(const ExtendedPose3& g) const; + + using LieGroup::inverse; // version with derivative + + /** + * wedge for ExtendedPose3: + * @param xi 9-dim twist (omega,nu,rho) + * @return 5*5 element of Lie algebra + */ + static Matrix wedge(double phix, double phiy, double phiz, double nux, double nuy, double nuz, double rhox, double rhoy, double rhoz) { + return (Matrix(5, 5) << 0., -phiz, phiy, nux, rhox, phiz, 0., -phix, nuy, rhoy, -phiy, phix, 0., nuz, rhoz, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.).finished(); + } + + /// @} + /// @name Group Action on Point3 + /// @{ + + /** + * @brief takes point in Pose coordinates and transforms it to world coordinates + * @param point point in Pose coordinates + * @param Hself optional 3*6 Jacobian wrpt this pose + * @param Hpoint optional 3*3 Jacobian wrpt point + * @return point in world coordinates + */ + Point3 transformFrom(const Point3& point, OptionalJacobian<3, 9> Hself = + boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; + + /** syntactic sugar for transformFrom */ + inline Point3 operator*(const Point3& point) const { + return transformFrom(point); + } + + /** + * @brief takes point in world coordinates and transforms it to Pose coordinates + * @param point point in world coordinates + * @param Hself optional 3*6 Jacobian wrpt this pose + * @param Hpoint optional 3*3 Jacobian wrpt point + * @return point in Pose coordinates + */ + Point3 transformTo(const Point3& point, OptionalJacobian<3, 9> Hself = + boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; + + + + /// @} + /// @name Standard Interface + /// @{ + + /// get rotation + const Rot3& rotation(OptionalJacobian<3, 9> Hself = boost::none) const; + + /// get translation + const Point3& velocity(OptionalJacobian<3, 9> Hself = boost::none) const; + + /// get position + const Point3& position(OptionalJacobian<3, 9> Hself = boost::none) const; + + const Point3& translation(OptionalJacobian<3, 9> Hself = boost::none) const; + + /// get x + double x() const { + return p_.x(); + } + + /// get y + double y() const { + return p_.y(); + } + + /// get z + double z() const { + return p_.z(); + } + + + /** convert to 5*5 matrix */ + Matrix5 matrix() const; + + /** + * Assuming self == wTa, takes a pose aTb in local coordinates + * and transforms it to world coordinates wTb = wTa * aTb. + * This is identical to compose. + */ + ExtendedPose3 transformPoseFrom(const ExtendedPose3& aTb, OptionalJacobian<9, 9> Hself = boost::none, OptionalJacobian<9, 9> HaTb = boost::none) const; + + /** + * Assuming self == wTa, takes a pose wTb in world coordinates + * and transforms it to local coordinates aTb = inv(wTa) * wTb + */ + ExtendedPose3 transformPoseTo(const ExtendedPose3& wTb, OptionalJacobian<9, 9> Hself = boost::none, OptionalJacobian<9, 9> HwTb = boost::none) const; + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @return range (double) + */ + double range(const Point3& point, OptionalJacobian<1, 9> Hself = boost::none, + OptionalJacobian<1, 3> Hpoint = boost::none) const; + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @return range (double) + */ + double range(const ExtendedPose3& pose, OptionalJacobian<1, 9> Hself = boost::none, OptionalJacobian<1, 9> Hpose = boost::none) const; + + /** + * Calculate bearing to a landmark + * @param point 3D location of landmark + * @return bearing (Unit3) + */ + Unit3 bearing(const Point3& point, OptionalJacobian<2, 9> Hself = boost::none, + OptionalJacobian<2, 3> Hpoint = boost::none) const; + + /** + * Calculate bearing to another pose + * @param other 3D location and orientation of other body. The orientation + * information is ignored. + * @return bearing (Unit3) + */ + Unit3 bearing(const ExtendedPose3& pose, OptionalJacobian<2, 9> Hself = boost::none, + OptionalJacobian<2, 9> Hpose = boost::none) const; + + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Return the start and end indices (inclusive) of the translation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + inline static std::pair velocityInterval() { + return std::make_pair(3, 5); + } + + /** + * Return the start and end indices (inclusive) of the translation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + inline static std::pair positionInterval() { + return std::make_pair(6, 8); + } + inline static std::pair translationInterval() { + return std::make_pair(6, 8); + } + + /** + * Return the start and end indices (inclusive) of the rotation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + static std::pair rotationInterval() { + return std::make_pair(0, 2); + } + + /// Output stream operator + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, const ExtendedPose3& p); + + + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(v_); + ar & BOOST_SERIALIZATION_NVP(p_); + } + /// @} + +#ifdef GTSAM_USE_QUATERNIONS + // Align if we are using Quaternions + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +#endif +}; +// ExtendedPose3 class + +/** + * wedge for ExtendedPose3: + * @param xi 9-dim twist (omega,nu,rho) + * @return 5*5 element of Lie algebra + */ +template<> +inline Matrix wedge(const Vector& xi) { + return ExtendedPose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5), xi(6), xi(7), xi(8)); +} + + + +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; + +// bearing and range traits, used in RangeFactor +template <> +struct Bearing : HasBearing {}; + +template<> +struct Bearing : HasBearing {}; + +template +struct Range : HasRange {}; +} // namespace gtsam diff --git a/gtsam/geometry/tests/testExtendedPose3.cpp b/gtsam/geometry/tests/testExtendedPose3.cpp new file mode 100644 index 0000000000..149249db85 --- /dev/null +++ b/gtsam/geometry/tests/testExtendedPose3.cpp @@ -0,0 +1,531 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testExtendedPose3.cpp + * @brief Unit tests for ExtendedPose3 class + */ + +#include +#include +#include + +#include // for operator += +using namespace boost::assign; + +#include +#include + +using namespace std; +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(ExtendedPose3) +GTSAM_CONCEPT_LIE_INST(ExtendedPose3) + +static const Point3 V(3,0.4,-2.2); +static const Point3 P(0.2,0.7,-2); +static const Rot3 R = Rot3::Rodrigues(0.3,0,0); +static const Point3 V2(-6.5,3.5,6.2); +static const Point3 P2(3.5,-8.2,4.2); +static const ExtendedPose3 T(R,V2,P2); +static const ExtendedPose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),V2,P2); +static const ExtendedPose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(5,6,7), Point3(1, 2, 3)); +static const double tol=1e-5; + +/* ************************************************************************* */ +TEST( ExtendedPose3, equals) +{ + ExtendedPose3 pose2 = T3; + EXPECT(T3.equals(pose2)); + ExtendedPose3 origin; + EXPECT(!T3.equals(origin)); +} + +/* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP +TEST( ExtendedPose3, retract_first_order) +{ + ExtendedPose3 id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(ExtendedPose3(R, Vector3(0,0,0), Point3(0,0,0)), id.retract(xi),1e-2)); + xi(3)=3;xi(4)=0.4;xi(5)=-2.2; + xi(6)=0.2;xi(7)=0.7;xi(8)=-2; + EXPECT(assert_equal(ExtendedPose3(R, V, P),id.retract(v),1e-2)); +} +#endif +/* ************************************************************************* */ +TEST( ExtendedPose3, retract_expmap) +{ + Vector xi = Z_9x1; xi(0) = 0.3; + ExtendedPose3 pose = ExtendedPose3::Expmap(xi); + EXPECT(assert_equal(ExtendedPose3(R, Point3(0,0,0), Point3(0,0,0)), pose, 1e-2)); + EXPECT(assert_equal(xi,ExtendedPose3::Logmap(pose),1e-2)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, expmap_a_full) +{ + ExtendedPose3 id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(expmap_default(id, xi), ExtendedPose3(R, Vector3(0,0,0), Point3(0,0,0)))); + xi(3)=-0.2;xi(4)=-0.394742;xi(5)=2.08998; + xi(6)=0.2;xi(7)=0.394742;xi(8)=-2.08998; + EXPECT(assert_equal(ExtendedPose3(R, -P, P),expmap_default(id, xi),1e-5)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, expmap_a_full2) +{ + ExtendedPose3 id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(expmap_default(id, xi), ExtendedPose3(R, Point3(0,0,0), Point3(0,0,0)))); + xi(3)=-0.2;xi(4)=-0.394742;xi(5)=2.08998; + xi(6)=0.2;xi(7)=0.394742;xi(8)=-2.08998; + EXPECT(assert_equal(ExtendedPose3(R, -P, P),expmap_default(id, xi),1e-5)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, expmap_b) +{ + ExtendedPose3 p1(Rot3(), Vector3(-100, 0, 0), Point3(100, 0, 0)); + ExtendedPose3 p2 = p1.retract((Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished()); + ExtendedPose3 expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0), Point3(100.0, 0.0, 0.0)); + EXPECT(assert_equal(expected, p2,1e-2)); +} + +/* ************************************************************************* */ +// test case for screw motion in the plane +namespace screwExtendedPose3 { + double a=0.3, c=cos(a), s=sin(a), w=0.3; + Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished(); + Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); + Point3 expectedV(0.29552, 0.0446635, 1); + Point3 expectedP(0.29552, 0.0446635, 1); + ExtendedPose3 expected(expectedR, expectedV, expectedP); +} + +/* ************************************************************************* */ +// Checks correct exponential map (Expmap) with brute force matrix exponential +TEST(ExtendedPose3, expmap_c_full) +{ + EXPECT(assert_equal(screwExtendedPose3::expected, expm(screwExtendedPose3::xi),1e-6)); + EXPECT(assert_equal(screwExtendedPose3::expected, ExtendedPose3::Expmap(screwExtendedPose3::xi),1e-6)); +} + +/* ************************************************************************* */ +// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) +TEST(ExtendedPose3, Adjoint_full) +{ + ExtendedPose3 expected = T * ExtendedPose3::Expmap( screwExtendedPose3::xi) * T.inverse(); + Vector xiprime = T.Adjoint( screwExtendedPose3::xi); + EXPECT(assert_equal(expected, ExtendedPose3::Expmap(xiprime), 1e-6)); + + ExtendedPose3 expected2 = T2 * ExtendedPose3::Expmap( screwExtendedPose3::xi) * T2.inverse(); + Vector xiprime2 = T2.Adjoint( screwExtendedPose3::xi); + EXPECT(assert_equal(expected2, ExtendedPose3::Expmap(xiprime2), 1e-6)); + + ExtendedPose3 expected3 = T3 * ExtendedPose3::Expmap( screwExtendedPose3::xi) * T3.inverse(); + Vector xiprime3 = T3.Adjoint( screwExtendedPose3::xi); + EXPECT(assert_equal(expected3, ExtendedPose3::Expmap(xiprime3), 1e-6)); +} + +/* ************************************************************************* */ +// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) +TEST(ExtendedPose3, Adjoint_hat) +{ + auto hat = [](const Vector& xi) { return ::wedge(xi); }; + Matrix5 expected = T.matrix() * hat( screwExtendedPose3::xi) * T.matrix().inverse(); + Matrix5 xiprime = hat(T.Adjoint( screwExtendedPose3::xi)); + + EXPECT(assert_equal(expected, xiprime, 1e-6)); + + Matrix5 expected2 = T2.matrix() * hat( screwExtendedPose3::xi) * T2.matrix().inverse(); + Matrix5 xiprime2 = hat(T2.Adjoint( screwExtendedPose3::xi)); + EXPECT(assert_equal(expected2, xiprime2, 1e-6)); + + Matrix5 expected3 = T3.matrix() * hat( screwExtendedPose3::xi) * T3.matrix().inverse(); + + Matrix5 xiprime3 = hat(T3.Adjoint( screwExtendedPose3::xi)); + EXPECT(assert_equal(expected3, xiprime3, 1e-6)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, expmaps_galore_full) +{ + Vector xi; ExtendedPose3 actual; + xi = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + actual = ExtendedPose3::Expmap(xi); + EXPECT(assert_equal(expm(xi), actual,1e-6)); + EXPECT(assert_equal(xi, ExtendedPose3::Logmap(actual),1e-6)); + + xi = (Vector(9) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6, -0.7, -0.8, -0.9).finished(); + for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { + Vector txi = xi*theta; + actual = ExtendedPose3::Expmap(txi); + EXPECT(assert_equal(expm(txi,30), actual,1e-6)); + Vector log = ExtendedPose3::Logmap(actual); + EXPECT(assert_equal(actual, ExtendedPose3::Expmap(log),1e-6)); + EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps + } + + // Works with large v as well, but expm needs 10 iterations! + xi = (Vector(9) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0, 12, 14, 45).finished(); + actual = ExtendedPose3::Expmap(xi); + EXPECT(assert_equal(expm(xi,10), actual,1e-5)); + EXPECT(assert_equal(xi, ExtendedPose3::Logmap(actual),1e-9)); +} + +/* ************************************************************************* */ +// Check position and its pushforward + +TEST(ExtendedPose3, position) { + Matrix actualH; + EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.position(actualH), 1e-8)); + Matrix numericalH = numericalDerivative11( + boost::bind(&ExtendedPose3::position, _1, boost::none), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + +/* ************************************************************************* */ +// Check rotation and its pushforward +TEST(ExtendedPose3, rotation) { + Matrix actualH; + EXPECT(assert_equal(R, T.rotation(actualH), 1e-8)); + + Matrix numericalH = numericalDerivative11( + boost::bind(&ExtendedPose3::rotation, _1, boost::none), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + +/* ************************************************************************* */ +// Check velocity and its pushforward +TEST(ExtendedPose3, velocity) { + Matrix actualH; + EXPECT(assert_equal(Point3(-6.5,3.5,6.2), T.velocity(actualH), 1e-8)); + Matrix numericalH = numericalDerivative11( + boost::bind(&ExtendedPose3::velocity, _1, boost::none), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, Adjoint_compose_full) +{ + // To debug derivatives of compose, assert that + // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 + const ExtendedPose3& T1 = T; + Vector x = (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished(); + ExtendedPose3 expected = T1 * ExtendedPose3::Expmap(x) * T2; + Vector y = T2.inverse().Adjoint(x); + ExtendedPose3 actual = T1 * T2 * ExtendedPose3::Expmap(y); + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +/* ************************************************************************* */ +// Check compose and its pushforward +// NOTE: testing::compose(t1,t2) = t1.compose(t2) (see lieProxies.h) +TEST( ExtendedPose3, compose ) +{ + Matrix actual = (T2*T2).matrix(); + + Matrix expected = T2.matrix()*T2.matrix(); + EXPECT(assert_equal(actual,expected,1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T2.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = numericalDerivative21(testing::compose, T2, T2); + + EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3)); + + Matrix numericalH2 = numericalDerivative22(testing::compose, T2, T2); + EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4)); +} + +/* ************************************************************************* */ +// Check compose and its pushforward, another case +TEST( ExtendedPose3, compose2 ) +{ + const ExtendedPose3& T1 = T; + Matrix actual = (T1*T2).matrix(); + Matrix expected = T1.matrix()*T2.matrix(); + EXPECT(assert_equal(actual,expected,1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T1.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = numericalDerivative21(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3)); + + Matrix numericalH2 = numericalDerivative22(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, inverse) +{ + Matrix actualDinverse; + Matrix actual = T.inverse(actualDinverse).matrix(); + Matrix expected = T.matrix().inverse(); + EXPECT(assert_equal(actual,expected,1e-8)); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); + EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, inverseDerivatives2) +{ + Rot3 R = Rot3::Rodrigues(0.3,0.4,-0.5); + Vector3 v(3.5,-8.2,4.2); + Point3 p(3.5,-8.2,4.2); + ExtendedPose3 T(R,v,p); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + Matrix actualDinverse; + T.inverse(actualDinverse); + EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); + EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, compose_inverse) +{ + Matrix actual = (T*T.inverse()).matrix(); + Matrix expected = I_5x5; + EXPECT(assert_equal(actual,expected,1e-8)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, Retract_LocalCoordinates) +{ + Vector9 d; + d << 1,2,3,4,5,6,7,8,9; d/=10; + const Rot3 R = Rot3::Retract(d.head<3>()); + ExtendedPose3 t = ExtendedPose3::Retract(d); + EXPECT(assert_equal(d, ExtendedPose3::LocalCoordinates(t))); +} +/* ************************************************************************* */ +TEST(ExtendedPose3, retract_localCoordinates) +{ + Vector9 d12; + d12 << 1,2,3,4,5,6,7,8,9; d12/=10; + ExtendedPose3 t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} +/* ************************************************************************* */ +TEST(ExtendedPose3, expmap_logmap) +{ + Vector d12 = Vector9::Constant(0.1); + ExtendedPose3 t1 = T, t2 = t1.expmap(d12); + EXPECT(assert_equal(d12, t1.logmap(t2))); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, retract_localCoordinates2) +{ + ExtendedPose3 t1 = T; + ExtendedPose3 t2 = T3; + ExtendedPose3 origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); + EXPECT(assert_equal(d12, -d21)); +} +/* ************************************************************************* */ +TEST(ExtendedPose3, manifold_expmap) +{ + ExtendedPose3 t1 = T; + ExtendedPose3 t2 = T3; + ExtendedPose3 origin; + Vector d12 = t1.logmap(t2); + EXPECT(assert_equal(t2, t1.expmap(d12))); + Vector d21 = t2.logmap(t1); + EXPECT(assert_equal(t1, t2.expmap(d21))); + + // Check that log(t1,t2)=-log(t2,t1) + EXPECT(assert_equal(d12,-d21)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, subgroups) +{ + // Frank - Below only works for correct "Agrawal06iros style expmap + // lines in canonical coordinates correspond to Abelian subgroups in SE(3) + Vector d = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + // exp(-d)=inverse(exp(d)) + EXPECT(assert_equal(ExtendedPose3::Expmap(-d),ExtendedPose3::Expmap(d).inverse())); + // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) + ExtendedPose3 T2 = ExtendedPose3::Expmap(2*d); + ExtendedPose3 T3 = ExtendedPose3::Expmap(3*d); + ExtendedPose3 T5 = ExtendedPose3::Expmap(5*d); + EXPECT(assert_equal(T5,T2*T3)); + EXPECT(assert_equal(T5,T3*T2)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, between ) +{ + ExtendedPose3 expected = T2.inverse() * T3; + Matrix actualDBetween1,actualDBetween2; + ExtendedPose3 actual = T2.between(T3, actualDBetween1,actualDBetween2); + EXPECT(assert_equal(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(testing::between , T2, T3); + EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3)); + + Matrix numericalH2 = numericalDerivative22(testing::between , T2, T3); + EXPECT(assert_equal(numericalH2,actualDBetween2,1e-5)); +} + + +/* ************************************************************************* */ +TEST( ExtendedPose3, adjointMap) { + Matrix res = ExtendedPose3::adjointMap( screwExtendedPose3::xi); + Matrix wh = skewSymmetric( screwExtendedPose3::xi(0), screwExtendedPose3::xi(1), screwExtendedPose3::xi(2)); + Matrix vh = skewSymmetric( screwExtendedPose3::xi(3), screwExtendedPose3::xi(4), screwExtendedPose3::xi(5)); + Matrix rh = skewSymmetric( screwExtendedPose3::xi(6), screwExtendedPose3::xi(7), screwExtendedPose3::xi(8)); + Matrix9 expected; + expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh; + EXPECT(assert_equal(expected,res,1e-5)); +} + +/* ************************************************************************* */ + +TEST( ExtendedPose3, ExpmapDerivative1) { + Matrix9 actualH; + Vector9 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + ExtendedPose3::Expmap(w,actualH); + Matrix expectedH = numericalDerivative21 >(&ExtendedPose3::Expmap, w, boost::none); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, LogmapDerivative) { + Matrix9 actualH; + Vector9 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0,7.0,8.0,9.0; + ExtendedPose3 p = ExtendedPose3::Expmap(w); + EXPECT(assert_equal(w, ExtendedPose3::Logmap(p,actualH), 1e-5)); + Matrix expectedH = numericalDerivative21 >(&ExtendedPose3::Logmap, p, boost::none); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, stream) +{ + ExtendedPose3 T; + std::ostringstream os; + os << T; + EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\nv:[0, 0, 0];\np:[0, 0, 0];\n"); +} + +//****************************************************************************** +TEST(ExtendedPose3 , Invariants) { + ExtendedPose3 id; + + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T3)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T3)); + + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T3)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T3)); +} + +//****************************************************************************** +TEST(ExtendedPose3 , LieGroupDerivatives) { + ExtendedPose3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T3); +} + +//****************************************************************************** +TEST(ExtendedPose3 , ChartDerivatives) { + ExtendedPose3 id; + if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { + CHECK_CHART_DERIVATIVES(id,id); +// CHECK_CHART_DERIVATIVES(id,T2); +// CHECK_CHART_DERIVATIVES(T2,id); +// CHECK_CHART_DERIVATIVES(T2,T3); + } +} + + +/* ************************************************************************* */ +TEST(ExtendedPose3, interpolate) { + EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0))); + EXPECT(assert_equal(T3, interpolate(T2,T3, 1.0))); +} + +/* ************************************************************************* */ + +TEST(ExtendedPose3, Create) { + Matrix93 actualH1, actualH2, actualH3; + ExtendedPose3 actual = ExtendedPose3::Create(R, V2, P2, actualH1, actualH2, actualH3); + EXPECT(assert_equal(T, actual)); + boost::function create = boost::bind(ExtendedPose3::Create,_1,_2,_3,boost::none,boost::none,boost::none); + EXPECT(assert_equal(numericalDerivative31(create, R, V2, P2), actualH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(create, R, V2, P2), actualH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(create, R, V2, P2), actualH3, 1e-9)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, print) { + std::stringstream redirectStream; + std::streambuf* ssbuf = redirectStream.rdbuf(); + std::streambuf* oldbuf = std::cout.rdbuf(); + // redirect cout to redirectStream + std::cout.rdbuf(ssbuf); + + ExtendedPose3 pose(Rot3::identity(), Vector3(1, 2, 3), Point3(1, 2, 3)); + // output is captured to redirectStream + pose.print(); + + // Generate the expected output + std::stringstream expected; + Vector3 velocity(1, 2, 3); + Point3 position(1, 2, 3); + +#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS + expected << "1\n" + "2\n" + "3;\n"; +#else + expected << "v:[" << velocity.x() << ", " << velocity.y() << ", " << velocity.z() << "]';\np:[" << position.x() << ", " << position.y() << ", " << position.z() << "]';\n"; +#endif + + // reset cout to the original stream + std::cout.rdbuf(oldbuf); + + // Get substring corresponding to position part + std::string actual = redirectStream.str().substr(38); + CHECK_EQUAL(expected.str(), actual); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From ad3e6b954e7a87520019806c1c273387e62e516e Mon Sep 17 00:00:00 2001 From: GAECHTER TOYA Stefan Date: Mon, 25 Nov 2024 18:05:31 +0100 Subject: [PATCH 3/8] Add Lie group pre-integration --- gtsam/navigation/LieGroupPreintegration.cpp | 200 ++++++++++++++++++ gtsam/navigation/LieGroupPreintegration.h | 135 ++++++++++++ .../tests/testLieGroupPreintegration.cpp | 114 ++++++++++ 3 files changed, 449 insertions(+) create mode 100644 gtsam/navigation/LieGroupPreintegration.cpp create mode 100644 gtsam/navigation/LieGroupPreintegration.h create mode 100644 gtsam/navigation/tests/testLieGroupPreintegration.cpp diff --git a/gtsam/navigation/LieGroupPreintegration.cpp b/gtsam/navigation/LieGroupPreintegration.cpp new file mode 100644 index 0000000000..c188792173 --- /dev/null +++ b/gtsam/navigation/LieGroupPreintegration.cpp @@ -0,0 +1,200 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LieGroupPreintegration.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + * @author Martin Brossard + **/ + +#include "LieGroupPreintegration.h" + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +LieGroupPreintegration::LieGroupPreintegration( + const boost::shared_ptr& p, const Bias& biasHat) : + PreintegrationBase(p, biasHat) { + resetIntegration(); +} + +//------------------------------------------------------------------------------ +void LieGroupPreintegration::resetIntegration() { + deltaTij_ = 0.0; + deltaXij_ = NavState(); + delRdelBiasOmega_.setZero(); + delPdelBiasAcc_.setZero(); + delPdelBiasOmega_.setZero(); + delVdelBiasAcc_.setZero(); + delVdelBiasOmega_.setZero(); +} + +//------------------------------------------------------------------------------ +bool LieGroupPreintegration::equals(const LieGroupPreintegration& other, + double tol) const { + return p_->equals(*other.p_, tol) && std::abs(deltaTij_ - other.deltaTij_) < tol + && biasHat_.equals(other.biasHat_, tol) + && deltaXij_.equals(other.deltaXij_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +void LieGroupPreintegration::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, + Matrix93* C) { + + // Correct for bias in the sensor frame + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); + + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + + // Save current rotation for updating Jacobians + const Rot3 oldRij = deltaXij_.attitude(); + + // Do update + deltaTij_ += dt; + updateFactor(acc, omega, dt, A, B, C); // functional + + if (p().body_P_sensor) { + //NOT checked + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B * D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } + + // Update Jacobians + // TODO(frank): Try same simplification as in new approach + Matrix3 D_acc_R; + oldRij.rotate(acc, D_acc_R); + const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; + + const Vector3 integratedOmega = omega * dt; + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt; + + double dt22 = 0.5 * dt * dt; + const Matrix3 dRij = oldRij.matrix(); // expensive + delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; + delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; + delVdelBiasAcc_ += -dRij * dt; + delVdelBiasOmega_ += D_acc_biasOmega * dt; +} + +//------------------------------------------------------------------------------ +void LieGroupPreintegration::updateFactor(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, + OptionalJacobian<9, 3> G2) { + + Vector9 xi; + Matrix39 D_xiP_state; + Vector3 b_v = deltaXij_.bodyVelocity(F ? &D_xiP_state : 0); + double dt22 = 0.5 * dt * dt; + + // Integrate on tangent space. TODO(frank): coriolis? + deltaXij_.dR(xi) << dt * b_omega; + deltaXij_.dP(xi) << dt * b_v + dt22 * b_acceleration; + deltaXij_.dV(xi) << dt * b_acceleration; + + // Bring back to manifold + Matrix9 D_newState_xi; + deltaXij_ = deltaXij_.retract(xi, F, G1 || G2 ? &D_newState_xi : 0); + + // Derivative wrt state is computed by retract directly + // However, as dP(xi) also depends on state, we need to add that contribution + if (F) { + F->middleRows<3>(3) += dt * F->block<3,3>(3,3) * D_xiP_state; + } + // derivative wrt acceleration + if (G1) { + // D_newState_dPxi = D_newState_xi.middleCols<3>(3) + // D_dPxi_acc = dt22 * I_3x3 + // D_newState_dVxi = D_newState_xi.rightCols<3>() + // D_dVxi_acc = dt * I_3x3 + // *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc + *G1 = D_newState_xi.middleCols<3>(3) * dt22 + + D_newState_xi.rightCols<3>() * dt; + } + // derivative wrt omega + if (G2) { + // D_newState_dRxi = D_newState_xi.leftCols<3>() + // D_dRxi_omega = dt * I_3x3 + // *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega + *G2 = D_newState_xi.leftCols<3>() * dt; + } + +} + +//------------------------------------------------------------------------------ +Vector9 LieGroupPreintegration::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // Correct deltaRij, derivative is delRdelBiasOmega_ + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + Matrix3 D_correctedRij_bias, D_dR_correctedRij; + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + H ? &D_correctedRij_bias : 0); + if (H) + D_correctedRij_bias *= delRdelBiasOmega_; + + Rot3 R = Rot3::Expmap(biasInducedOmega); + ExtendedPose3 T0(deltaRij(),deltaPij(),deltaVij()); + Vector9 xi_corr; + Matrix9 H2, H3; + xi_corr << delRdelBiasOmega_ * biasIncr.gyroscope(), + delPdelBiasAcc_ * biasIncr.accelerometer() + delPdelBiasOmega_ * biasIncr.gyroscope(), + delVdelBiasAcc_ * biasIncr.accelerometer() + + delVdelBiasOmega_ * biasIncr.gyroscope(); + + ExtendedPose3 Tplus = ExtendedPose3::Expmap(xi_corr, H2); + Vector9 xi; + ExtendedPose3 T(correctedRij, + T0.position() + Tplus.position(), + T0.velocity() + Tplus.velocity()); + xi = T.boxminus(T); + Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); + + if (H) { + Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; + Matrix3 Qv = H2.block<3,3>(6,0); + Matrix3 Qt = H2.block<3,3>(3,0); + Matrix3 Jw = H2.block<3,3>(0,0); + + D_dR_bias << Z_3x3, D_dR_correctedRij*D_correctedRij_bias; + D_dP_bias << Jw*delPdelBiasAcc_, Jw*delPdelBiasOmega_ + Qt*delRdelBiasOmega_; + D_dV_bias << Jw*delVdelBiasAcc_, Jw*delVdelBiasOmega_ + Qv*delRdelBiasOmega_; + (*H) << D_dR_bias, R.matrix()*D_dP_bias, R.matrix()*D_dV_bias; + (*H) = (*H); + } + return xi; +} + +//------------------------------------------------------------------------------ + +}// namespace gtsam diff --git a/gtsam/navigation/LieGroupPreintegration.h b/gtsam/navigation/LieGroupPreintegration.h new file mode 100644 index 0000000000..8200ce8d14 --- /dev/null +++ b/gtsam/navigation/LieGroupPreintegration.h @@ -0,0 +1,135 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LieGroupPreintegration.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + * @author Martin Brossard + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * IMU pre-integration on SE_2(3) Lie group. + */ +class GTSAM_EXPORT LieGroupPreintegration : public PreintegrationBase { + protected: + + /** + * Pre-integrated navigation state, from frame i to frame j + * Note: relative position does not take into account velocity at time i, see deltap+, in [2] + * Note: velocity is now also in frame i, as opposed to deltaVij in [2] + */ + NavState deltaXij_; + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + + /// Default constructor for serialization + LieGroupPreintegration() { + resetIntegration(); + } + +public: + /// @name Constructors + /// @{ + + /** + * Constructor, initializes the variables in the base class + * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases + */ + LieGroupPreintegration(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements + void resetIntegration() override; + + /// @} + + /// @name Instance variables access + /// @{ + NavState deltaXij() const override { return deltaXij_; } + Rot3 deltaRij() const override { return deltaXij_.attitude(); } + Vector3 deltaPij() const override { return deltaXij_.position(); } + Vector3 deltaVij() const override { return deltaXij_.velocity(); } + + Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; } + Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; } + Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; } + Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; } + Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; } + + /// @name Testable + /// @{ + bool equals(const LieGroupPreintegration& other, double tol) const; + /// @} + + /// @name Main functionality + /// @{ + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) override; + + // Integrate forward in time given angular velocity and acceleration in body frame + /// Uses second order integration for position, returns derivatives except dt. + void updateFactor(const Vector3& b_acceleration, const Vector3& b_omega, + const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, + OptionalJacobian<9, 3> G2); + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const override; + + /** Dummy clone for MATLAB */ + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(); + } + + /// @} + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(deltaXij_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testLieGroupPreintegration.cpp b/gtsam/navigation/tests/testLieGroupPreintegration.cpp new file mode 100644 index 0000000000..1de7c19529 --- /dev/null +++ b/gtsam/navigation/tests/testLieGroupPreintegration.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testLieGroupPreintegration.cpp + * @brief Unit test for the LieGroupPreintegration + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include "imuFactorTesting.h" + +namespace testing { +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr Params() { + auto p = PreintegrationParams::MakeSharedD(kGravity); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0001 * I_3x3; + return p; +} +} + +/* ************************************************************************* */ +TEST(LieGroupPreintegration, BiasCorrectionJacobians) { + testing::SomeMeasurements measurements; + + boost::function deltaRij = + [=](const Vector3& a, const Vector3& w) { + LieGroupPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaRij(); + }; + + boost::function deltaPij = + [=](const Vector3& a, const Vector3& w) { + LieGroupPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaPij(); + }; + + boost::function deltaVij = + [=](const Vector3& a, const Vector3& w) { + LieGroupPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaVij(); + }; + + // Actual pre-integrated values + LieGroupPreintegration pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + EXPECT( + assert_equal(numericalDerivative21(deltaRij, kZero, kZero), + Matrix3(Z_3x3))); + EXPECT( + assert_equal(numericalDerivative22(deltaRij, kZero, kZero), + pim.delRdelBiasOmega(), 1e-3)); + + EXPECT( + assert_equal(numericalDerivative21(deltaPij, kZero, kZero), + pim.delPdelBiasAcc())); + EXPECT( + assert_equal(numericalDerivative22(deltaPij, kZero, kZero), + pim.delPdelBiasOmega(), 1e-3)); + + EXPECT( + assert_equal(numericalDerivative21(deltaVij, kZero, kZero), + pim.delVdelBiasAcc())); + EXPECT( + assert_equal(numericalDerivative22(deltaVij, kZero, kZero), + pim.delVdelBiasOmega(), 1e-3)); +} + +/* ************************************************************************* */ +TEST(LieGroupPreintegration, computeError) { + LieGroupPreintegration pim(testing::Params()); + NavState x1, x2; + imuBias::ConstantBias bias; + Matrix9 aH1, aH2; + Matrix96 aH3; + pim.computeError(x1, x2, bias, aH1, aH2, aH3); + boost::function f = + boost::bind(&LieGroupPreintegration::computeError, pim, _1, _2, _3, + boost::none, boost::none, boost::none); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 05d54bb1cb68b5cadf6af1b0f0c220c663cdfcc2 Mon Sep 17 00:00:00 2001 From: GAECHTER TOYA Stefan Date: Mon, 25 Nov 2024 18:12:47 +0100 Subject: [PATCH 4/8] Use extended pose in NavState --- gtsam/navigation/NavState.cpp | 35 ++++++++++++++----------- gtsam/navigation/NavState.h | 1 + gtsam/navigation/tests/testNavState.cpp | 14 +++++----- 3 files changed, 27 insertions(+), 23 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 3e2817752d..82c7d54e6b 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -103,6 +103,16 @@ bool NavState::equals(const NavState& other, double tol) const { && equal_with_abs_tol(v_, other.v_, tol); } +//------------------------------------------------------------------------------ +#ifdef GTSAM_EXTENDED_POSE_RETRACT +NavState NavState::retract(const Vector9& xi, // + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + TIE(nRb, n_t, n_v, *this); + ExtendedPose3 T(nRb, n_t, n_v); // flip velocity and position + T = T.retract(xi, H1, H2); + return NavState(T.rotation(), T.velocity(), T.position()); +} +#else //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { @@ -129,27 +139,20 @@ NavState NavState::retract(const Vector9& xi, // } return NavState(nRc, t, v); } +#endif //------------------------------------------------------------------------------ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { - Matrix3 D_dR_R, D_dt_R, D_dv_R; - const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); - const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); - const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); - - Vector9 xi; - Matrix3 D_xi_R; - xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; - if (H1) { - *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // - D_dt_R, -I_3x3, Z_3x3, // - D_dv_R, Z_3x3, -I_3x3; - } + const ExtendedPose3 T0(R_, t_, v_); + const ExtendedPose3 T(g.R_, g.t_, g.v_); + const ExtendedPose3 DeltaT(T0.inverse()*T); + const Vector9 xi = T.Logmap(DeltaT, H1); if (H2) { - *H2 << D_xi_R, Z_3x3, Z_3x3, // - Z_3x3, dR.matrix(), Z_3x3, // - Z_3x3, Z_3x3, dR.matrix(); + *H2 = *H1; + } + if (H1) { + *H1 = -DeltaT.AdjointMap() * (*H1); } return xi; } diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index f778a71231..8cdc6bdae8 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index b8f0815189..2be71f2005 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -41,7 +41,7 @@ TEST(NavState, Constructor) { std::function create = std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, nullptr, nullptr, nullptr); - Matrix aH1, aH2, aH3; + Matrix93 aH1, aH2, aH3; EXPECT( assert_equal(kState1, NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3))); @@ -53,7 +53,7 @@ TEST(NavState, Constructor) { numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); EXPECT( assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); + numericalDerivative33(create, kAttitude, kPosition, kVelocity), aH3)); } /* ************************************************************************* */ @@ -200,10 +200,10 @@ TEST(NavState, Coriolis2) { } TEST(NavState, Coriolis3) { - /** Consider a massless planet with an attached nav frame at - * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with + /** Consider a massless planet with an attached nav frame at + * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with * velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously - * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and + * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and * second order Coriolis corrections are as expected. */ @@ -216,9 +216,9 @@ TEST(NavState, Coriolis3) { bRn = nRb.inverse(); // Get expected first and second order corrections in the nav frame - Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, + Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2), - n_dV1e = dt * n_aCorr1, + n_dV1e = dt * n_aCorr1, n_dV2e = dt * (n_aCorr1 + n_aCorr2); // Get expected first and second order corrections in the body frame From a1351b87da13ec88649399f633df34062122115f Mon Sep 17 00:00:00 2001 From: GAECHTER TOYA Stefan Date: Mon, 25 Nov 2024 18:13:13 +0100 Subject: [PATCH 5/8] Enable Lie group pre-integration --- gtsam/navigation/CombinedImuFactor.h | 7 +++++++ gtsam/navigation/ImuFactor.h | 5 +++++ 2 files changed, 12 insertions(+) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 0ffb386a05..5e4a54de25 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -24,14 +24,19 @@ /* GTSAM includes */ #include +#include namespace gtsam { #ifdef GTSAM_TANGENT_PREINTEGRATION typedef TangentPreintegration PreintegrationType; #else +#ifdef GTSAM_LIEGROUP_PREINTEGRATION +typedef LieGroupPreintegration PreintegrationType; +#else typedef ManifoldPreintegration PreintegrationType; #endif +#endif /* * If you are using the factor, please cite: @@ -53,6 +58,8 @@ typedef ManifoldPreintegration PreintegrationType; * Robotics: Science and Systems (RSS), 2015. */ + + /** * PreintegratedCombinedMeasurements integrates the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 7254838fd4..de8b6023a1 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -24,6 +24,7 @@ /* GTSAM includes */ #include #include +#include #include #include @@ -32,8 +33,12 @@ namespace gtsam { #ifdef GTSAM_TANGENT_PREINTEGRATION typedef TangentPreintegration PreintegrationType; #else +#ifdef GTSAM_LIEGROUP_PREINTEGRATION +typedef LieGroupPreintegration PreintegrationType; +#else typedef ManifoldPreintegration PreintegrationType; #endif +#endif /* * If you are using the factor, please cite: From 4e289b67be975490c72c25287ef1b8ee6fd14d71 Mon Sep 17 00:00:00 2001 From: GAECHTER TOYA Stefan Date: Mon, 25 Nov 2024 18:13:47 +0100 Subject: [PATCH 6/8] Consider rotating earth --- gtsam/navigation/NavState.cpp | 90 ++++++++++++++++--------- gtsam/navigation/PreintegrationBase.cpp | 38 ++++++++--- 2 files changed, 86 insertions(+), 42 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 82c7d54e6b..bf7a021383 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -221,37 +221,32 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); - // Get perturbations in nav frame - Vector9 n_xi, xi; - Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav; - dR(n_xi) << ((-dt) * omega); - dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper - dV(n_xi) << ((-2.0 * dt) * omega_cross_vel); + Vector9 xi; + Matrix3 D_dP_R, D_dP_dR, D_dV_dR; + dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0); + dP(xi) << nRb.unrotate((-dt2) * omega_cross_vel, H ? &D_dP_dR : 0); + dV(xi) << nRb.unrotate((-2.0 * dt) * omega_cross_vel, H ? &D_dV_dR : 0); + if (secondOrder) { const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t)); - dP(n_xi) -= (0.5 * dt2) * omega_cross2_t; - dV(n_xi) -= dt * omega_cross2_t; + dP(xi) -= (0.5 * dt2) * omega_cross2_t; + dV(xi) -= dt * omega_cross2_t; } - - // Transform n_xi into the body frame - xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0), - nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0), - nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0); - if (H) { H->setZero(); const Matrix3 Omega = skewSymmetric(omega); const Matrix3 D_cross_state = Omega * R(); H->setZero(); - D_R_R(H) << D_dR_R; - D_t_v(H) << D_body_nav * (-dt2) * D_cross_state; - D_t_R(H) << D_dP_R; - D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state; - D_v_R(H) << D_dV_R; + D_R_R(H) << D_dP_R; + D_t_v(H) << nRb.transpose() * (-dt2) * D_cross_state; + D_v_v(H) << nRb.transpose() * (-2.0 * dt) * D_cross_state; + D_t_R(H) << D_dP_dR; + D_v_R(H) << D_dV_dR; + if (secondOrder) { const Matrix3 D_cross2_state = Omega * D_cross_state; - D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state; - D_v_t(H) -= D_body_nav * dt * D_cross2_state; + D_t_t(H) -= (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= dt * D_cross2_state; } } return xi; @@ -264,29 +259,58 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt, OptionalJacobian<9, 9> H2) const { const Rot3& nRb = R_; const Velocity3& n_v = v_; // derivative is Ri ! + const Point3& n_t = t_; // derivative is Ri ! const double dt22 = 0.5 * dt * dt; Vector9 xi; - Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri; - dR(xi) = dR(pim); - dP(xi) = dP(pim) - + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0) - + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0); - dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0); + Matrix3 D_dP_Ri, D_dV_Ri, D_dP_Gx, D_dV_Gv; + Matrix3 D_Gx_nt = Matrix::Zero(3, 3); + Matrix3 D_Gv_nt = Matrix::Zero(3, 3); + Matrix3 D_Gx_nv = dt * Matrix::Identity(3, 3); + Vector3 Gamma_v, Gamma_p; if (omegaCoriolis) { - xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); + Vector6 omega_n_gravity; + omega_n_gravity << -(*omegaCoriolis)*dt, n_gravity*dt; + Pose3 pose = Pose3::Expmap(omega_n_gravity); + Rot3 Gamma_R = pose.rotation(); + Matrix3 GOmGt = skewSymmetric(Gamma_R.rotate(*omegaCoriolis)); + Vector3 n_v_prime = n_v + GOmGt * n_t; + + // add initial state contribution + Gamma_v = pose.translation() + GOmGt * n_t; + // add velocity and initial position contribution + Matrix3 Omega = skewSymmetric(*omegaCoriolis); + double phi = (*omegaCoriolis).norm(); + double c = phi * dt; + double phi3 = phi * phi * phi; + double phi4 = phi3 * phi; + double a = (c * cos(c) - sin(c)) /(phi3); + double b = (c*c/2 - cos(c) - c*sin(c) + 1) / (phi4); + Matrix3 mat = dt22 * Matrix::Identity(3, 3); + mat += a * Omega + b * Omega * Omega; + Gamma_p = dt * n_v_prime + mat * n_gravity; + + D_Gv_nt = GOmGt; + D_Gx_nt = dt * GOmGt; + } else { + Gamma_v = dt * n_gravity; + Gamma_p = dt * n_v + dt22 * n_gravity; } + dR(xi) = dR(pim); + dP(xi) = dP(pim)+nRb.unrotate(Gamma_p, H1 ? &D_dP_Ri : 0, H1 ? &D_dP_Gx : 0); + dV(xi) = dV(pim)+nRb.unrotate(Gamma_v, H1 ? &D_dV_Ri : 0, H1 ? &D_dV_Gv : 0); if (H1 || H2) { Matrix3 Ri = nRb.matrix(); if (H1) { - if (!omegaCoriolis) - H1->setZero(); // if coriolis H1 is already initialized - D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2; - D_t_v(H1) += dt * D_dP_nv * Ri; - D_v_R(H1) += dt * D_dV_Ri; + H1->setZero(); // if coriolis H1 is already initialized + D_t_R(H1) += D_dP_Ri; + D_t_v(H1) += D_dP_Gx * D_Gx_nv * Ri; + D_v_R(H1) += D_dV_Ri; + D_t_t(H1) += D_dP_Gx * D_Gx_nt * Ri; + D_v_t(H1) += D_dV_Gv * D_Gv_nt * Ri; } if (H2) { H2->setIdentity(); diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 7f998987bd..8c4ebd9076 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -117,23 +117,43 @@ NavState PreintegrationBase::predict(const NavState& state_i, OptionalJacobian<9, 6> H2) const { Matrix96 D_biasCorrected_bias; Vector9 biasCorrected = biasCorrectedDelta(bias_i, - H2 ? &D_biasCorrected_bias : nullptr); + H2 ? &D_biasCorrected_bias : nullptr); + + // Correct for Earth rate + NavState state_ii = state_i; + if (p().omegaCoriolis){ + const Vector3 omega = *(p().omegaCoriolis); + const Rot3 Gamma_R = Rot3::Expmap(-omega*deltaTij_); + + state_ii = NavState(Gamma_R * state_i.attitude(), + Gamma_R * state_i.position(), + Gamma_R * state_i.velocity()); + // Jacobian is identity + } // Correct for initial velocity and gravity Matrix9 D_delta_state, D_delta_biasCorrected; - Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, - p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr, - H2 ? &D_delta_biasCorrected : nullptr); + Vector9 xi = state_ii.correctPIM(biasCorrected, deltaTij_, p().n_gravity, + p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr, + H2 ? &D_delta_biasCorrected : nullptr); // Use retract to get back to NavState manifold Matrix9 D_predict_state, D_predict_delta; - NavState state_j = state_i.retract(xi, - H1 ? &D_predict_state : nullptr, - H1 || H2 ? &D_predict_delta : nullptr); + NavState state_j = state_ii.retract(xi, D_predict_state, D_predict_delta); + + Matrix99 H = Matrix99::Identity(9, 9); + if (p().omegaCoriolis){ + // apply v_t = v'_t - Omega p_t + const Rot3 nRb = state_j.attitude(); + const Matrix3 Omega = skewSymmetric(*(p().omegaCoriolis)); + state_j = NavState(nRb, state_j.position(), + state_j.velocity() - Omega * state_j.position()); + H2 || H2 ? &D_predict_delta : nullptr); + } if (H1) - *H1 = D_predict_state + D_predict_delta * D_delta_state; + *H1 = H * (D_predict_state + D_predict_delta * D_delta_state); if (H2) - *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; + *H2 = H * (D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias); return state_j; } From bdb54329b00e837246972531804ffd57972e93b4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 10 Jan 2025 15:29:25 -0500 Subject: [PATCH 7/8] Fix boost issues --- gtsam/geometry/tests/testExtendedPose3.cpp | 22 +++---- gtsam/navigation/LieGroupPreintegration.cpp | 6 +- gtsam/navigation/LieGroupPreintegration.h | 10 +-- gtsam/navigation/PreintegrationBase.cpp | 5 +- .../tests/testLieGroupPreintegration.cpp | 61 ++++++++----------- 5 files changed, 49 insertions(+), 55 deletions(-) diff --git a/gtsam/geometry/tests/testExtendedPose3.cpp b/gtsam/geometry/tests/testExtendedPose3.cpp index 149249db85..bd1e779749 100644 --- a/gtsam/geometry/tests/testExtendedPose3.cpp +++ b/gtsam/geometry/tests/testExtendedPose3.cpp @@ -18,9 +18,6 @@ #include #include -#include // for operator += -using namespace boost::assign; - #include #include @@ -38,7 +35,6 @@ static const Point3 P2(3.5,-8.2,4.2); static const ExtendedPose3 T(R,V2,P2); static const ExtendedPose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),V2,P2); static const ExtendedPose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(5,6,7), Point3(1, 2, 3)); -static const double tol=1e-5; /* ************************************************************************* */ TEST( ExtendedPose3, equals) @@ -193,7 +189,7 @@ TEST(ExtendedPose3, position) { Matrix actualH; EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.position(actualH), 1e-8)); Matrix numericalH = numericalDerivative11( - boost::bind(&ExtendedPose3::position, _1, boost::none), T); + std::bind(&ExtendedPose3::position, std::placeholders::_1, nullptr), T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -204,7 +200,7 @@ TEST(ExtendedPose3, rotation) { EXPECT(assert_equal(R, T.rotation(actualH), 1e-8)); Matrix numericalH = numericalDerivative11( - boost::bind(&ExtendedPose3::rotation, _1, boost::none), T); + std::bind(&ExtendedPose3::rotation, std::placeholders::_1, nullptr), T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -214,7 +210,7 @@ TEST(ExtendedPose3, velocity) { Matrix actualH; EXPECT(assert_equal(Point3(-6.5,3.5,6.2), T.velocity(actualH), 1e-8)); Matrix numericalH = numericalDerivative11( - boost::bind(&ExtendedPose3::velocity, _1, boost::none), T); + std::bind(&ExtendedPose3::velocity, std::placeholders::_1, nullptr), T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -411,7 +407,7 @@ TEST( ExtendedPose3, ExpmapDerivative1) { Vector9 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; ExtendedPose3::Expmap(w,actualH); Matrix expectedH = numericalDerivative21 >(&ExtendedPose3::Expmap, w, boost::none); + OptionalJacobian<9, 9> >(&ExtendedPose3::Expmap, w, nullptr); EXPECT(assert_equal(expectedH, actualH)); } @@ -422,7 +418,7 @@ TEST( ExtendedPose3, LogmapDerivative) { ExtendedPose3 p = ExtendedPose3::Expmap(w); EXPECT(assert_equal(w, ExtendedPose3::Logmap(p,actualH), 1e-5)); Matrix expectedH = numericalDerivative21 >(&ExtendedPose3::Logmap, p, boost::none); + OptionalJacobian<9, 9> >(&ExtendedPose3::Logmap, p, nullptr); EXPECT(assert_equal(expectedH, actualH)); } @@ -484,7 +480,9 @@ TEST(ExtendedPose3, Create) { Matrix93 actualH1, actualH2, actualH3; ExtendedPose3 actual = ExtendedPose3::Create(R, V2, P2, actualH1, actualH2, actualH3); EXPECT(assert_equal(T, actual)); - boost::function create = boost::bind(ExtendedPose3::Create,_1,_2,_3,boost::none,boost::none,boost::none); + std::function create = std::bind( + ExtendedPose3::Create, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, nullptr, nullptr, nullptr); EXPECT(assert_equal(numericalDerivative31(create, R, V2, P2), actualH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(create, R, V2, P2), actualH2, 1e-9)); EXPECT(assert_equal(numericalDerivative33(create, R, V2, P2), actualH3, 1e-9)); @@ -498,7 +496,7 @@ TEST(ExtendedPose3, print) { // redirect cout to redirectStream std::cout.rdbuf(ssbuf); - ExtendedPose3 pose(Rot3::identity(), Vector3(1, 2, 3), Point3(1, 2, 3)); + ExtendedPose3 pose(Rot3(), Vector3(1, 2, 3), Point3(1, 2, 3)); // output is captured to redirectStream pose.print(); @@ -520,7 +518,7 @@ TEST(ExtendedPose3, print) { // Get substring corresponding to position part std::string actual = redirectStream.str().substr(38); - CHECK_EQUAL(expected.str(), actual); + CHECK(expected.str() == actual); } /* ************************************************************************* */ diff --git a/gtsam/navigation/LieGroupPreintegration.cpp b/gtsam/navigation/LieGroupPreintegration.cpp index c188792173..6e6c4b1cd1 100644 --- a/gtsam/navigation/LieGroupPreintegration.cpp +++ b/gtsam/navigation/LieGroupPreintegration.cpp @@ -28,7 +28,7 @@ namespace gtsam { //------------------------------------------------------------------------------ LieGroupPreintegration::LieGroupPreintegration( - const boost::shared_ptr& p, const Bias& biasHat) : + const std::shared_ptr& p, const Bias& biasHat) : PreintegrationBase(p, biasHat) { resetIntegration(); } @@ -69,7 +69,7 @@ void LieGroupPreintegration::update(const Vector3& measuredAcc, // Possibly correct for sensor pose Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; if (p().body_P_sensor) - boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); // Save current rotation for updating Jacobians @@ -158,7 +158,7 @@ Vector9 LieGroupPreintegration::biasCorrectedDelta( const imuBias::ConstantBias biasIncr = bias_i - biasHat_; Matrix3 D_correctedRij_bias, D_dR_correctedRij; const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); - const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, {}, H ? &D_correctedRij_bias : 0); if (H) D_correctedRij_bias *= delRdelBiasOmega_; diff --git a/gtsam/navigation/LieGroupPreintegration.h b/gtsam/navigation/LieGroupPreintegration.h index 8200ce8d14..ddf40e6aa7 100644 --- a/gtsam/navigation/LieGroupPreintegration.h +++ b/gtsam/navigation/LieGroupPreintegration.h @@ -59,7 +59,7 @@ class GTSAM_EXPORT LieGroupPreintegration : public PreintegrationBase { * @param p Parameters, typically fixed in a single application * @param bias Current estimate of acceleration and rotation rate biases */ - LieGroupPreintegration(const boost::shared_ptr& p, + LieGroupPreintegration(const std::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); /// @} @@ -107,17 +107,18 @@ class GTSAM_EXPORT LieGroupPreintegration : public PreintegrationBase { /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const override; + OptionalJacobian<9, 6> H = {}) const override; /** Dummy clone for MATLAB */ - virtual boost::shared_ptr clone() const { - return boost::shared_ptr(); + virtual std::shared_ptr clone() const { + return std::shared_ptr(); } /// @} private: /** Serialization function */ +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { @@ -130,6 +131,7 @@ class GTSAM_EXPORT LieGroupPreintegration : public PreintegrationBase { ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); } +#endif }; } /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 2474fde24d..e5f089fd8d 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -141,7 +141,9 @@ NavState PreintegrationBase::predict(const NavState& state_i, // Use retract to get back to NavState manifold Matrix9 D_predict_state, D_predict_delta; - NavState state_j = state_ii.retract(xi, D_predict_state, D_predict_delta); + NavState state_j = state_i.retract(xi, + H1 ? &D_predict_state : nullptr, + H1 || H2 ? &D_predict_delta : nullptr); Matrix99 H = Matrix99::Identity(9, 9); if (p().omegaCoriolis){ @@ -150,7 +152,6 @@ NavState PreintegrationBase::predict(const NavState& state_i, const Matrix3 Omega = skewSymmetric(*(p().omegaCoriolis)); state_j = NavState(nRb, state_j.position(), state_j.velocity() - Omega * state_j.position()); - H2 || H2 ? &D_predict_delta : nullptr); } if (H1) *H1 = H * (D_predict_state + D_predict_delta * D_delta_state); diff --git a/gtsam/navigation/tests/testLieGroupPreintegration.cpp b/gtsam/navigation/tests/testLieGroupPreintegration.cpp index 1de7c19529..db26985134 100644 --- a/gtsam/navigation/tests/testLieGroupPreintegration.cpp +++ b/gtsam/navigation/tests/testLieGroupPreintegration.cpp @@ -15,47 +15,45 @@ * @author Luca Carlone */ -#include +#include #include -#include +#include #include #include - -#include -#include +#include #include "imuFactorTesting.h" namespace testing { // Create default parameters with Z-down and above noise parameters -static boost::shared_ptr Params() { +static std::shared_ptr Params() { auto p = PreintegrationParams::MakeSharedD(kGravity); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; p->integrationCovariance = 0.0001 * I_3x3; return p; } -} +} // namespace testing /* ************************************************************************* */ TEST(LieGroupPreintegration, BiasCorrectionJacobians) { testing::SomeMeasurements measurements; - boost::function deltaRij = + std::function deltaRij = [=](const Vector3& a, const Vector3& w) { LieGroupPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.deltaRij(); }; - boost::function deltaPij = + std::function deltaPij = [=](const Vector3& a, const Vector3& w) { LieGroupPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.deltaPij(); }; - boost::function deltaVij = + std::function deltaVij = [=](const Vector3& a, const Vector3& w) { LieGroupPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); @@ -66,40 +64,35 @@ TEST(LieGroupPreintegration, BiasCorrectionJacobians) { LieGroupPreintegration pim(testing::Params()); testing::integrateMeasurements(measurements, &pim); - EXPECT( - assert_equal(numericalDerivative21(deltaRij, kZero, kZero), - Matrix3(Z_3x3))); - EXPECT( - assert_equal(numericalDerivative22(deltaRij, kZero, kZero), - pim.delRdelBiasOmega(), 1e-3)); - - EXPECT( - assert_equal(numericalDerivative21(deltaPij, kZero, kZero), - pim.delPdelBiasAcc())); - EXPECT( - assert_equal(numericalDerivative22(deltaPij, kZero, kZero), - pim.delPdelBiasOmega(), 1e-3)); - - EXPECT( - assert_equal(numericalDerivative21(deltaVij, kZero, kZero), - pim.delVdelBiasAcc())); - EXPECT( - assert_equal(numericalDerivative22(deltaVij, kZero, kZero), - pim.delVdelBiasOmega(), 1e-3)); + EXPECT(assert_equal(numericalDerivative21(deltaRij, kZero, kZero), + Matrix3(Z_3x3))); + EXPECT(assert_equal(numericalDerivative22(deltaRij, kZero, kZero), + pim.delRdelBiasOmega(), 1e-3)); + + EXPECT(assert_equal(numericalDerivative21(deltaPij, kZero, kZero), + pim.delPdelBiasAcc())); + EXPECT(assert_equal(numericalDerivative22(deltaPij, kZero, kZero), + pim.delPdelBiasOmega(), 1e-3)); + + EXPECT(assert_equal(numericalDerivative21(deltaVij, kZero, kZero), + pim.delVdelBiasAcc())); + EXPECT(assert_equal(numericalDerivative22(deltaVij, kZero, kZero), + pim.delVdelBiasOmega(), 1e-3)); } /* ************************************************************************* */ TEST(LieGroupPreintegration, computeError) { + using namespace std::placeholders; LieGroupPreintegration pim(testing::Params()); NavState x1, x2; imuBias::ConstantBias bias; Matrix9 aH1, aH2; Matrix96 aH3; pim.computeError(x1, x2, bias, aH1, aH2, aH3); - boost::function f = - boost::bind(&LieGroupPreintegration::computeError, pim, _1, _2, _3, - boost::none, boost::none, boost::none); + std::function + f = std::bind(&LieGroupPreintegration::computeError, pim, _1, _2, _3, + nullptr, nullptr, nullptr); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); From 62b22b2e5537130ef460fb0946c5bcb19bb382a2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 10 Jan 2025 15:43:13 -0500 Subject: [PATCH 8/8] Inlined everything --- gtsam/geometry/ExtendedPose3.cpp | 442 +---------------------------- gtsam/geometry/ExtendedPose3.h | 461 +++++++++++++++++++++++++++---- 2 files changed, 401 insertions(+), 502 deletions(-) diff --git a/gtsam/geometry/ExtendedPose3.cpp b/gtsam/geometry/ExtendedPose3.cpp index 89dfd84efc..7b44221269 100644 --- a/gtsam/geometry/ExtendedPose3.cpp +++ b/gtsam/geometry/ExtendedPose3.cpp @@ -28,448 +28,8 @@ using namespace std; namespace gtsam { /** instantiate concept checks */ -GTSAM_CONCEPT_POSE_INST(ExtendedPose3); +// GTSAM_CONCEPT_POSE_INST(ExtendedPose3); -/* ************************************************************************* */ -ExtendedPose3 ExtendedPose3::Create(const Rot3& R, const Point3& v, const Point3& p, OptionalJacobian<9, 3> HR, OptionalJacobian<9, 3> Hv, - OptionalJacobian<9, 3> Hp) { - if (HR) *HR << I_3x3, Z_3x3, Z_3x3; - if (Hv) *Hv << Z_3x3, R.transpose(), Z_3x3; - if (Hp) *Hp << Z_3x3, Z_3x3, R.transpose(); - return ExtendedPose3(R, v, p); -} -/* ************************************************************************* */ -ExtendedPose3 ExtendedPose3::inverse() const { - Rot3 Rt = R_.inverse(); - return ExtendedPose3(Rt, Rt * (-v_), Rt * (-p_)); -} - -/* ************************************************************************* */ -// Calculate Adjoint map -// Ad_pose is 9*9 matrix that when applied to twist xi, returns Ad_pose(xi) -Matrix9 ExtendedPose3::AdjointMap() const { - const Matrix3 R = R_.matrix(); - Matrix3 A = skewSymmetric(v_.x(), v_.y(), v_.z()) * R; - Matrix3 B = skewSymmetric(p_.x(), p_.y(), p_.z()) * R; - Matrix9 adj; - adj << R, Z_3x3, Z_3x3, - A, R, Z_3x3, - B, Z_3x3, R; - return adj; -} - -/* ************************************************************************* */ -Matrix9 ExtendedPose3::adjointMap(const Vector9& xi) { - Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); - Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); - Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8)); - Matrix9 adj; - adj << w_hat, Z_3x3, Z_3x3, - v_hat, w_hat, Z_3x3, - a_hat, Z_3x3, w_hat; - return adj; -} - -/* ************************************************************************* */ -Vector9 ExtendedPose3::adjoint(const Vector9& xi, const Vector9& y, - OptionalJacobian<9, 9> Hxi) { - if (Hxi) { - Hxi->setZero(); - for (int i = 0; i < 9; ++i) { - Vector9 dxi; - dxi.setZero(); - dxi(i) = 1.0; - Matrix9 Gi = adjointMap(dxi); - Hxi->col(i) = Gi * y; - } - } - return adjointMap(xi) * y; -} - -/* ************************************************************************* */ -Vector9 ExtendedPose3::adjointTranspose(const Vector9& xi, const Vector9& y, - OptionalJacobian<9, 9> Hxi) { - if (Hxi) { - Hxi->setZero(); - for (int i = 0; i < 9; ++i) { - Vector9 dxi; - dxi.setZero(); - dxi(i) = 1.0; - Matrix9 GTi = adjointMap(dxi).transpose(); - Hxi->col(i) = GTi * y; - } - } - return adjointMap(xi).transpose() * y; -} - - -/* ************************************************************************* */ -void ExtendedPose3::print(const string& s) const { - cout << s; - R_.print("R:\n"); - cout << "v:" << v_ << ";" << endl; - cout << "p:" << p_ << ";" << endl; -} - -/* ************************************************************************* */ -bool ExtendedPose3::equals(const ExtendedPose3& pose, double tol) const { - return R_.equals(pose.R_, tol) && traits::Equals(v_, pose.v_, tol) && traits::Equals(p_, pose.p_, tol); -} - -/* ************************************************************************* */ -ExtendedPose3 ExtendedPose3::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - if (Hxi) *Hxi = ExpmapDerivative(xi); - - Vector3 phi(xi(0), xi(1), xi(2)), nu(xi(3), xi(4), xi(5)), rho(xi(6), xi(7), xi(8)); - - Rot3 R = Rot3::Expmap(phi); - - double phi_norm = phi.norm(); - if (phi_norm < 1e-8) - return ExtendedPose3(Rot3(), Point3(nu), Point3(rho)); - else { - Matrix W = skewSymmetric(phi/phi_norm); - Matrix A = I_3x3 + ((1 - cos(phi_norm)) / phi_norm) * W + ((phi_norm - sin(phi_norm)) / phi_norm) * (W * W); - return ExtendedPose3(Rot3::Expmap(phi), Point3(A * nu), Point3(A * rho)); - } - -} - -/* ************************************************************************* */ -Vector9 ExtendedPose3::Logmap(const ExtendedPose3& pose, OptionalJacobian<9, 9> Hpose) { - if (Hpose) *Hpose = LogmapDerivative(pose); - const Vector3 phi = Rot3::Logmap(pose.rotation()); - const Vector3 v = pose.velocity(); - const Vector3 p = pose.position(); - const double t = phi.norm(); - if (t < 1e-8) { - Vector9 log; - log << phi, v, p; - return log; - } else { - const Matrix3 W = skewSymmetric(phi / t); - - const double Tan = tan(0.5 * t); - const Vector3 Wp = W * p; - const Vector3 Wv = W * v; - const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); - const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); - Vector9 log; - log << phi, nu, rho; - return log; - } -} - - -/* ************************************************************************* */ -ExtendedPose3 ExtendedPose3::ChartAtOrigin::Retract(const Vector9& xi, ChartJacobian Hxi) { -#ifdef GTSAM_POSE3_EXPMAP - return Expmap(xi, Hxi); -#else - -#endif -} - -/* ************************************************************************* */ -Vector9 ExtendedPose3::ChartAtOrigin::Local(const ExtendedPose3& pose, ChartJacobian Hpose) { -#ifdef GTSAM_POSE3_EXPMAP - return Logmap(pose, Hpose); -#else - -#endif -} - -/* ************************************************************************* */ - Vector9 ExtendedPose3::boxminus(const ExtendedPose3& g) const { - // Matrix3 D_dR_R, D_dt_R, D_dv_R; - // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); - // const Point3 dt = R_.unrotate(g.p_ - p_, H1 ? &D_dt_R : 0); - // const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); - - - // Vector9 xi; - // Matrix3 D_xi_R; - // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv; - // if (H1) { - // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // - // D_dt_R, -I_3x3, Z_3x3, // - // D_dv_R, Z_3x3, -I_3x3; - // } - // if (H2) { - // *H2 << D_xi_R, Z_3x3, Z_3x3, // - // Z_3x3, dR.matrix(), Z_3x3, // - // Z_3x3, Z_3x3, dR.matrix(); - // } - - Matrix3 D_dR_R, D_dt_R, D_dv_R; - const Rot3 dR = g.R_; - const Point3 dt = g.p_; - const Vector dv = g.v_; - - Vector9 xi; - xi << Rot3::Logmap(dR), dt, dv; - return xi; -} - -/* ************************************************************************* */ -/** - * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative matrix - */ -static Matrix63 computeQforExpmapDerivative(const Vector9& xi) { - const auto omega = xi.head<3>(); - const auto nu = xi.segment<3>(3); - const auto rho = xi.tail<3>(); - const Matrix3 V = skewSymmetric(nu); - const Matrix3 P = skewSymmetric(rho); - const Matrix3 W = skewSymmetric(omega); - - Matrix3 Qv, Qp; - Matrix63 Q; - -#ifdef NUMERICAL_EXPMAP_DERIV - - -#else - // The closed-form formula in Barfoot14tro eq. (102) - double phi = omega.norm(); - if (std::abs(phi)>1e-5) { - const double sinPhi = sin(phi), cosPhi = cos(phi); - const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; - // Invert the sign of odd-order terms to have the right Jacobian - Qv = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) - + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) - - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); - Qp = -0.5*P + (phi-sinPhi)/phi3*(W*P + P*W - W*P*W) - + (1-phi2/2-cosPhi)/phi4*(W*W*P + P*W*W - 3*W*P*W) - - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*P*W*W + W*W*P*W); - } - else { - Qv = -0.5*V + 1./6.*(W*V + V*W - W*V*W) - + 1./24.*(W*W*V + V*W*W - 3*W*V*W) - - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); - Qp = -0.5*P + 1./6.*(W*P + P*W - W*P*W) - + 1./24.*(W*W*P + P*W*W - 3*W*P*W) - - 0.5*(1./24. + 3./120.)*(W*P*W*W + W*W*P*W); - } -#endif - Q << Qv, Qp; - return Q; -} - -/* ************************************************************************* */ -Matrix9 ExtendedPose3::ExpmapDerivative(const Vector9& xi) { - const Vector3 w = xi.head<3>(); - const Matrix3 Jw = Rot3::ExpmapDerivative(w); - const Matrix63 Q = computeQforExpmapDerivative(xi); - const Matrix3 Qv = Q.topRows<3>(); - const Matrix3 Qp = Q.bottomRows<3>(); - Matrix9 J; - J << Jw, Z_3x3, Z_3x3, - Qv, Jw, Z_3x3, - Qp, Z_3x3, Jw; - return J; -} - -/* ************************************************************************* */ -Matrix9 ExtendedPose3::LogmapDerivative(const ExtendedPose3& pose) { - const Vector9 xi = Logmap(pose); - const Vector3 w = xi.head<3>(); - const Matrix3 Jw = Rot3::LogmapDerivative(w); - const Matrix63 Q = computeQforExpmapDerivative(xi); - const Matrix3 Qv = Q.topRows<3>(); - const Matrix3 Qp = Q.bottomRows<3>(); - const Matrix3 Qv2 = -Jw*Qv*Jw; - const Matrix3 Qp2 = -Jw*Qp*Jw; - Matrix9 J; - - J << Jw, Z_3x3, Z_3x3, - Qv2, Jw, Z_3x3, - Qp2, Z_3x3, Jw; - return J; -} - -/* ************************************************************************* */ -const Point3& ExtendedPose3::position(OptionalJacobian<3, 9> Hself) const { - if (Hself) *Hself << Z_3x3, Z_3x3, rotation().matrix(); - return p_; -} - -const Point3& ExtendedPose3::translation(OptionalJacobian<3, 9> Hself) const { - if (Hself) *Hself << Z_3x3, Z_3x3, rotation().matrix(); - return p_; -} - -/* ************************************************************************* */ - -const Point3& ExtendedPose3::velocity(OptionalJacobian<3, 9> Hself) const { - if (Hself) { - *Hself << Z_3x3, rotation().matrix(), Z_3x3; - } - return v_; -} - -/* ************************************************************************* */ - -const Rot3& ExtendedPose3::rotation(OptionalJacobian<3, 9> Hself) const { - if (Hself) { - *Hself << I_3x3, Z_3x3, Z_3x3; - } - return R_; -} - -/* ************************************************************************* */ -Matrix5 ExtendedPose3::matrix() const { - Matrix5 mat; - mat << R_.matrix(), v_, p_, - 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0; - return mat; -} - -/* ************************************************************************* */ -ExtendedPose3 ExtendedPose3::transformPoseFrom(const ExtendedPose3& aTb, OptionalJacobian<9, 9> Hself, OptionalJacobian<9, 9> HaTb) const { - const ExtendedPose3& wTa = *this; - return wTa.compose(aTb, Hself, HaTb); -} - -/* ************************************************************************* */ -ExtendedPose3 ExtendedPose3::transformPoseTo(const ExtendedPose3& wTb, OptionalJacobian<9, 9> Hself, OptionalJacobian<9, 9> HwTb) const { - if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap(); - if (HwTb) *HwTb = I_9x9; - const ExtendedPose3& wTa = *this; - return wTa.inverse() * wTb; -} - -/* ************************************************************************* */ -Point3 ExtendedPose3::transformFrom(const Point3& point, OptionalJacobian<3, 9> Hself, - OptionalJacobian<3, 3> Hpoint) const { - // Only get matrix once, to avoid multiple allocations, - // as well as multiple conversions in the Quaternion case - const Matrix3 R = R_.matrix(); - if (Hself) { - Hself->leftCols<3>() = R * skewSymmetric(-point.x(), -point.y(), -point.z()); - Hself->rightCols<3>() = R; - } - if (Hpoint) { - *Hpoint = R; - } - return R_ * point + p_; -} - -/* ************************************************************************* */ -Point3 ExtendedPose3::transformTo(const Point3& point, OptionalJacobian<3, 9> Hself, - OptionalJacobian<3, 3> Hpoint) const { - // Only get transpose once, to avoid multiple allocations, - // as well as multiple conversions in the Quaternion case - const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(point - p_)); - if (Hself) { - const double wx = q.x(), wy = q.y(), wz = q.z(); - (*Hself) << - 0.0, -wz, +wy,0.0,0.0,0.0,-1.0, 0.0, 0.0, - +wz, 0.0, -wx,0.0,0.0,0.0, 0.0,-1.0, 0.0, - -wy, +wx, 0.0,0.0,0.0,0.0, 0.0, 0.0,-1.0; - } - if (Hpoint) { - *Hpoint = Rt; - } - return q; -} - -/* ************************************************************************* */ -double ExtendedPose3::range(const Point3& point, OptionalJacobian<1, 9> Hself, - OptionalJacobian<1, 3> Hpoint) const { - Matrix39 D_local_pose; - Matrix3 D_local_point; - Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0); - if (!Hself && !Hpoint) { - return local.norm(); - } else { - Matrix13 D_r_local; - const double r = norm3(local, D_r_local); - if (Hself) *Hself = D_r_local * D_local_pose; - if (Hpoint) *Hpoint = D_r_local * D_local_point; - return r; - } -} - -/* ************************************************************************* */ -double ExtendedPose3::range(const ExtendedPose3& pose, OptionalJacobian<1, 9> Hself, - OptionalJacobian<1, 9> Hpose) const { - Matrix13 D_local_point; - double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0); - if (Hpose) *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); - return r; -} - -/* ************************************************************************* */ -Unit3 ExtendedPose3::bearing(const Point3& point, OptionalJacobian<2, 9> Hself, - OptionalJacobian<2, 3> Hpoint) const { - Matrix39 D_local_pose; - Matrix3 D_local_point; - Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0); - if (!Hself && !Hpoint) { - return Unit3(local); - } else { - Matrix23 D_b_local; - Unit3 b = Unit3::FromPoint3(local, D_b_local); - if (Hself) *Hself = D_b_local * D_local_pose; - if (Hpoint) *Hpoint = D_b_local * D_local_point; - return b; - } -} - -/* ************************************************************************* */ -Unit3 ExtendedPose3::bearing(const ExtendedPose3& pose, OptionalJacobian<2, 9> Hself, - OptionalJacobian<2, 9> Hpose) const { - if (Hpose) { - Hpose->setZero(); - return bearing(pose.translation(), Hself, Hpose.cols<3>(3)); - } - return bearing(pose.translation(), Hself, boost::none); -} - -/* ************************************************************************* */ -boost::optional ExtendedPose3::Align(const std::vector& abPointPairs) { - const size_t n = abPointPairs.size(); - if (n < 3) - return boost::none; // we need at least three pairs - - // calculate centroids - Point3 aCentroid(0,0,0), bCentroid(0,0,0); - for(const Point3Pair& abPair: abPointPairs) { - aCentroid += abPair.first; - bCentroid += abPair.second; - } - double f = 1.0 / n; - aCentroid *= f; - bCentroid *= f; - - // Add to form H matrix - Matrix3 H = Z_3x3; - for(const Point3Pair& abPair: abPointPairs) { - Point3 da = abPair.first - aCentroid; - Point3 db = abPair.second - bCentroid; - H += da * db.transpose(); - } - - // ClosestTo finds rotation matrix closest to H in Frobenius sense - Rot3 aRb = Rot3::ClosestTo(H); - Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); - Point3 v; - return ExtendedPose3(aRb, v, aTb); -} - - -/* ************************************************************************* */ -std::ostream &operator<<(std::ostream &os, const ExtendedPose3& pose) { - os << pose.rotation(); - const Point3& v = pose.velocity(); - const Point3& p = pose.position(); - os << "v:[" << v.x() << ", " << v.y() << ", " << v.z() << "];\n"; - os << "p:[" << p.x() << ", " << p.y() << ", " << p.z() << "];\n"; - return os; -} } // namespace gtsam diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h index 20c0eba4f0..41fd153dd3 100644 --- a/gtsam/geometry/ExtendedPose3.h +++ b/gtsam/geometry/ExtendedPose3.h @@ -24,6 +24,11 @@ #include #include +#include +#include +#include +#include + namespace gtsam { /** @@ -74,38 +79,83 @@ class GTSAM_EXPORT ExtendedPose3: public LieGroup { /// Named constructor with derivatives static ExtendedPose3 Create(const Rot3& R, const Point3& v, const Point3& p, - OptionalJacobian<9, 3> HR = boost::none, - OptionalJacobian<9, 3> Hv = boost::none, - OptionalJacobian<9, 3> Hp = boost::none); + OptionalJacobian<9, 3> HR = {}, + OptionalJacobian<9, 3> Hv = {}, + OptionalJacobian<9, 3> Hp = {}) { + if (HR) *HR << I_3x3, Z_3x3, Z_3x3; + if (Hv) *Hv << Z_3x3, R.transpose(), Z_3x3; + if (Hp) *Hp << Z_3x3, Z_3x3, R.transpose(); + return ExtendedPose3(R, v, p); + } /** * Create Pose3 by aligning two point pairs * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point * Note this allows for noise on the points but in that case the mapping will not be exact. */ - static boost::optional Align(const std::vector& abPointPairs); + static std::optional Align(const std::vector& abPointPairs) { + const size_t n = abPointPairs.size(); + if (n < 3) + return {}; // we need at least three pairs + + // calculate centroids + Point3 aCentroid(0,0,0), bCentroid(0,0,0); + for(const Point3Pair& abPair: abPointPairs) { + aCentroid += abPair.first; + bCentroid += abPair.second; + } + double f = 1.0 / n; + aCentroid *= f; + bCentroid *= f; + + // Add to form H matrix + Matrix3 H = Z_3x3; + for(const Point3Pair& abPair: abPointPairs) { + Point3 da = abPair.first - aCentroid; + Point3 db = abPair.second - bCentroid; + H += da * db.transpose(); + } + + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot3 aRb = Rot3::ClosestTo(H); + Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); + Point3 v; + return ExtendedPose3(aRb, v, aTb); + } /// @} /// @name Testable /// @{ /// print with optional string - void print(const std::string& s = "") const; + void print(const std::string& s = "") const { + std::cout << s; + R_.print("R:\n"); + std::cout << "v:" << v_ << ";" << std::endl; + std::cout << "p:" << p_ << ";" << std::endl; + } /// assert equality up to a tolerance - bool equals(const ExtendedPose3& pose, double tol = 1e-9) const; + bool equals(const ExtendedPose3& pose, double tol = 1e-9) const { + return R_.equals(pose.R_, tol) && traits::Equals(v_, pose.v_, tol) && traits::Equals(p_, pose.p_, tol); + } /// @} /// @name Group /// @{ /// identity for group operation - static ExtendedPose3 identity() { + static ExtendedPose3 Identity() { return ExtendedPose3(); } - /// inverse transformation with derivatives - ExtendedPose3 inverse() const; + /// inverse transformation without derivative + ExtendedPose3 inverse() const { + Rot3 Rt = R_.inverse(); + return ExtendedPose3(Rt, Rt * (-v_), Rt * (-p_)); + } + + using LieGroup::inverse; // version with derivative /// compose syntactic sugar ExtendedPose3 operator*(const ExtendedPose3& T) const { @@ -117,16 +167,62 @@ class GTSAM_EXPORT ExtendedPose3: public LieGroup { /// @{ /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,V_x,V_y,V_z,P_x,P_y,P_z] \f$ - static ExtendedPose3 Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = boost::none); + static ExtendedPose3 Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {}) { + if (Hxi) *Hxi = ExpmapDerivative(xi); + + Vector3 phi(xi(0), xi(1), xi(2)), nu(xi(3), xi(4), xi(5)), rho(xi(6), xi(7), xi(8)); + + Rot3 R = Rot3::Expmap(phi); + + double phi_norm = phi.norm(); + if (phi_norm < 1e-8) + return ExtendedPose3(Rot3(), Point3(nu), Point3(rho)); + else { + Matrix W = skewSymmetric(phi/phi_norm); + Matrix A = I_3x3 + ((1 - cos(phi_norm)) / phi_norm) * W + ((phi_norm - sin(phi_norm)) / phi_norm) * (W * W); + return ExtendedPose3(Rot3::Expmap(phi), Point3(A * nu), Point3(A * rho)); + } + } /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,V_x,V_y,V_z,P_x,P_y,P_z] \f$ of this rotation - static Vector9 Logmap(const ExtendedPose3& pose, OptionalJacobian<9, 9> Hpose = boost::none); + static Vector9 Logmap(const ExtendedPose3& pose, OptionalJacobian<9, 9> Hpose = {}) { + if (Hpose) *Hpose = LogmapDerivative(pose); + const Vector3 phi = Rot3::Logmap(pose.rotation()); + const Vector3 v = pose.velocity(); + const Vector3 p = pose.position(); + const double t = phi.norm(); + if (t < 1e-8) { + Vector9 log; + log << phi, v, p; + return log; + } else { + const Matrix3 W = skewSymmetric(phi / t); + + const double Tan = tan(0.5 * t); + const Vector3 Wp = W * p; + const Vector3 Wv = W * v; + const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); + const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); + Vector9 log; + log << phi, nu, rho; + return log; + } + } /** * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame * Ad_pose is 9*9 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,V_x,V_y,V_z,P_x,P_y,P_z] \f$, returns Ad_pose(xi) */ - Matrix9 AdjointMap() const; /// FIXME Not tested - marked as incorrect + Matrix9 AdjointMap() const { + const Matrix3 R = R_.matrix(); + Matrix3 A = skewSymmetric(v_.x(), v_.y(), v_.z()) * R; + Matrix3 B = skewSymmetric(p_.x(), p_.y(), p_.z()) * R; + Matrix9 adj; + adj << R, Z_3x3, Z_3x3, + A, R, Z_3x3, + B, Z_3x3, R; + return adj; + } /** * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame @@ -134,44 +230,136 @@ class GTSAM_EXPORT ExtendedPose3: public LieGroup { */ Vector9 Adjoint(const Vector9& xi_b) const { return AdjointMap() * xi_b; - } /// FIXME Not tested - marked as incorrect + } /** * Compute the ad operator */ - static Matrix9 adjointMap(const Vector9 &xi); + static Matrix9 adjointMap(const Vector9 &xi) { + Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); + Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8)); + Matrix9 adj; + adj << w_hat, Z_3x3, Z_3x3, + v_hat, w_hat, Z_3x3, + a_hat, Z_3x3, w_hat; + return adj; + } /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ static Vector9 adjoint(const Vector9 &xi, const Vector9 &y, - OptionalJacobian<9, 9> Hxi = boost::none); - - // temporary fix for wrappers until case issue is resolved - static Matrix9 adjointMap_(const Vector9 &xi) { return adjointMap(xi);} - static Vector9 adjoint_(const Vector9 &xi, const Vector9 &y) { return adjoint(xi, y);} + OptionalJacobian<9, 9> Hxi = {}) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 Gi = adjointMap(dxi); + Hxi->col(i) = Gi * y; + } + } + return adjointMap(xi) * y; + } /** * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ static Vector9 adjointTranspose(const Vector9& xi, const Vector9& y, - OptionalJacobian<9, 9> Hxi = boost::none); + OptionalJacobian<9, 9> Hxi = {}) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 GTi = adjointMap(dxi).transpose(); + Hxi->col(i) = GTi * y; + } + } + return adjointMap(xi).transpose() * y; + } /// Derivative of Expmap - static Matrix9 ExpmapDerivative(const Vector9& xi); + static Matrix9 ExpmapDerivative(const Vector9& xi) { + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + const Matrix63 Q = computeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, + Qv, Jw, Z_3x3, + Qp, Z_3x3, Jw; + return J; + } /// Derivative of Logmap - static Matrix9 LogmapDerivative(const ExtendedPose3& xi); + static Matrix9 LogmapDerivative(const ExtendedPose3& pose) { + const Vector9 xi = Logmap(pose); + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + const Matrix63 Q = computeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + const Matrix3 Qv2 = -Jw*Qv*Jw; + const Matrix3 Qp2 = -Jw*Qp*Jw; + Matrix9 J; + + J << Jw, Z_3x3, Z_3x3, + Qv2, Jw, Z_3x3, + Qp2, Z_3x3, Jw; + return J; + } // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP struct ChartAtOrigin { - static ExtendedPose3 Retract(const Vector9& xi, ChartJacobian Hxi = boost::none); - static Vector9 Local(const ExtendedPose3& pose, ChartJacobian Hpose = boost::none); - }; + static ExtendedPose3 Retract(const Vector9& xi, ChartJacobian Hxi = {}) { +#ifdef GTSAM_POSE3_EXPMAP + return Expmap(xi, Hxi); +#else - Vector9 boxminus(const ExtendedPose3& g) const; +#endif + } + static Vector9 Local(const ExtendedPose3& pose, ChartJacobian Hpose = {}) { +#ifdef GTSAM_POSE3_EXPMAP + return Logmap(pose, Hpose); +#else - using LieGroup::inverse; // version with derivative +#endif + } + }; + + Vector9 boxminus(const ExtendedPose3& g) const { + // Matrix3 D_dR_R, D_dt_R, D_dv_R; + // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); + // const Point3 dt = R_.unrotate(g.p_ - p_, H1 ? &D_dt_R : 0); + // const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + + // Vector9 xi; + // Matrix3 D_xi_R; + // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv; + // if (H1) { + // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + // D_dt_R, -I_3x3, Z_3x3, // + // D_dv_R, Z_3x3, -I_3x3; + // } + // if (H2) { + // *H2 << D_xi_R, Z_3x3, Z_3x3, // + // Z_3x3, dR.matrix(), Z_3x3, // + // Z_3x3, Z_3x3, dR.matrix(); + // } + Matrix3 D_dR_R, D_dt_R, D_dv_R; + const Rot3 dR = g.R_; + const Point3 dt = g.p_; + const Vector dv = g.v_; + + Vector9 xi; + xi << Rot3::Logmap(dR), dt, dv; + return xi; + } /** * wedge for ExtendedPose3: @@ -182,6 +370,49 @@ class GTSAM_EXPORT ExtendedPose3: public LieGroup { return (Matrix(5, 5) << 0., -phiz, phiy, nux, rhox, phiz, 0., -phix, nuy, rhoy, -phiy, phix, 0., nuz, rhoz, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.).finished(); } + /** + * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative matrix + */ + static Matrix63 computeQforExpmapDerivative(const Vector9& xi) { + const auto omega = xi.head<3>(); + const auto nu = xi.segment<3>(3); + const auto rho = xi.tail<3>(); + const Matrix3 V = skewSymmetric(nu); + const Matrix3 P = skewSymmetric(rho); + const Matrix3 W = skewSymmetric(omega); + + Matrix3 Qv, Qp; + Matrix63 Q; + +#ifdef NUMERICAL_EXPMAP_DERIV + +#else + // The closed-form formula in Barfoot14tro eq. (102) + double phi = omega.norm(); + if (std::abs(phi)>1e-5) { + const double sinPhi = sin(phi), cosPhi = cos(phi); + const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Qv = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); + Qp = -0.5*P + (phi-sinPhi)/phi3*(W*P + P*W - W*P*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*P + P*W*W - 3*W*P*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*P*W*W + W*W*P*W); + } + else { + Qv = -0.5*V + 1./6.*(W*V + V*W - W*V*W) + + 1./24.*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + Qp = -0.5*P + 1./6.*(W*P + P*W - W*P*W) + + 1./24.*(W*W*P + P*W*W - 3*W*P*W) + - 0.5*(1./24. + 3./120.)*(W*P*W*W + W*W*P*W); + } +#endif + Q << Qv, Qp; + return Q; + } + /// @} /// @name Group Action on Point3 /// @{ @@ -193,8 +424,22 @@ class GTSAM_EXPORT ExtendedPose3: public LieGroup { * @param Hpoint optional 3*3 Jacobian wrpt point * @return point in world coordinates */ - Point3 transformFrom(const Point3& point, OptionalJacobian<3, 9> Hself = - boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; + Point3 transformFrom(const Point3& point, + OptionalJacobian<3, 9> Hself = {}, + OptionalJacobian<3, 3> Hpoint = {}) const { + // Only get matrix once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + const Matrix3 R = R_.matrix(); + if (Hself) { + Hself->leftCols<3>() = + R * skewSymmetric(-point.x(), -point.y(), -point.z()); + Hself->rightCols<3>() = R; + } + if (Hpoint) { + *Hpoint = R; + } + return R_ * point + p_; + } /** syntactic sugar for transformFrom */ inline Point3 operator*(const Point3& point) const { @@ -208,25 +453,55 @@ class GTSAM_EXPORT ExtendedPose3: public LieGroup { * @param Hpoint optional 3*3 Jacobian wrpt point * @return point in Pose coordinates */ - Point3 transformTo(const Point3& point, OptionalJacobian<3, 9> Hself = - boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; - - + Point3 transformTo(const Point3& point, + OptionalJacobian<3, 9> Hself = {}, + OptionalJacobian<3, 3> Hpoint = {}) const { + // Only get transpose once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + const Matrix3 Rt = R_.transpose(); + const Point3 q(Rt * (point - p_)); + if (Hself) { + const double wx = q.x(), wy = q.y(), wz = q.z(); + (*Hself) << 0.0, -wz, +wy, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, +wz, 0.0, -wx, + 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, -wy, +wx, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + -1.0; + } + if (Hpoint) { + *Hpoint = Rt; + } + return q; + } /// @} /// @name Standard Interface /// @{ /// get rotation - const Rot3& rotation(OptionalJacobian<3, 9> Hself = boost::none) const; + const Rot3& rotation(OptionalJacobian<3, 9> Hself = {}) const { + if (Hself) { + *Hself << I_3x3, Z_3x3, Z_3x3; + } + return R_; + } /// get translation - const Point3& velocity(OptionalJacobian<3, 9> Hself = boost::none) const; + const Point3& velocity(OptionalJacobian<3, 9> Hself = {}) const { + if (Hself) { + *Hself << Z_3x3, rotation().matrix(), Z_3x3; + } + return v_; + } /// get position - const Point3& position(OptionalJacobian<3, 9> Hself = boost::none) const; + const Point3& position(OptionalJacobian<3, 9> Hself = {}) const { + if (Hself) *Hself << Z_3x3, Z_3x3, rotation().matrix(); + return p_; + } - const Point3& translation(OptionalJacobian<3, 9> Hself = boost::none) const; + const Point3& translation(OptionalJacobian<3, 9> Hself = {}) const { + if (Hself) *Hself << Z_3x3, Z_3x3, rotation().matrix(); + return p_; + } /// get x double x() const { @@ -243,45 +518,97 @@ class GTSAM_EXPORT ExtendedPose3: public LieGroup { return p_.z(); } - /** convert to 5*5 matrix */ - Matrix5 matrix() const; - - /** - * Assuming self == wTa, takes a pose aTb in local coordinates - * and transforms it to world coordinates wTb = wTa * aTb. - * This is identical to compose. - */ - ExtendedPose3 transformPoseFrom(const ExtendedPose3& aTb, OptionalJacobian<9, 9> Hself = boost::none, OptionalJacobian<9, 9> HaTb = boost::none) const; - - /** - * Assuming self == wTa, takes a pose wTb in world coordinates - * and transforms it to local coordinates aTb = inv(wTa) * wTb + Matrix5 matrix() const { + Matrix5 mat; + mat << R_.matrix(), v_, p_, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 1.0; + return mat; + } + + /** + * Assuming self == wTa, takes a pose aTb in local coordinates + * and transforms it to world coordinates wTb = wTa * aTb. + * This is identical to compose. */ - ExtendedPose3 transformPoseTo(const ExtendedPose3& wTb, OptionalJacobian<9, 9> Hself = boost::none, OptionalJacobian<9, 9> HwTb = boost::none) const; + ExtendedPose3 transformPoseFrom( + const ExtendedPose3& aTb, OptionalJacobian<9, 9> Hself = {}, + OptionalJacobian<9, 9> HaTb = {}) const { + const ExtendedPose3& wTa = *this; + return wTa.compose(aTb, Hself, HaTb); + } + + /** + * Assuming self == wTa, takes a pose wTb in world coordinates + * and transforms it to local coordinates aTb = inv(wTa) * wTb + */ + ExtendedPose3 transformPoseTo( + const ExtendedPose3& wTb, OptionalJacobian<9, 9> Hself = {}, + OptionalJacobian<9, 9> HwTb = {}) const { + if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap(); + if (HwTb) *HwTb = I_9x9; + const ExtendedPose3& wTa = *this; + return wTa.inverse() * wTb; + } /** * Calculate range to a landmark * @param point 3D location of landmark * @return range (double) */ - double range(const Point3& point, OptionalJacobian<1, 9> Hself = boost::none, - OptionalJacobian<1, 3> Hpoint = boost::none) const; + double range(const Point3& point, OptionalJacobian<1, 9> Hself = {}, + OptionalJacobian<1, 3> Hpoint = {}) const { + Matrix39 D_local_pose; + Matrix3 D_local_point; + Point3 local = transformTo(point, Hself ? &D_local_pose : 0, + Hpoint ? &D_local_point : 0); + if (!Hself && !Hpoint) { + return local.norm(); + } else { + Matrix13 D_r_local; + const double r = norm3(local, D_r_local); + if (Hself) *Hself = D_r_local * D_local_pose; + if (Hpoint) *Hpoint = D_r_local * D_local_point; + return r; + } + } /** * Calculate range to another pose * @param pose Other SO(3) pose * @return range (double) */ - double range(const ExtendedPose3& pose, OptionalJacobian<1, 9> Hself = boost::none, OptionalJacobian<1, 9> Hpose = boost::none) const; + double range(const ExtendedPose3& pose, + OptionalJacobian<1, 9> Hself = {}, + OptionalJacobian<1, 9> Hpose = {}) const { + Matrix13 D_local_point; + double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0); + if (Hpose) + *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); + return r; + } /** * Calculate bearing to a landmark * @param point 3D location of landmark * @return bearing (Unit3) */ - Unit3 bearing(const Point3& point, OptionalJacobian<2, 9> Hself = boost::none, - OptionalJacobian<2, 3> Hpoint = boost::none) const; + Unit3 bearing(const Point3& point, OptionalJacobian<2, 9> Hself = {}, + OptionalJacobian<2, 3> Hpoint = {}) const { + Matrix39 D_local_pose; + Matrix3 D_local_point; + Point3 local = transformTo(point, Hself ? &D_local_pose : 0, + Hpoint ? &D_local_point : 0); + if (!Hself && !Hpoint) { + return Unit3(local); + } else { + Matrix23 D_b_local; + Unit3 b = Unit3::FromPoint3(local, D_b_local); + if (Hself) *Hself = D_b_local * D_local_pose; + if (Hpoint) *Hpoint = D_b_local * D_local_point; + return b; + } + } /** * Calculate bearing to another pose @@ -289,8 +616,15 @@ class GTSAM_EXPORT ExtendedPose3: public LieGroup { * information is ignored. * @return bearing (Unit3) */ - Unit3 bearing(const ExtendedPose3& pose, OptionalJacobian<2, 9> Hself = boost::none, - OptionalJacobian<2, 9> Hpose = boost::none) const; + Unit3 bearing(const ExtendedPose3& pose, + OptionalJacobian<2, 9> Hself = {}, + OptionalJacobian<2, 9> Hpose = {}) const { + if (Hpose) { + Hpose->setZero(); + return bearing(pose.translation(), Hself, Hpose.cols<3>(3)); + } + return bearing(pose.translation(), Hself, {}); + } /// @} /// @name Advanced Interface @@ -328,9 +662,14 @@ class GTSAM_EXPORT ExtendedPose3: public LieGroup { /// Output stream operator GTSAM_EXPORT - friend std::ostream &operator<<(std::ostream &os, const ExtendedPose3& p); - - + friend std::ostream& operator<<(std::ostream& os, const ExtendedPose3& pose) { + os << pose.rotation(); + const Point3& v = pose.velocity(); + const Point3& p = pose.position(); + os << "v:[" << v.x() << ", " << v.y() << ", " << v.z() << "];\n"; + os << "p:[" << p.x() << ", " << p.y() << ", " << p.z() << "];\n"; + return os; + } private: /** Serialization function */