diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md index b2179615fd..5998d60cf0 100644 --- a/.github/copilot-instructions.md +++ b/.github/copilot-instructions.md @@ -4,8 +4,16 @@ For reviewing PRs: * Use meaningful variable names, e.g. `measurement` not `msm`, avoid abbreviations. * Flag overly complex or long/functions: break up in smaller functions * On Windows it is necessary to explicitly export all functions from the library which should be externally accessible. To do this, include the macro `GTSAM_EXPORT` in your class or function definition. +* When adding or modifying public classes/methods, always review and follow `Using-GTSAM-EXPORT.md` before finalizing changes, including template specialization/export rules. * If we add a C++ function to a `.i` file to expose it to the wrapper, we must ensure that the parameter names match exactly between the declaration in the header file and the declaration in the `.i`. Similarly, if we change any parameter names in a wrapped function in a header file, or change any parameter names in a `.i` file, we must change the corresponding function in the other file to reflect those changes. * Classes are Uppercase, methods and functions lowerMixedCase. * Public fields in structs keep plain names (no trailing underscore). * Apart from those naming conventions, we adopt Google C++ style. +* Notebooks in `*/doc/*.ipynb` and `*/examples/*.ipynb` should follow the standard preamble: + 1) title/introduction markdown cell, + 2) copyright markdown cell tagged `remove-cell`, + 3) Colab badge markdown cell, + 4) Colab install code cell tagged `remove-cell`, + 5) imports/setup code cell. + Use the same `remove-cell` tagging convention as existing notebooks so docs build and Colab behavior stay consistent. * After any code change, always run relevant tests via `make -j6 testXXX.run` in the build folder $WORKSPACE/build. If in VS code, ask for escalated permissions if needed. diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 1a6023ee95..96e0efc161 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -209,6 +209,7 @@ struct LieGroupTraits : public GetDimensionImpl { // GetDimensionImpl handles resolving this to a static value or providing GetDimension(obj). inline constexpr static auto dimension = Class::dimension; using TangentVector = Eigen::Matrix; + using Jacobian = Eigen::Matrix; using ChartJacobian = OptionalJacobian; static TangentVector Local(const Class& origin, const Class& other, diff --git a/gtsam/base/MatrixLieGroup.h b/gtsam/base/MatrixLieGroup.h index 7653d28f1c..6987d36406 100644 --- a/gtsam/base/MatrixLieGroup.h +++ b/gtsam/base/MatrixLieGroup.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include namespace gtsam { @@ -56,6 +57,9 @@ namespace gtsam { using Jacobian = typename Base::Jacobian; using TangentVector = typename Base::TangentVector; + /// @name Matrix Lie Group + /// @{ + /** * Vectorize the matrix representation of a Lie group element. * The derivative `H` is the `(N*N) x D` Jacobian of this vectorization map. @@ -130,7 +134,149 @@ namespace gtsam { return adj; } + /** + * Adjoint action on a tangent vector. + * + * Returns Ad_g * xi with optional Jacobians with respect to g and xi. + */ + TangentVector Adjoint(const TangentVector& xi, + ChartJacobian H_this = {}, + ChartJacobian H_xi = {}) const { + const auto& m = static_cast(*this); + const Jacobian Ad = m.AdjointMap(); + if (H_this) *H_this = -Ad * Class::adjointMap(xi); + if (H_xi) *H_xi = Ad; + return Ad * xi; + } + + /** + * Dual Adjoint action on a tangent covector. + * + * Returns Ad_g^T * x with optional Jacobians with respect to g and x. + */ + TangentVector AdjointTranspose(const TangentVector& x, + ChartJacobian H_this = {}, + ChartJacobian H_x = {}) const { + const auto& m = static_cast(*this); + const Jacobian Ad = m.AdjointMap(); + const TangentVector AdTx = Ad.transpose() * x; + + if (H_this) { + const Eigen::Index d = tangentDim(&m, nullptr); + setZeroJacobian(H_this, d); + if constexpr (D == Eigen::Dynamic) { + for (Eigen::Index i = 0; i < d; ++i) { + H_this->col(i) = + Class::adjointMap(TangentVector::Unit(d, i)).transpose() * AdTx; + } + } else { + const auto& basis = adjointBasis(); + for (Eigen::Index i = 0; i < d; ++i) { + H_this->col(i) = basis[static_cast(i)].transpose() * AdTx; + } + } + } + + if (H_x) *H_x = Ad.transpose(); + return AdTx; + } + + /** + * Lie algebra adjoint map ad_xi, with optional specialization in derived + * classes. + */ + static Jacobian adjointMap(const TangentVector& xi) { + const Eigen::Index d = tangentDim(nullptr, &xi); + Jacobian ad; + if constexpr (D == Eigen::Dynamic) { + ad.setZero(d, d); + } else { + ad.setZero(); + } + const auto Xi = Class::Hat(xi); + for (Eigen::Index i = 0; i < d; ++i) { + const auto Ei = Class::Hat(TangentVector::Unit(d, i)); + ad.col(i) = Class::Vee(Xi * Ei - Ei * Xi); + } + return ad; + } + + /** + * Lie algebra action ad_xi(y), with optional Jacobians. + */ + static TangentVector adjoint(const TangentVector& xi, + const TangentVector& y, ChartJacobian Hxi = {}, + ChartJacobian H_y = {}) { + const Jacobian ad_xi = Class::adjointMap(xi); + if (Hxi) *Hxi = -Class::adjointMap(y); + if (H_y) *H_y = ad_xi; + return ad_xi * y; + } + + /** + * Dual Lie algebra action ad_xi^T(y), with optional Jacobians. + */ + static TangentVector adjointTranspose(const TangentVector& xi, + const TangentVector& y, + ChartJacobian Hxi = {}, + ChartJacobian H_y = {}) { + const Jacobian adT_xi = Class::adjointMap(xi).transpose(); + if (Hxi) { + const Eigen::Index d = tangentDim(nullptr, &xi); + setZeroJacobian(Hxi, d); + if constexpr (D == Eigen::Dynamic) { + for (Eigen::Index i = 0; i < d; ++i) { + Hxi->col(i) = + Class::adjointMap(TangentVector::Unit(d, i)).transpose() * y; + } + } else { + const auto& basis = adjointBasis(); + for (Eigen::Index i = 0; i < d; ++i) { + Hxi->col(i) = basis[static_cast(i)].transpose() * y; + } + } + } + if (H_y) *H_y = adT_xi; + return adT_xi * y; + } + + /// @} + private: + static Eigen::Index tangentDim(const Class* m, const TangentVector* xi) { + if constexpr (D == Eigen::Dynamic) { + return m ? static_cast(traits::GetDimension(*m)) + : static_cast(xi->size()); + } else { + (void)m; + (void)xi; + return D; + } + } + + static void setZeroJacobian(ChartJacobian H, Eigen::Index d) { + if constexpr (D == Eigen::Dynamic) { + H->setZero(d, d); + } else { + (void)d; + H->setZero(); + } + } + + /// Basis maps ad_{e_i}, cached for fixed-size groups. + template = 0> + static const std::array& adjointBasis() { + static const std::array basis = []() { + std::array B{}; + for (int i = 0; i < DD; ++i) { + B[static_cast(i)] = + Class::adjointMap(TangentVector::Unit(DD, i)); + } + return B; + }(); + return basis; + } + /// Pre-compute and store vectorized generators for fixed-size groups. inline static const Eigen::Matrix& VectorizedGenerators() { @@ -142,10 +288,12 @@ namespace gtsam { namespace internal { - /// Adds LieAlgebra, Hat, Vee, and Vec to LieGroupTraits + /// Adds MatrixLieGroup methods to LieGroupTraits template struct MatrixLieGroupTraits : LieGroupTraits { using LieAlgebra = typename Class::LieAlgebra; using TangentVector = typename LieGroupTraits::TangentVector; + using Jacobian = typename LieGroupTraits::Jacobian; + using ChartJacobian = typename LieGroupTraits::ChartJacobian; static LieAlgebra Hat(const TangentVector& v) { return Class::Hat(v); @@ -162,6 +310,37 @@ namespace gtsam { LieGroupTraits::dimension> H = {}) { return m.vec(H); } + + static TangentVector AdjointTranspose(const Class& m, + const TangentVector& x, + ChartJacobian Hm = {}, + ChartJacobian Hx = {}) { + return m.AdjointTranspose(x, Hm, Hx); + } + + static TangentVector Adjoint(const Class& m, const TangentVector& x, + ChartJacobian Hm = {}, + ChartJacobian Hx = {}) { + return m.Adjoint(x, Hm, Hx); + } + + static Jacobian adjointMap(const TangentVector& xi) { + return Class::adjointMap(xi); + } + + static TangentVector adjoint(const TangentVector& xi, + const TangentVector& y, + ChartJacobian Hxi = {}, + ChartJacobian H_y = {}) { + return Class::adjoint(xi, y, Hxi, H_y); + } + + static TangentVector adjointTranspose(const TangentVector& xi, + const TangentVector& y, + ChartJacobian Hxi = {}, + ChartJacobian H_y = {}) { + return Class::adjointTranspose(xi, y, Hxi, H_y); + } }; /// Both LieGroupTraits and Testable diff --git a/gtsam/base/doc/MatrixLieGroup.md b/gtsam/base/doc/MatrixLieGroup.md index f2228fb0db..0788b1da5e 100644 --- a/gtsam/base/doc/MatrixLieGroup.md +++ b/gtsam/base/doc/MatrixLieGroup.md @@ -29,9 +29,12 @@ You must implement everything from the `LieGroup.md` guide, plus the following: By inheriting from `gtsam::MatrixLieGroup`, you get: -* **A default `AdjointMap()` implementation**: This generic version works by repeatedly calling your `Hat` and `Vee` methods. While it is correct, it is often slow. For performance-critical applications, you should still provide a faster, closed-form `AdjointMap()` implementation in your class, which will override the default. +* **Default group adjoint methods**: `AdjointMap()`, `Adjoint(xi)`, and `AdjointTranspose(x)` are provided generically, including Jacobians for the latter two. +* **Default Lie algebra methods**: `adjointMap(xi)`, `adjoint(xi, y)`, and `adjointTranspose(xi, y)` are provided generically, including Jacobians for the latter two. * **A `vec()` method**: This vectorizes the `N x N` matrix representation of your group element into an `(N*N) x 1` vector. +**Performance Note:** The generic implementation of `AdjointMap()` is correct but may be slower because it is derived via the `Hat`/`Vee` mappings. If a closed-form expression for `AdjointMap()` is available for your group, consider overriding the default implementation for better performance. + ### 4. Traits and Concept Checking Finally, the traits specialization in your header file must be updated to reflect that your class is now a `MatrixLieGroup`. diff --git a/gtsam/geometry/ExtendedPose3-inl.h b/gtsam/geometry/ExtendedPose3-inl.h new file mode 100644 index 0000000000..8cadbccb52 --- /dev/null +++ b/gtsam/geometry/ExtendedPose3-inl.h @@ -0,0 +1,395 @@ +/* ---------------------------------------------------------------------------- + + * 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-inl.h + * @brief Template implementations for ExtendedPose3 + * @author Frank Dellaert, et al. + */ + +#pragma once + +namespace gtsam { + +template +size_t ExtendedPose3::RuntimeK(const TangentVector& xi) { + if constexpr (K == Eigen::Dynamic) { + assert(xi.size() >= 3 && (xi.size() - 3) % 3 == 0); + return static_cast((xi.size() - 3) / 3); + } else { + return static_cast(K); + } +} + +template +void ExtendedPose3::ZeroJacobian(ChartJacobian H, Eigen::Index d) { + if (!H) return; + if constexpr (dimension == Eigen::Dynamic) { + H->setZero(d, d); + } else { + (void)d; + H->setZero(); + } +} + +template +ExtendedPose3::ExtendedPose3(const Rot3& R, const Matrix3K& x) + : R_(R), t_(x) {} + +template +ExtendedPose3::ExtendedPose3(const MatrixRep& T) { + const Eigen::Index n = T.rows(); + if constexpr (K == Eigen::Dynamic) { + if (T.cols() != n || n < 3) { + throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); + } + t_.resize(3, n - 3); + } else { + if (n != matrixDim || T.cols() != matrixDim) { + throw std::invalid_argument("ExtendedPose3: invalid matrix shape."); + } + } + + R_ = Rot3(T.template block<3, 3>(0, 0)); + t_ = T.block(0, 3, 3, n - 3); +} + +template +const Rot3& ExtendedPose3::rotation(ComponentJacobian H) const { + if (H) { + if constexpr (dimension == Eigen::Dynamic) { + H->setZero(3, static_cast(dim())); + } else { + H->setZero(); + } + H->block(0, 0, 3, 3) = I_3x3; + } + return R_; +} + +template +Point3 ExtendedPose3::x(size_t i, ComponentJacobian H) const { + if (i >= k()) throw std::out_of_range("ExtendedPose3: x(i) out of range."); + if (H) { + if constexpr (dimension == Eigen::Dynamic) { + H->setZero(3, static_cast(dim())); + } else { + H->setZero(); + } + const Eigen::Index idx = 3 + 3 * static_cast(i); + H->block(0, idx, 3, 3) = R_.matrix(); + } + return t_.col(static_cast(i)); +} + +template +const typename ExtendedPose3::Matrix3K& +ExtendedPose3::xMatrix() const { + return t_; +} + +template +typename ExtendedPose3::Matrix3K& +ExtendedPose3::xMatrix() { + return t_; +} + +template +void ExtendedPose3::print(const std::string& s) const { + std::cout << (s.empty() ? s : s + " ") << *this << std::endl; +} + +template +bool ExtendedPose3::equals(const ExtendedPose3& other, + double tol) const { + return R_.equals(other.R_, tol) && equal_with_abs_tol(t_, other.t_, tol); +} + +template +typename ExtendedPose3::This ExtendedPose3::inverse() + const { + const Rot3 Rt = R_.inverse(); + const Matrix3K x = -(Rt.matrix() * t_); + return MakeReturn(ExtendedPose3(Rt, x)); +} + +template +typename ExtendedPose3::This ExtendedPose3::operator*( + const This& other) const { + const ExtendedPose3& otherBase = AsBase(other); + if constexpr (K == Eigen::Dynamic) { + if (k() != otherBase.k()) { + throw std::invalid_argument("ExtendedPose3: compose requires matching k."); + } + } + Matrix3K x = t_ + R_.matrix() * otherBase.t_; + return MakeReturn(ExtendedPose3(R_ * otherBase.R_, x)); +} + +// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's +// elegant Lie group document, at https://www.ethaneade.org/lie.pdf. +// See also [this document](doc/Jacobians.md) +template +typename ExtendedPose3::This ExtendedPose3::Expmap( + const TangentVector& xi, ChartJacobian Hxi) { + // Get angular velocity omega + const Vector3 w = xi.template head<3>(); + + // Instantiate functor for Dexp-related operations: + const so3::DexpFunctor local(w); + + // Compute rotation using Expmap +#ifdef GTSAM_USE_QUATERNIONS + // Reuse any quaternion-specific validation inside Rot3::Expmap. + const Rot3 R = Rot3::Expmap(w); +#else + const Rot3 R(local.expmap()); +#endif + + const Eigen::Index k = static_cast(RuntimeK(xi)); + + // The translation t = local.Jacobian().left() * v. + // Below we call local.Jacobian().applyLeft, which is faster if you don't need + // Jacobians, and returns Jacobian of t with respect to w if asked. + // NOTE(Frank): this does the same as the intuitive formulas: + // t_parallel = w * w.dot(v); // translation parallel to axis + // w_cross_v = w.cross(v); // translation orthogonal to axis + // t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; + // but Local does not need R, deals automatically with the case where theta2 + // is near zero, and also gives us the machinery for the Jacobians. + + Matrix3K x; + if constexpr (K == Eigen::Dynamic) x.resize(3, k); + + if (Hxi) { + ZeroJacobian(Hxi, 3 + 3 * k); + const Matrix3 Jr = local.Jacobian().right(); + Hxi->block(0, 0, 3, 3) = Jr; // Jr here *is* the Jacobian of expmap + const Matrix3 Rt = R.transpose(); + for (Eigen::Index i = 0; i < k; ++i) { + Matrix3 H_xi_w; + const Eigen::Index idx = 3 + 3 * i; + const Vector3 rho = xi.template segment<3>(idx); + x.col(i) = local.Jacobian().applyLeft(rho, &H_xi_w); + Hxi->block(idx, 0, 3, 3) = Rt * H_xi_w; + Hxi->block(idx, idx, 3, 3) = Jr; + // In the last row, Jr = R^T * Jl, see Barfoot eq. (8.83). + // Jl is the left Jacobian of SO(3) at w. + } + } else { + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; + const Vector3 rho = xi.template segment<3>(idx); + x.col(i) = local.Jacobian().applyLeft(rho); + } + } + + return MakeReturn(ExtendedPose3(R, x)); +} + +template +typename ExtendedPose3::TangentVector +ExtendedPose3::Logmap(const This& pose, ChartJacobian H) { + const ExtendedPose3& poseBase = AsBase(pose); + const Vector3 w = Rot3::Logmap(poseBase.R_); + const so3::DexpFunctor local(w); + + TangentVector xi; + if constexpr (K == Eigen::Dynamic) + xi.resize(static_cast(poseBase.dim())); + xi.template head<3>() = w; + const Eigen::Index k = static_cast(poseBase.k()); + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; + xi.template segment<3>(idx) = + local.InvJacobian().applyLeft(poseBase.t_.col(i)); + } + + if (H) *H = LogmapDerivative(xi); + return xi; +} + +template +typename ExtendedPose3::Jacobian +ExtendedPose3::AdjointMap() const { + const Matrix3 R = R_.matrix(); + + Jacobian adj; + if constexpr (dimension == Eigen::Dynamic) { + adj.setZero(dim(), dim()); + } else { + adj.setZero(); + } + + adj.block(0, 0, 3, 3) = R; + const Eigen::Index k = static_cast(this->k()); + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; + adj.block(idx, 0, 3, 3) = skewSymmetric(t_.col(i)) * R; + adj.block(idx, idx, 3, 3) = R; + } + return adj; +} + +template +typename ExtendedPose3::Jacobian +ExtendedPose3::adjointMap(const TangentVector& xi) { + const Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + + const Eigen::Index k = static_cast(RuntimeK(xi)); + + Jacobian adj; + if constexpr (dimension == Eigen::Dynamic) { + adj.setZero(3 + 3 * k, 3 + 3 * k); + } else { + adj.setZero(); + } + + adj.block(0, 0, 3, 3) = w_hat; + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; + adj.block(idx, 0, 3, 3) = + skewSymmetric(xi(idx + 0), xi(idx + 1), xi(idx + 2)); + adj.block(idx, idx, 3, 3) = w_hat; + } + return adj; +} + +template +typename ExtendedPose3::Jacobian +ExtendedPose3::ExpmapDerivative(const TangentVector& xi) { + Jacobian J; + Expmap(xi, J); + return J; +} + +template +typename ExtendedPose3::Jacobian +ExtendedPose3::LogmapDerivative(const TangentVector& xi) { + const Vector3 w = xi.template head<3>(); + + // Instantiate functor for Dexp-related operations: + const so3::DexpFunctor local(w); + + const Eigen::Index k = static_cast(RuntimeK(xi)); + const Matrix3 Rt = local.expmap().transpose(); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + + Jacobian J; + if constexpr (dimension == Eigen::Dynamic) { + J.setZero(3 + 3 * k, 3 + 3 * k); + } else { + J.setZero(); + } + + J.block(0, 0, 3, 3) = Jw; + for (Eigen::Index i = 0; i < k; ++i) { + Matrix3 H_xi_w; + const Eigen::Index idx = 3 + 3 * i; + local.Jacobian().applyLeft(xi.template segment<3>(idx), H_xi_w); + const Matrix3 Q = Rt * H_xi_w; + J.block(idx, 0, 3, 3) = -Jw * Q * Jw; + J.block(idx, idx, 3, 3) = Jw; + } + return J; +} + +template +typename ExtendedPose3::Jacobian +ExtendedPose3::LogmapDerivative(const This& pose) { + return LogmapDerivative(Logmap(pose)); +} + +template +typename ExtendedPose3::This +ExtendedPose3::ChartAtOrigin::Retract(const TangentVector& xi, + ChartJacobian Hxi) { + return ExtendedPose3::Expmap(xi, Hxi); +} + +template +typename ExtendedPose3::TangentVector +ExtendedPose3::ChartAtOrigin::Local(const This& pose, + ChartJacobian H) { + return ExtendedPose3::Logmap(pose, H); +} + +template +typename ExtendedPose3::MatrixRep +ExtendedPose3::matrix() const { + MatrixRep M; + if constexpr (matrixDim == Eigen::Dynamic) { + const Eigen::Index k = static_cast(this->k()); + const Eigen::Index n = 3 + k; + M = MatrixRep::Identity(n, n); + } else { + M = MatrixRep::Identity(); + } + M.template block<3, 3>(0, 0) = R_.matrix(); + M.block(0, 3, 3, static_cast(this->k())) = t_; + return M; +} + +template +typename ExtendedPose3::LieAlgebra ExtendedPose3::Hat( + const TangentVector& xi) { + const Eigen::Index k = static_cast(RuntimeK(xi)); + LieAlgebra X; + if constexpr (matrixDim == Eigen::Dynamic) { + X.setZero(3 + k, 3 + k); + } else { + X.setZero(); + } + X.block(0, 0, 3, 3) = skewSymmetric(xi(0), xi(1), xi(2)); + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; + X.block(0, 3 + i, 3, 1) = xi.template segment<3>(idx); + } + return X; +} + +template +typename ExtendedPose3::TangentVector +ExtendedPose3::Vee(const LieAlgebra& X) { + if (X.rows() != X.cols() || X.rows() < 3) { + throw std::invalid_argument("ExtendedPose3::Vee: invalid matrix shape."); + } + + const Eigen::Index k = [&]() -> Eigen::Index { + if constexpr (K == Eigen::Dynamic) { + return X.cols() - 3; + } else { + if (X.rows() != matrixDim) { + throw std::invalid_argument( + "ExtendedPose3::Vee: invalid matrix shape."); + } + return static_cast(K); + } + }(); + + TangentVector xi; + if constexpr (dimension == Eigen::Dynamic) { + xi.resize(3 + 3 * k); + xi.setZero(); + } else { + xi.setZero(); + } + xi(0) = X(2, 1); + xi(1) = X(0, 2); + xi(2) = X(1, 0); + for (Eigen::Index i = 0; i < k; ++i) { + const Eigen::Index idx = 3 + 3 * i; + xi.template segment<3>(idx) = X.template block<3, 1>(0, 3 + i); + } + return xi; +} + +} // namespace gtsam diff --git a/gtsam/geometry/ExtendedPose3.cpp b/gtsam/geometry/ExtendedPose3.cpp new file mode 100644 index 0000000000..128233cd18 --- /dev/null +++ b/gtsam/geometry/ExtendedPose3.cpp @@ -0,0 +1,28 @@ +/* ---------------------------------------------------------------------------- + + * 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 Explicit instantiations for ExtendedPose3 + * @author Frank Dellaert, et al. + */ + +#include + +namespace gtsam { + +template class GTSAM_EXPORT ExtendedPose3<1>; +template class GTSAM_EXPORT ExtendedPose3<2>; +template class GTSAM_EXPORT ExtendedPose3<3>; +template class GTSAM_EXPORT ExtendedPose3<4>; +template class GTSAM_EXPORT ExtendedPose3<6>; + +} // namespace gtsam diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h new file mode 100644 index 0000000000..0d121a94b1 --- /dev/null +++ b/gtsam/geometry/ExtendedPose3.h @@ -0,0 +1,410 @@ +/* ---------------------------------------------------------------------------- + + * 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 Extended pose Lie group SE_k(3), with static or dynamic k. + * @author Frank Dellaert, et al. + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#if GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + +namespace gtsam { + +template +class ExtendedPose3; + +/** + * Lie group SE_k(3): semidirect product of SO(3) with k copies of R^3. + * State ordering is (R, x_1, ..., x_k) with R in SO(3) and x_i in R^3. + * Tangent ordering is [omega, rho_1, ..., rho_k], each block in R^3. + * + * The manifold dimension is 3+3k and the homogeneous matrix size is 3+k. + * Template parameter K can be fixed (K >= 1) or Eigen::Dynamic. + */ +template +class ExtendedPose3 + : public MatrixLieGroup, + ExtendedPose3, Derived>, + (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K, + (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + K> { + public: + using This = std::conditional_t, + ExtendedPose3, Derived>; + inline constexpr static int dimension = + (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K; + inline constexpr static int matrixDim = + (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + K; + + using Base = MatrixLieGroup; + using TangentVector = typename Base::TangentVector; + using Jacobian = typename Base::Jacobian; + using ChartJacobian = typename Base::ChartJacobian; + using ComponentJacobian = + std::conditional_t, + OptionalJacobian<3, dimension>>; + /// Homogeneous matrix representation in the group. + using MatrixRep = Eigen::Matrix; + /// Lie algebra matrix type used by Hat/Vee. + using LieAlgebra = Eigen::Matrix; + using Matrix3K = Eigen::Matrix; + + static_assert(K == Eigen::Dynamic || K >= 1, + "ExtendedPose3: K should be >= 1 or Eigen::Dynamic."); + + protected: + Rot3 R_; ///< Rotation component. + Matrix3K t_; ///< K translation-like columns in world frame. + + template + using IsDynamic = typename std::enable_if::type; + template + using IsFixed = typename std::enable_if= 1, void>::type; + + public: + /// @name Constructors + /// @{ + + /** + * Construct a fixed-size identity element. + * + * For fixed K, this creates R=I and x_i=0 for i=1..k. + * The manifold dimension is 3+3k and matrix size is (3+k)x(3+k). + */ + template > + ExtendedPose3() : R_(Rot3::Identity()), t_(Matrix3K::Zero()) {} + + /** + * Construct a dynamic-size identity element. + * + * @param k Number of R^3 blocks. + * Creates R=I and x_i=0 for i=1..k. + * The manifold dimension is 3+3k and matrix size is (3+k)x(3+k). + */ + template > + explicit ExtendedPose3(size_t k = 0) + : R_(Rot3::Identity()), t_(3, static_cast(k)) { + t_.setZero(); + } + + /** Copy constructor. */ + ExtendedPose3(const ExtendedPose3&) = default; + + /** Copy assignment. */ + ExtendedPose3& operator=(const ExtendedPose3&) = default; + + /** + * Construct from rotation and 3xk block. + * + * @param R Rotation in SO(3). + * @param x Matrix in R^(3xk), where column i stores x_i. + */ + ExtendedPose3(const Rot3& R, const Matrix3K& x); + + /** + * Construct from homogeneous matrix representation. + * + * @param T Homogeneous matrix in R^((3+k)x(3+k)). + * Top-left 3x3 is R, top-right 3xk stores x_1..x_k. + */ + explicit ExtendedPose3(const MatrixRep& T); + + /// @} + /// @name Access + /// @{ + + /** + * Runtime manifold dimension helper. + * + * @param k Number of R^3 blocks. + * @return 3+3k. + */ + static size_t Dimension(size_t k) { return 3 + 3 * k; } + + /** @return Number of R^3 blocks, k. */ + size_t k() const { return static_cast(t_.cols()); } + + /** @return Runtime manifold dimension, 3+3k. */ + size_t dim() const { return Dimension(k()); } + + /** + * Rotation component. + * + * @param H Optional Jacobian in R^(3xdim) for local rotation coordinates. + * @return Rotation R. + */ + const Rot3& rotation(ComponentJacobian H = {}) const; + + /** + * i-th R^3 component, returned by value. + * + * @param i Zero-based block index in [0, k). + * @param H Optional Jacobian in R^(3xdim). + * @return x_i in R^3. + */ + Point3 x(size_t i, ComponentJacobian H = {}) const; + + /** + * Access all x_i blocks. + * + * @return Matrix in R^(3xk) with columns x_1..x_k. + */ + const Matrix3K& xMatrix() const; + + /** + * Mutable access to all x_i blocks. + * + * @return Matrix in R^(3xk) with columns x_1..x_k. + */ + Matrix3K& xMatrix(); + + /// @} + /// @name Testable + /// @{ + + /** + * Print this state. + * + * @param s Optional prefix string. + */ + void print(const std::string& s = "") const; + + /** + * Equality check with tolerance. + * + * @param other Other state. + * @param tol Absolute tolerance. + * @return True if rotation and all x_i blocks are equal within tol. + */ + bool equals(const ExtendedPose3& other, double tol = 1e-9) const; + + /// @} + /// @name Group + /// @{ + + /** + * Identity element for fixed-size K. + * + * @return Identity with manifold dimension 3+3k and matrix size 3+k. + */ + template > + static This Identity() { + return MakeReturn(ExtendedPose3()); + } + + /** + * Identity element for dynamic-size K. + * + * @param k Number of R^3 blocks. + * @return Identity with manifold dimension 3+3k and matrix size 3+k. + */ + template > + static This Identity(size_t k = 0) { + return MakeReturn(ExtendedPose3(k)); + } + + /** + * Group inverse. + * + * @return X^{-1}. + */ + This inverse() const; + + /** + * Group composition. + * + * @param other Right-hand operand with the same k. + * @return this * other. + */ + This operator*(const This& other) const; + + /// @} + /// @name Lie Group + /// @{ + + /** + * Exponential map from tangent to group. + * + * @param xi Tangent vector in R^dim. + * @param Hxi Optional Jacobian in R^(dimxdim). + * @return Group element in SE_k(3). + */ + static This Expmap(const TangentVector& xi, ChartJacobian Hxi = {}); + + /** + * Logarithm map from group to tangent. + * + * @param pose Group element in SE_k(3). + * @param Hpose Optional Jacobian in R^(dimxdim). + * @return Tangent vector in R^dim. + */ + static TangentVector Logmap(const This& pose, ChartJacobian Hpose = {}); + + /** + * Adjoint map. + * + * @return Matrix in R^(dimxdim). + */ + Jacobian AdjointMap() const; + + /** + * Lie algebra adjoint map. + * + * @param xi Tangent vector in R^dim. + * @return ad_xi matrix in R^(dimxdim). + */ + static Jacobian adjointMap(const TangentVector& xi); + + /** + * Jacobian of Expmap. + * + * @param xi Tangent vector in R^dim. + * @return Matrix in R^(dimxdim). + */ + static Jacobian ExpmapDerivative(const TangentVector& xi); + + /** + * Jacobian of Logmap evaluated from tangent coordinates. + * + * @param xi Tangent vector in R^dim. + * @return Matrix in R^(dimxdim). + */ + static Jacobian LogmapDerivative(const TangentVector& xi); + + /** + * Jacobian of Logmap evaluated at a group element. + * + * @param pose Group element in SE_k(3). + * @return Matrix in R^(dimxdim). + */ + static Jacobian LogmapDerivative(const This& pose); + + /** Chart operations at identity for LieGroup/Manifold compatibility. */ + struct ChartAtOrigin { + /** + * Retract at identity. + * + * @param xi Tangent vector in R^dim. + * @param Hxi Optional Jacobian in R^(dimxdim). + * @return Expmap(xi). + */ + static This Retract(const TangentVector& xi, ChartJacobian Hxi = {}); + + /** + * Local coordinates at identity. + * + * @param pose Group element in SE_k(3). + * @param Hpose Optional Jacobian in R^(dimxdim). + * @return Logmap(pose) in R^dim. + */ + static TangentVector Local(const This& pose, ChartJacobian Hpose = {}); + }; + + using LieGroup::inverse; + + /// @} + /// @name Matrix Lie Group + /// @{ + + /** + * Homogeneous matrix representation. + * + * @return Matrix in R^((3+k)x(3+k)). + */ + MatrixRep matrix() const; + + /** + * Hat operator from tangent to Lie algebra. + * + * @param xi Tangent vector in R^dim. + * @return Matrix in R^((3+k)x(3+k)). + */ + static LieAlgebra Hat(const TangentVector& xi); + + /** + * Vee operator from Lie algebra to tangent. + * + * @param X Matrix in R^((3+k)x(3+k)). + * @return Tangent vector in R^dim. + */ + static TangentVector Vee(const LieAlgebra& X); + + /// @} + + friend std::ostream& operator<<(std::ostream& os, const ExtendedPose3& p) { + os << "R: " << p.R_ << "\n"; + os << "x: " << p.t_; + return os; + } + + protected: + static This MakeReturn(const ExtendedPose3& value) { + if constexpr (std::is_void_v) { + return value; + } else { + return This(value); + } + } + + static const ExtendedPose3& AsBase(const This& value) { + if constexpr (std::is_void_v) { + return value; + } else { + return static_cast(value); + } + } + + static size_t RuntimeK(const TangentVector& xi); + static void ZeroJacobian(ChartJacobian H, Eigen::Index d); + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(R_); + ar& BOOST_SERIALIZATION_NVP(t_); + } +#endif +}; + +/// Convenience typedef for dynamic k. +using ExtendedPose3Dynamic = ExtendedPose3; + +template +struct traits> + : public internal::MatrixLieGroup, + ExtendedPose3::matrixDim> {}; + +template +struct traits> + : public internal::MatrixLieGroup, + ExtendedPose3::matrixDim> {}; + +} // namespace gtsam + +#include "ExtendedPose3-inl.h" diff --git a/gtsam/geometry/Gal3.cpp b/gtsam/geometry/Gal3.cpp index 6e5be3b7c6..abcc05eeb4 100644 --- a/gtsam/geometry/Gal3.cpp +++ b/gtsam/geometry/Gal3.cpp @@ -389,29 +389,6 @@ Gal3::Jacobian Gal3::AdjointMap() const { return Ad; } -//------------------------------------------------------------------------------ -Gal3::TangentVector Gal3::Adjoint(const TangentVector& xi, OptionalJacobian<10, 10> H_g, OptionalJacobian<10, 10> H_xi) const { - Jacobian Ad = AdjointMap(); - TangentVector y = Ad * xi; - - if (H_xi) { - *H_xi = Ad; - } - - if (H_g) { - // NOTE: Using numerical derivative for the Jacobian with respect to - // the group element instead of deriving the analytical expression. - // Future work to use analytical instead. - std::function adjoint_action_wrt_g = - [&](const Gal3& g_in, const TangentVector& xi_in) { - return g_in.Adjoint(xi_in); - }; - *H_g = numericalDerivative21(adjoint_action_wrt_g, *this, xi, 1e-7); - } - return y; -} - -//------------------------------------------------------------------------------ Gal3::Jacobian Gal3::adjointMap(const TangentVector& xi) { // Implements adjoint representation as in Equation 28, Page 10 const Matrix3 Omega = skewSymmetric(xi_w(xi)); @@ -428,16 +405,6 @@ Gal3::Jacobian Gal3::adjointMap(const TangentVector& xi) { return ad; } -//------------------------------------------------------------------------------ -Gal3::TangentVector Gal3::adjoint(const TangentVector& xi, const TangentVector& y, OptionalJacobian<10, 10> Hxi, OptionalJacobian<10, 10> Hy) { - Jacobian ad_xi = adjointMap(xi); - if (Hy) *Hy = ad_xi; - if (Hxi) { - *Hxi = -adjointMap(y); - } - return ad_xi * y; -} - //------------------------------------------------------------------------------ Gal3::Jacobian Gal3::ExpmapDerivative(const TangentVector& xi) { Gal3::Jacobian J; diff --git a/gtsam/geometry/Gal3.h b/gtsam/geometry/Gal3.h index a03689ba34..f824898007 100644 --- a/gtsam/geometry/Gal3.h +++ b/gtsam/geometry/Gal3.h @@ -188,17 +188,6 @@ class GTSAM_EXPORT Gal3 : public MatrixLieGroup { /// Calculate Adjoint map Ad_g Jacobian AdjointMap() const; - /// Apply this element's AdjointMap Ad_g to a tangent vector xi_base at - /// identity - TangentVector Adjoint(const TangentVector& xi_base, - OptionalJacobian<10, 10> H_g = {}, - OptionalJacobian<10, 10> H_xi = {}) const; - - /// The adjoint action `ad(xi, y)` = `adjointMap(xi) * y` - static TangentVector adjoint(const TangentVector& xi, const TangentVector& y, - OptionalJacobian<10, 10> Hxi = {}, - OptionalJacobian<10, 10> Hy = {}); - /// Compute the adjoint map `ad(xi)` associated with tangent vector xi static Jacobian adjointMap(const TangentVector& xi); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 2fd5d34dc5..5da5c24ab4 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -158,30 +158,11 @@ class GTSAM_EXPORT Pose2: public MatrixLieGroup { */ Matrix3 AdjointMap() const; - /// Apply AdjointMap to twist xi - inline Vector3 Adjoint(const Vector3& xi) const { - return AdjointMap()*xi; - } - /** * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 */ static Matrix3 adjointMap(const Vector3& v); - /** - * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives - */ - static Vector3 adjoint(const Vector3& xi, const Vector3& y) { - return adjointMap(xi) * y; - } - - /** - * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. - */ - static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) { - return adjointMap(xi).transpose() * y; - } - // temporary fix for wrappers until case issue is resolved static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);} static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);} @@ -393,4 +374,3 @@ template struct Range : HasRange {}; } // namespace gtsam - diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 163c3c0a7f..846be08064 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -31,10 +31,9 @@ namespace gtsam { GTSAM_CONCEPT_POSE_INST(Pose3) /* ************************************************************************* */ -Pose3::Pose3(const Pose2& pose2) : - R_(Rot3::Rodrigues(0, 0, pose2.theta())), t_( - Point3(pose2.x(), pose2.y(), 0)) { -} +Pose3::Pose3(const Pose2& pose2) + : Base(Rot3::Rodrigues(0, 0, pose2.theta()), + Vector3(pose2.x(), pose2.y(), 0.0)) {} /* ************************************************************************* */ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR, @@ -58,124 +57,6 @@ Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { return Pose3(p); } -/* ************************************************************************* */ -Pose3 Pose3::inverse() const { - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt * (-t_)); -} - -/* ************************************************************************* */ -// Calculate Adjoint map -// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) -Matrix6 Pose3::AdjointMap() const { - const Matrix3 R = R_.matrix(); - Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; - Matrix6 adj; - adj << R, Z_3x3, A, R; // Gives [R 0; A R] - return adj; -} - -/* ************************************************************************* */ -// Calculate AdjointMap applied to xi_b, with Jacobians -Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose, - OptionalJacobian<6, 6> H_xib) const { - const Matrix6 Ad = AdjointMap(); - - // Jacobians - // D1 Ad_T(xi_b) = D1 Ad_T Ad_I(xi_b) = Ad_T * D1 Ad_I(xi_b) = Ad_T * ad_xi_b - // D2 Ad_T(xi_b) = Ad_T - // See docs/math.pdf for more details. - // In D1 calculation, we could be more efficient by writing it out, but do not - // for readability - if (H_pose) *H_pose = -Ad * adjointMap(xi_b); - if (H_xib) *H_xib = Ad; - - return Ad * xi_b; -} - -/* ************************************************************************* */ -/// The dual version of Adjoint -Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, - OptionalJacobian<6, 6> H_x) const { - const Matrix6 Ad = AdjointMap(); - const Vector6 AdTx = Ad.transpose() * x; - - // Jacobians - // See docs/math.pdf for more details. - if (H_pose) { - const auto w_T_hat = skewSymmetric(AdTx.head<3>()), - v_T_hat = skewSymmetric(AdTx.tail<3>()); - *H_pose << w_T_hat, v_T_hat, // - /* */ v_T_hat, Z_3x3; - } - if (H_x) { - *H_x = Ad.transpose(); - } - - return AdTx; -} - -/* ************************************************************************* */ -Matrix6 Pose3::adjointMap(const Vector6& xi) { - Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); - Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); - Matrix6 adj; - adj << w_hat, Z_3x3, v_hat, w_hat; - - return adj; -} - -/* ************************************************************************* */ -Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { - if (Hxi) { - Hxi->setZero(); - for (int i = 0; i < 6; ++i) { - Vector6 dxi; - dxi.setZero(); - dxi(i) = 1.0; - Matrix6 Gi = adjointMap(dxi); - Hxi->col(i) = Gi * y; - } - } - const Matrix6& ad_xi = adjointMap(xi); - if (H_y) *H_y = ad_xi; - return ad_xi * y; -} - -/* ************************************************************************* */ -Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { - if (Hxi) { - Hxi->setZero(); - for (int i = 0; i < 6; ++i) { - Vector6 dxi; - dxi.setZero(); - dxi(i) = 1.0; - Matrix6 GTi = adjointMap(dxi).transpose(); - Hxi->col(i) = GTi * y; - } - } - const Matrix6& adT_xi = adjointMap(xi).transpose(); - if (H_y) *H_y = adT_xi; - return adT_xi * y; -} - -/* ************************************************************************* */ -Matrix4 Pose3::Hat(const Vector6& xi) { - Matrix4 X; - const double wx = xi(0), wy = xi(1), wz = xi(2), vx = xi(3), vy = xi(4), vz = xi(5); - X << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.; - return X; -} - -/* ************************************************************************* */ -Vector6 Pose3::Vee(const Matrix4& Xi) { - Vector6 xi; - xi << Xi(2, 1), Xi(0, 2), Xi(1, 0), Xi(0, 3), Xi(1, 3), Xi(2, 3); - return xi; -} - /* ************************************************************************* */ void Pose3::print(const std::string& s) const { std::cout << (s.empty() ? s : s + " ") << *this << std::endl; @@ -209,13 +90,9 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t, } return Pose3(interpolate(R_, T.R_, t), interpolate(t_, T.t_, t)); - } /* ************************************************************************* */ -// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's -// elegant Lie group document, at https://www.ethaneade.org/lie.pdf. -// See also [this document](doc/Jacobians.md) Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // Get angular velocity omega and translational velocity v from twist xi const Vector3 w = xi.head<3>(), v = xi.tail<3>(); @@ -231,57 +108,33 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { #endif // The translation t = local.Jacobian().left() * v. - // Here we call local.Jacobian().applyLeft, which is faster if you don't need - // Jacobians, and returns Jacobian of t with respect to w if asked. - // NOTE(Frank): this does the same as the intuitive formulas: - // t_parallel = w * w.dot(v); // translation parallel to axis - // w_cross_v = w.cross(v); // translation orthogonal to axis - // t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; - // but Local does not need R, deals automatically with the case where theta2 - // is near zero, and also gives us the machinery for the Jacobians. Matrix3 H; const Vector3 t = local.Jacobian().applyLeft(v, Hxi ? &H : nullptr); if (Hxi) { // The Jacobian of expmap is given by the right Jacobian of SO(3): const Matrix3 Jr = local.Jacobian().right(); - // We are creating a Pose3, so we still need to chain H with R^T, the - // Jacobian of Pose3::Create with respect to t. + // Chain H with R^T, the Jacobian of Pose3::Create with respect to t. const Matrix3 Rt = R.transpose(); *Hxi << Jr, Z_3x3, // Jr here *is* the Jacobian of expmap - Rt * H, Jr; // Here Jr = R^T * Jl, with Jl the Jacobian of t in v. + Rt * H, Jr; // Jr = R^T * Jl, with Jl Jacobian of t in v. } return Pose3(R, t); } -/* ************************************************************************* */ -Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) { - const Vector3 w = Rot3::Logmap(pose.rotation()); - - // Instantiate functor for Dexp-related operations: - const so3::DexpFunctor local(w); - - const Vector3 t = pose.translation(); - const Vector3 u = local.InvJacobian().applyLeft(t); - Vector6 xi; - xi << w, u; - if (Hpose) *Hpose = LogmapDerivative(xi); - return xi; -} - /* ************************************************************************* */ Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) { #ifdef GTSAM_POSE3_EXPMAP return Expmap(xi, Hxi); #else Matrix3 DR; - Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : 0); + const Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : nullptr); if (Hxi) { - *Hxi = I_6x6; - Hxi->topLeftCorner<3, 3>() = DR; + Hxi->setIdentity(); + Hxi->block<3, 3>(0, 0) = DR; } - return Pose3(R, Point3(xi.tail<3>())); + return Pose3(R, xi.tail<3>()); #endif } @@ -291,99 +144,23 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { return Logmap(pose, Hpose); #else Matrix3 DR; - Vector3 omega = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : 0); + Vector6 xi; + xi.head<3>() = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : nullptr); + xi.tail<3>() = pose.translation(); if (Hpose) { - *Hpose = I_6x6; - Hpose->topLeftCorner<3, 3>() = DR; + Hpose->setIdentity(); + Hpose->block<3, 3>(0, 0) = DR; } - Vector6 xi; - xi << omega, pose.translation(); return xi; #endif } -/* ************************************************************************* */ -Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { - Matrix6 J; - Expmap(xi, J); - return J; -} - -/* ************************************************************************* */ -Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { - const Vector3 w = xi.head<3>(); - Vector3 v = xi.segment<3>(3); - - // Instantiate functor for Dexp-related operations: - const so3::DexpFunctor local(w); - - // Call applyLeftJacobian to get its Jacobians - Matrix3 H_t_w; - local.Jacobian().applyLeft(v, H_t_w); - - // Multiply with R^T to account for NavState::Create Jacobian. - const Matrix3 R = local.expmap(); - const Matrix3 Qt = R.transpose() * H_t_w; - - // Now compute the blocks of the LogmapDerivative Jacobian - const Matrix3 Jw = Rot3::LogmapDerivative(w); - const Matrix3 Qt2 = -Jw * Qt * Jw; - - Matrix6 J; - J << Jw, Z_3x3, Qt2, Jw; - return J; -} - -/* ************************************************************************* */ -Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { - const Vector6 xi = Logmap(pose); - return LogmapDerivative(xi); -} - /* ************************************************************************* */ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { if (Hself) *Hself << Z_3x3, rotation().matrix(); return t_; } -/* ************************************************************************* */ -const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const { - if (Hself) { - *Hself << I_3x3, Z_3x3; - } - return R_; -} - -/* ************************************************************************* */ -Matrix4 Pose3::matrix() const { - static const auto A14 = Eigen::RowVector4d(0,0,0,1); - Matrix4 mat; - mat << R_.matrix(), t_, A14; - return mat; -} - -/* ************************************************************************* */ -Pose3::Vector16 Pose3::vec(OptionalJacobian<16, 6> H) const { - // Vectorize - const Matrix4 M = matrix(); - const Vector16 v = Eigen::Map(M.data()); - - // If requested, calculate H - if (H) { - H->setZero(); - auto R = M.block<3, 3>(0, 0); - H->block<3, 1>(0, 1) = -R.col(2); - H->block<3, 1>(0, 2) = R.col(1); - H->block<3, 1>(4, 0) = R.col(2); - H->block<3, 1>(4, 2) = -R.col(0); - H->block<3, 1>(8, 0) = -R.col(1); - H->block<3, 1>(8, 1) = R.col(0); - H->block<3,3>(12,3) = R; - } - - return v; -} - /* ************************************************************************* */ Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself, OptionalJacobian<6, 6> HaTb) const { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index a6b9c40b94..6d4bf0c3bc 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -20,10 +20,15 @@ #include #include +#include #include #include #include +#if GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + namespace gtsam { class Pose2; @@ -34,45 +39,41 @@ class Pose2; * @ingroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose3: public MatrixLieGroup { +class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> { public: + using Base = ExtendedPose3<1, Pose3>; /** Pose Concept requirements */ typedef Rot3 Rotation; typedef Point3 Translation; - -private: - - Rot3 R_; ///< Rotation gRp, between global and pose frame - Point3 t_; ///< Translation gPp, from global origin to pose frame origin + inline constexpr static auto dimension = 6; public: using Vector16 = Eigen::Matrix; + using Base::operator*; /// @name Standard Constructors /// @{ /** Default constructor is origin */ - Pose3() : R_(traits::Identity()), t_(traits::Identity()) {} + Pose3() : Base() {} /** Copy constructor */ Pose3(const Pose3& pose) = default; Pose3& operator=(const Pose3& other) = default; + Pose3(const Base& other) : Base(other) {} + /** Construct from R,t */ - Pose3(const Rot3& R, const Point3& t) : - R_(R), t_(t) { - } + Pose3(const Rot3& R, const Point3& t) + : Base(R, Vector3(t.x(), t.y(), t.z())) {} /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); /** Constructor from 4*4 matrix */ - Pose3(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)), t_(T(0, 3), T(1, 3), T(2, 3)) { - } + Pose3(const Matrix &T) : Base(Matrix4(T)) {} /// Named constructor with derivatives static Pose3 Create(const Rot3& R, const Point3& t, @@ -106,19 +107,6 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup { /// @name Group /// @{ - /// identity for group operation - static Pose3 Identity() { - return Pose3(); - } - - /// inverse transformation - Pose3 inverse() const; - - /// compose syntactic sugar - Pose3 operator*(const Pose3& T) const { - return Pose3(R_ * T.R_, t_ + R_ * T.t_); - } - /** * Interpolate between two poses via individual rotation and translation * interpolation. @@ -138,103 +126,30 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup { OptionalJacobian<6, 6> Harg = {}, OptionalJacobian<6, 1> Ht = {}) const; + /// Compose syntactic sugar. + Pose3 operator*(const Pose3& T) const { + return Pose3(R_ * T.R_, t_ + R_ * T.t_); + } + /// @} /// @name Lie Group /// @{ using LieAlgebra = Matrix4; - /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ + /// Exponential map at identity. static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {}); - /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation - static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {}); - - /** - * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame - * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) - */ - Matrix6 AdjointMap() const; - - /** - * 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$ - * Note that H_xib = AdjointMap() - */ - Vector6 Adjoint(const Vector6& xi_b, - OptionalJacobian<6, 6> H_this = {}, - OptionalJacobian<6, 6> H_xib = {}) const; - - /// The dual version of Adjoint - Vector6 AdjointTranspose(const Vector6& x, - OptionalJacobian<6, 6> H_this = {}, - OptionalJacobian<6, 6> H_x = {}) const; - - /** - * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 - * [ad(w,v)] = [w^, zero3; v^, w^] - * Note that this is the matrix representation of the adjoint operator for se3 Lie algebra, - * aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3. - * - * Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its - * vector representation. - * We have the following relationship: - * \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$ - * - * We use this to compute the discrete version of the inverse right-trivialized tangent map, - * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. - * - */ - static Matrix6 adjointMap(const Vector6& xi); - - /** - * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives - */ - static Vector6 adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi = {}, - OptionalJacobian<6, 6> H_y = {}); - // temporary fix for wrappers until case issue is resolved static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);} - /** - * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. - */ - static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi = {}, - OptionalJacobian<6, 6> H_y = {}); - - /// Derivative of Expmap - static Matrix6 ExpmapDerivative(const Vector6& xi); - - /// Derivative of Logmap - static Matrix6 LogmapDerivative(const Vector6& xi); - - /// Derivative of Logmap, Pose3 version. TODO(Frank): deprecate? - static Matrix6 LogmapDerivative(const Pose3& pose); - // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP struct GTSAM_EXPORT ChartAtOrigin { static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {}); static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {}); }; - using LieGroup::inverse; // version with derivative - - /** - * Hat for Pose3: - * @param xi 6-dim twist (omega,v) where - * omega = (wx,wy,wz) 3D angular velocity - * v (vx,vy,vz) = 3D velocity - * @return xihat, 4*4 element of Lie algebra that can be exponentiated - */ - static Matrix4 Hat(const Vector6& xi); - - /// Vee maps from Lie algebra to tangent vector - static Vector6 Vee(const Matrix4& X); - /// @} /// @name Group Action on Point3 /// @{ @@ -282,33 +197,24 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup { /// @name Standard Interface /// @{ - /// get rotation - const Rot3& rotation(OptionalJacobian<3, 6> Hself = {}) const; - /// get translation const Point3& translation(OptionalJacobian<3, 6> Hself = {}) const; /// get x double x() const { - return t_.x(); + return translation().x(); } /// get y double y() const { - return t_.y(); + return translation().y(); } /// get z double z() const { - return t_.z(); + return translation().z(); } - /** convert to 4*4 matrix */ - Matrix4 matrix() const; - - /// Return vectorized SE(3) matrix in column order. - Vector16 vec(OptionalJacobian<16, 6> H = {}) const; - /** * Assuming self == wTa, takes a pose aTb in local coordinates * and transforms it to world coordinates wTb = wTa * aTb. diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index b18a32d5e0..0d82e5bdb2 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -121,7 +121,7 @@ Unit3 Rot3::unrotate(const Unit3& p, OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { Matrix32 Dp; Unit3 q = Unit3(unrotate(p.point3(Dp))); - if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp; + if (Hp) *Hp = q.basis().transpose() * matrix().transpose() * Dp; if (HR) *HR = q.basis().transpose() * q.skew(); return q; } @@ -245,19 +245,6 @@ Matrix3 Rot3::LogmapDerivative(const Vector3& x) { return SO3::LogmapDerivative(x); } -/* ************************************************************************* */ -Matrix3 Rot3::adjointMap(const Vector3& xi) { return Hat(xi); } - -/* ************************************************************************* */ -Vector3 Rot3::adjoint(const Vector3& xi, const Vector3& y, - OptionalJacobian<3, 3> Hxi, - OptionalJacobian<3, 3> Hy) { - const Matrix3 ad_xi = adjointMap(xi); - if (Hxi) *Hxi = -Hat(y); - if (Hy) *Hy = ad_xi; - return ad_xi * y; -} - /* ************************************************************************* */ pair RQ(const Matrix3& A, OptionalJacobian<3, 9> H) { const double x = -atan2(-A(2, 1), A(2, 2)); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 31947c6491..5c5b0496cd 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -55,7 +55,7 @@ namespace gtsam { * if it is defined. * @ingroup geometry */ -class GTSAM_EXPORT Rot3 : public LieGroup { +class GTSAM_EXPORT Rot3 : public MatrixLieGroup { public: static constexpr size_t MatrixM = 3; private: @@ -397,12 +397,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { Matrix3 AdjointMap() const { return matrix(); } /// Matrix representation of the Lie-algebra adjoint operator ad_xi on so(3). - static Matrix3 adjointMap(const Vector3& xi); - - /// Apply the Lie-algebra adjoint map to y with optional derivatives. - static Vector3 adjoint(const Vector3& xi, const Vector3& y, - OptionalJacobian<3, 3> Hxi = {}, - OptionalJacobian<3, 3> Hy = {}); + static Matrix3 adjointMap(const Vector3& xi) { return Hat(xi); } // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE struct GTSAM_EXPORT ChartAtOrigin { diff --git a/gtsam/geometry/doc/ExtendedPose3.ipynb b/gtsam/geometry/doc/ExtendedPose3.ipynb new file mode 100644 index 0000000000..b2be902286 --- /dev/null +++ b/gtsam/geometry/doc/ExtendedPose3.ipynb @@ -0,0 +1,441 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "090d20ac", + "metadata": {}, + "source": [ + "# ExtendedPose3\n", + "\n", + "The `ExtendedPose3` class is the generic matrix Lie group $SE_K(3)$: the semi-direct product of `SO(3)` with `K` copies of $\\mathbb{R}^3$.\n", + "\n", + "This notebook focuses on Python usage of `ExtendedPose36` (the `K=6` instantiation), including construction, group operations, and manifold/Lie operations.\n" + ] + }, + { + "cell_type": "markdown", + "id": "e5b31ed7", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information\n" + ] + }, + { + "cell_type": "markdown", + "id": "b45cbfc5", + "metadata": {}, + "source": [ + "\"Open\n" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "acccf586", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "# Install GTSAM from pip if running in Google Colab\n", + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not in Colab\n" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "c70f339f", + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Rot3\n", + "\n", + "ExtendedPose36 = gtsam.ExtendedPose36\n" + ] + }, + { + "cell_type": "markdown", + "id": "9b4dc088", + "metadata": {}, + "source": [ + "## Lie Group Math Overview\n", + "\n", + "An element of $SE_K(3)$ is $X=(R, x_1, \\ldots, x_K)$ with $R \\in SO(3)$ and $x_i \\in \\mathbb{R}^3$.\n", + "\n", + "Group composition is:\n", + "\n", + "\\begin{equation}\n", + "(R, x_i) \\cdot (S, y_i) = (RS, x_i + R y_i).\n", + "\\end{equation}\n", + "\n", + "Group inverse is:\n", + "\n", + "\\begin{equation}\n", + "(R, x_i)^{-1} = (R^T, -R^T x_i).\n", + "\\end{equation}\n", + "\n", + "The tangent vector is ordered as $\\xi=[\\omega, \\rho_1, \\ldots, \\rho_K] \\in \\mathbb{R}^{3+3K}$.\n", + "The exponential and logarithm maps are:\n", + "\n", + "\\begin{equation}\n", + "X = \\operatorname{Exp}(\\xi), \\quad \\xi = \\operatorname{Log}(X).\n", + "\\end{equation}\n", + "\n", + "For the matrix-Lie-group view, $\\operatorname{Hat}(\\xi)$ maps tangent vectors to Lie algebra matrices and $\\operatorname{Vee}(\\cdot)$ is the inverse map:\n", + "\n", + "\\begin{equation}\n", + "\\operatorname{Vee}(\\operatorname{Hat}(\\xi)) = \\xi.\n", + "\\end{equation}\n", + "\n", + "The adjoint map transports perturbations between frames:\n", + "\n", + "\\begin{equation}\n", + "\\operatorname{Ad}_X \\eta = \\operatorname{AdjointMap}(X) \\; \\eta.\n", + "\\end{equation}\n", + "\n", + "As a manifold, local updates use retraction and local coordinates:\n", + "\n", + "\\begin{equation}\n", + "X' = X \\oplus \\delta = \\operatorname{Retract}_X(\\delta), \\quad \\delta = \\operatorname{Local}_X(X').\n", + "\\end{equation}\n" + ] + }, + { + "cell_type": "markdown", + "id": "fa791c1f", + "metadata": {}, + "source": [ + "## Constructing `ExtendedPose36`\n", + "\n", + "`ExtendedPose36` stores one `Rot3` and six 3-vectors $x_1,\\dots,x_6$.\n", + "The manifold dimension is `3 + 3*K = 21`, and the homogeneous matrix is `(3+K) x (3+K) = 9 x 9`.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "207ee70e", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "k = 6, dim = 21, Dim = 21\n", + "matrix shape: (9, 9)\n", + "[[1. 0. 0. 0. 0. 0. 0. 0. 0.]\n", + " [0. 1. 0. 0. 0. 0. 0. 0. 0.]\n", + " [0. 0. 1. 0. 0. 0. 0. 0. 0.]\n", + " [0. 0. 0. 1. 0. 0. 0. 0. 0.]\n", + " [0. 0. 0. 0. 1. 0. 0. 0. 0.]\n", + " [0. 0. 0. 0. 0. 1. 0. 0. 0.]\n", + " [0. 0. 0. 0. 0. 0. 1. 0. 0.]\n", + " [0. 0. 0. 0. 0. 0. 0. 1. 0.]\n", + " [0. 0. 0. 0. 0. 0. 0. 0. 1.]]\n" + ] + } + ], + "source": [ + "X_identity = ExtendedPose36()\n", + "print(f\"k = {X_identity.k()}, dim = {X_identity.dim()}, Dim = {ExtendedPose36.Dim()}\")\n", + "print(\"matrix shape:\", X_identity.matrix().shape)\n", + "print(X_identity.matrix())\n" + ] + }, + { + "cell_type": "markdown", + "id": "44ec8306", + "metadata": {}, + "source": [ + "### Construct from rotation + 3xK block\n" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "3c6f232d", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "x(0): [1. 2. 3.]\n", + "xMatrix: [[ 1. 4. -1. 0.5 2.1 -0.3]\n", + " [ 2. 5. 0.5 -1.2 3.3 0.8]\n", + " [ 3. 6. 2. 1.4 -2.7 0.6]]\n" + ] + } + ], + "source": [ + "R = Rot3.Yaw(np.deg2rad(20.0))\n", + "X_block = np.array([\n", + " [1.0, 4.0, -1.0, 0.5, 2.1, -0.3],\n", + " [2.0, 5.0, 0.5, -1.2, 3.3, 0.8],\n", + " [3.0, 6.0, 2.0, 1.4, -2.7, 0.6],\n", + "])\n", + "\n", + "X1 = ExtendedPose36(R, X_block)\n", + "print(\"x(0):\", np.array(X1.x(0)))\n", + "print(\"xMatrix:\", X1.xMatrix())\n" + ] + }, + { + "cell_type": "markdown", + "id": "2c4a45a8", + "metadata": {}, + "source": [ + "### Construct from homogeneous matrix\n" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "3408e416", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "equals: True\n" + ] + } + ], + "source": [ + "T = X1.matrix()\n", + "X1_from_T = ExtendedPose36(T)\n", + "print(\"equals:\", X1.equals(X1_from_T, 1e-9))" + ] + }, + { + "cell_type": "markdown", + "id": "caa03a92", + "metadata": {}, + "source": [ + "## Group operations\n", + "\n", + "Composition, inverse, and between follow the same `Pose3`-style API.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "51421896", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "compose == matrix multiply: True\n", + "between == inverse-compose: True\n" + ] + } + ], + "source": [ + "xi2 = np.array([\n", + " 0.02, -0.01, 0.03,\n", + " 0.1, 0.2, -0.1,\n", + " -0.2, 0.3, 0.4,\n", + " 0.5, -0.6, 0.2,\n", + " -0.3, 0.1, 0.2,\n", + " 0.4, 0.2, -0.5,\n", + " -0.1, 0.7, 0.2,\n", + "])\n", + "X2 = ExtendedPose36.Expmap(xi2)\n", + "\n", + "X12 = X1.compose(X2)\n", + "print(\"compose == matrix multiply:\", np.allclose(X12.matrix(), X1.matrix() @ X2.matrix(), atol=1e-9))\n", + "\n", + "X_between = X1.between(X2)\n", + "X_between_expected = X1.inverse().compose(X2)\n", + "print(\"between == inverse-compose:\", X_between.equals(X_between_expected, 1e-9))\n" + ] + }, + { + "cell_type": "markdown", + "id": "45698ac4", + "metadata": {}, + "source": [ + "## Expmap / Logmap and tangent vectors\n" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "id": "25a46216", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "log(exp(xi)) ~= xi: True\n" + ] + } + ], + "source": [ + "xi = np.array([\n", + " 0.11, -0.07, 0.05,\n", + " 0.30, -0.40, 0.10,\n", + " -0.20, 0.60, -0.50,\n", + " 0.70, -0.10, 0.20,\n", + " -0.40, 0.30, 0.80,\n", + " 0.50, -0.20, -0.30,\n", + " 0.10, 0.20, -0.60,\n", + "])\n", + "\n", + "X = ExtendedPose36.Expmap(xi)\n", + "xi_round_trip = ExtendedPose36.Logmap(X)\n", + "print(\"log(exp(xi)) ~= xi:\", np.allclose(xi_round_trip, xi, atol=1e-9))" + ] + }, + { + "cell_type": "markdown", + "id": "1a2eb160", + "metadata": {}, + "source": [ + "### Hat / Vee\n" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "id": "576cb79d", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "vee(hat(xi)) ~= xi: True\n" + ] + } + ], + "source": [ + "X_hat = ExtendedPose36.Hat(xi)\n", + "xi_from_hat = ExtendedPose36.Vee(X_hat)\n", + "print(\"vee(hat(xi)) ~= xi:\", np.allclose(xi_from_hat, xi, atol=1e-9))" + ] + }, + { + "cell_type": "markdown", + "id": "494d04cc", + "metadata": {}, + "source": [ + "## Manifold operations\n", + "\n", + "As with `Pose3`, optimization-facing operations are `retract` and `localCoordinates`.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "id": "20419fb4", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "localCoordinates(retract(delta)) ~= delta: True\n" + ] + } + ], + "source": [ + "delta = 1e-2 * np.ones(21)\n", + "X_perturbed = X1.retract(delta)\n", + "delta_back = X1.localCoordinates(X_perturbed)\n", + "print(\"localCoordinates(retract(delta)) ~= delta:\", np.allclose(delta_back, delta, atol=1e-7))" + ] + }, + { + "cell_type": "markdown", + "id": "cde834ce", + "metadata": {}, + "source": [ + "## Adjoint\n" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "id": "46a15f60", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "AdjointMap shape: (21, 21)\n", + "Adjoint(eta) == AdjointMap @ eta: True\n" + ] + } + ], + "source": [ + "eta = np.linspace(-0.3, 0.3, 21)\n", + "Ad = X1.AdjointMap()\n", + "eta_adjoint = X1.Adjoint(eta)\n", + "print(\"AdjointMap shape:\", Ad.shape)\n", + "print(\"Adjoint(eta) == AdjointMap @ eta:\", np.allclose(eta_adjoint, Ad @ eta, atol=1e-9))" + ] + }, + { + "cell_type": "markdown", + "id": "6a2a253f", + "metadata": {}, + "source": [ + "## Notes and references\n", + "\n", + "`ExtendedPose3` is meaning-agnostic: it only defines the group structure. Semantic interpretation of each $x_i$ (e.g., position, velocity, bias, contact points) is left to derived/application-level code.\n", + "\n", + "For an invariant-filter application context (including walking-robot style state design ideas), see:\n", + "- [InvariantEKF notebook](../../navigation/doc/InvariantEKF.ipynb)\n", + "- [NavState notebook](../../navigation/doc/NavState.ipynb)\n", + "\n", + "For `Pose3`-specific behavior and conventions, see:\n", + "- [Pose3 notebook](./Pose3.ipynb)\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 05b637e075..f282d53c73 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -177,6 +177,12 @@ class Rot2 { gtsam::Vector logmap(const gtsam::Rot2& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -293,6 +299,12 @@ class SO3 { gtsam::Vector3 logmap(const gtsam::SO3& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -332,6 +344,12 @@ class SO4 { gtsam::Vector logmap(const gtsam::SO4& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -373,6 +391,12 @@ class SOn { gtsam::Vector logmap(const gtsam::SOn& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -450,6 +474,12 @@ class Rot3 { gtsam::Vector logmap(const gtsam::Rot3& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -530,13 +560,16 @@ class Pose2 { gtsam::Vector logmap(const gtsam::Pose2& g, Eigen::Ref H1, Eigen::Ref H2); static gtsam::Matrix ExpmapDerivative(gtsam::Vector v); static gtsam::Matrix LogmapDerivative(const gtsam::Pose2& v); + + // Matrix Lie Group gtsam::Matrix AdjointMap() const; gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); static gtsam::Matrix adjointMap_(gtsam::Vector xi); static gtsam::Vector adjoint_(gtsam::Vector xi, gtsam::Vector y); - static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); - - // Matrix Lie Group gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -624,6 +657,7 @@ class Pose3 { gtsam::Vector logmap(const gtsam::Pose3& g); gtsam::Vector logmap(const gtsam::Pose3& g, Eigen::Ref H1, Eigen::Ref H2); + // Matrix Lie Group gtsam::Matrix AdjointMap() const; gtsam::Vector Adjoint(gtsam::Vector xi_b) const; gtsam::Vector Adjoint(gtsam::Vector xi_b, Eigen::Ref H_this, @@ -633,11 +667,9 @@ class Pose3 { Eigen::Ref H_x) const; static gtsam::Matrix adjointMap(gtsam::Vector xi); static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); static gtsam::Matrix adjointMap_(gtsam::Vector xi); static gtsam::Vector adjoint_(gtsam::Vector xi, gtsam::Vector y); - static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); - - // Matrix Lie Group gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -686,6 +718,63 @@ class Pose3 { void serialize() const; }; +#include +template +class ExtendedPose3 { + // Standard Constructors + ExtendedPose3(); + ExtendedPose3(const This& other); + ExtendedPose3(const gtsam::Rot3& R, const gtsam::Matrix& x); + ExtendedPose3(const gtsam::Matrix& T); + + // Testable + void print(string s = "") const; + bool equals(const This& other, double tol = 1e-9) const; + + // Access + size_t k() const; + gtsam::Rot3 rotation() const; + gtsam::Point3 x(size_t i) const; + gtsam::Matrix xMatrix() const; + + // Group + static This Identity(); + This inverse() const; + This compose(const This& g) const; + This between(const This& g) const; + + // Operator Overloads + This operator*(const This& other) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + This retract(gtsam::Vector v) const; + gtsam::Vector localCoordinates(const This& g) const; + + // Lie Group + static This Expmap(gtsam::Vector xi); + static gtsam::Vector Logmap(const This& pose); + static gtsam::Matrix ExpmapDerivative(gtsam::Vector xi); + static gtsam::Matrix LogmapDerivative(gtsam::Vector xi); + static gtsam::Matrix LogmapDerivative(const This& pose); + + // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi_b) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); + gtsam::Vector vec() const; + gtsam::Matrix matrix() const; + static gtsam::Matrix Hat(const gtsam::Vector& xi); + static gtsam::Vector Vee(const gtsam::Matrix& X); + + // enabling serialization functionality + void serialize() const; +}; + #include class SL4 { // Standard constructors @@ -727,6 +816,12 @@ class SL4 { gtsam::Vector logmap(const gtsam::SL4& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); @@ -1412,13 +1507,22 @@ class Similarity2 { static gtsam::Vector Logmap(const gtsam::Similarity2& S); gtsam::Similarity2 expmap(const gtsam::Vector& v); gtsam::Vector logmap(const gtsam::Similarity2& g); + + // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); + gtsam::Vector vec() const; + gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); static gtsam::Vector Vee(const gtsam::Matrix& X); // Standard Interface bool equals(const gtsam::Similarity2& sim, double tol) const; void print(string s = "") const; - gtsam::Matrix matrix() const; gtsam::Rot2& rotation(); gtsam::Point2& translation(); double scale() const; @@ -1459,13 +1563,22 @@ class Similarity3 { static gtsam::Vector Logmap(const gtsam::Similarity3& s); gtsam::Similarity3 expmap(const gtsam::Vector& v); gtsam::Vector logmap(const gtsam::Similarity3& g); + + // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); + gtsam::Vector vec() const; + gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); static gtsam::Vector Vee(const gtsam::Matrix& X); // Standard Interface bool equals(const gtsam::Similarity3& sim, double tol) const; void print(string s = "") const; - gtsam::Matrix matrix() const; gtsam::Rot3& rotation(); gtsam::Point3& translation(); double scale() const; @@ -1529,6 +1642,12 @@ class Gal3 { gtsam::Vector10 logmap(const gtsam::Gal3& g); // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); diff --git a/gtsam/geometry/tests/testExtendedPose3.cpp b/gtsam/geometry/tests/testExtendedPose3.cpp new file mode 100644 index 0000000000..32bef7936e --- /dev/null +++ b/gtsam/geometry/tests/testExtendedPose3.cpp @@ -0,0 +1,318 @@ +/* ---------------------------------------------------------------------------- + + * 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 + */ + +#include +#include +#include +#include +#include +#include + +#include + +using namespace gtsam; + +using ExtendedPose33 = ExtendedPose3<3>; +using ExtendedPose3d = ExtendedPose3; + +GTSAM_CONCEPT_TESTABLE_INST(ExtendedPose33) +GTSAM_CONCEPT_MATRIX_LIE_GROUP_INST(ExtendedPose33) +GTSAM_CONCEPT_TESTABLE_INST(ExtendedPose3d) +GTSAM_CONCEPT_MATRIX_LIE_GROUP_INST(ExtendedPose3d) + +namespace { + +const Rot3 kR1 = Rot3::RzRyRx(0.1, -0.2, 0.3); +const Rot3 kR2 = Rot3::RzRyRx(-0.3, 0.4, -0.2); +const Matrix3 kX1 = + (Matrix3() << 1.0, 4.0, -1.0, 2.0, 5.0, 0.5, 3.0, 6.0, 2.0).finished(); +const Matrix3 kX2 = + (Matrix3() << -2.0, 1.0, 0.1, 0.3, -1.5, 2.2, 0.7, -0.9, 1.6).finished(); +const Vector12 kXi = (Vector12() << 0.11, -0.07, 0.05, 0.3, -0.4, 0.1, -0.2, + 0.6, -0.5, 0.7, -0.1, 0.2) + .finished(); + +} // namespace + +//****************************************************************************** +TEST(ExtendedPose3, Concept) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); + + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); +} + +//****************************************************************************** +TEST(ExtendedPose3, Dimensions) { + const ExtendedPose33 fixed; + EXPECT_LONGS_EQUAL(12, traits::GetDimension(fixed)); + EXPECT_LONGS_EQUAL(3, fixed.k()); + + const ExtendedPose3d placeholder; + EXPECT_LONGS_EQUAL(0, placeholder.k()); + EXPECT_LONGS_EQUAL(3, placeholder.dim()); + + const ExtendedPose3d dynamic(3); + EXPECT_LONGS_EQUAL(3, dynamic.k()); + EXPECT_LONGS_EQUAL(12, dynamic.dim()); +} + +//****************************************************************************** +TEST(ExtendedPose3, ConstructorsAndAccess) { + const ExtendedPose33 fixed(kR1, kX1); + const ExtendedPose3d dynamic(kR1, ExtendedPose3d::Matrix3K(kX1)); + + EXPECT(assert_equal(kR1, fixed.rotation())); + EXPECT(assert_equal(kR1, dynamic.rotation())); + + for (size_t i = 0; i < 3; ++i) { + EXPECT(assert_equal(Point3(kX1.col(static_cast(i))), + fixed.x(i))); + EXPECT(assert_equal(Point3(kX1.col(static_cast(i))), + dynamic.x(i))); + } + + EXPECT(assert_equal(fixed.matrix(), dynamic.matrix())); +} + +//****************************************************************************** +TEST(ExtendedPose3, GroupOperationsMatch) { + const ExtendedPose33 f1(kR1, kX1), f2(kR2, kX2); + const ExtendedPose3d d1(kR1, ExtendedPose3d::Matrix3K(kX1)); + const ExtendedPose3d d2(kR2, ExtendedPose3d::Matrix3K(kX2)); + + const auto f12 = f1 * f2; + const auto d12 = d1 * d2; + EXPECT(assert_equal(f12.matrix(), d12.matrix())); + + const auto f_id = f1 * f1.inverse(); + const auto d_id = d1 * d1.inverse(); + EXPECT( + assert_equal(ExtendedPose33::Identity().matrix(), f_id.matrix(), 1e-9)); + EXPECT( + assert_equal(ExtendedPose3d::Identity(3).matrix(), d_id.matrix(), 1e-9)); + + const auto f_between = f1.between(f2); + const auto d_between = d1.between(d2); + EXPECT(assert_equal(f_between.matrix(), d_between.matrix(), 1e-9)); +} + +//****************************************************************************** +TEST(ExtendedPose3, HatVee) { + const Vector xi = kXi; + const auto Xf = ExtendedPose33::Hat(kXi); + const auto Xd = ExtendedPose3d::Hat(xi); + + EXPECT(assert_equal(xi, Vector(ExtendedPose33::Vee(Xf)))); + EXPECT(assert_equal(xi, ExtendedPose3d::Vee(Xd))); + EXPECT(assert_equal(Xf, Xd)); +} + +//****************************************************************************** +TEST(ExtendedPose3, ExpmapLogmapRoundTrip) { + const Vector xi = kXi; + const ExtendedPose33 f = ExtendedPose33::Expmap(kXi); + const ExtendedPose3d d = ExtendedPose3d::Expmap(xi); + + EXPECT(assert_equal(f.matrix(), d.matrix(), 1e-9)); + EXPECT(assert_equal(xi, Vector(ExtendedPose33::Logmap(f)), 1e-9)); + EXPECT(assert_equal(xi, ExtendedPose3d::Logmap(d), 1e-9)); +} + +//****************************************************************************** +TEST(ExtendedPose3, AdjointConsistency) { + const ExtendedPose33 f(kR1, kX1); + const ExtendedPose3d d(kR1, ExtendedPose3d::Matrix3K(kX1)); + const Vector xi = kXi; + + const auto f_conj = f * ExtendedPose33::Expmap(kXi) * f.inverse(); + const auto d_conj = d * ExtendedPose3d::Expmap(xi) * d.inverse(); + + const auto f_adj = ExtendedPose33::Expmap(f.Adjoint(kXi)); + const auto d_adj = ExtendedPose3d::Expmap(d.Adjoint(xi)); + EXPECT(assert_equal(f_conj.matrix(), f_adj.matrix(), 1e-9)); + EXPECT(assert_equal(d_conj.matrix(), d_adj.matrix(), 1e-9)); + + const ExtendedPose33::Jacobian f_generic = + static_cast*>(&f) + ->AdjointMap(); + const Matrix d_generic = + static_cast*>(&d) + ->AdjointMap(); + EXPECT(assert_equal(Matrix(f_generic), Matrix(f.AdjointMap()), 1e-9)); + EXPECT(assert_equal(d_generic, d.AdjointMap(), 1e-9)); +} + +//****************************************************************************** +TEST(ExtendedPose3, AdjointTranspose) { + const ExtendedPose33 f(kR1, kX1); + const ExtendedPose3d d(kR1, ExtendedPose3d::Matrix3K(kX1)); + const Vector12 x = kXi; + const Vector x_dynamic = x; + + EXPECT(assert_equal(Vector(f.AdjointMap().transpose() * x), + Vector(f.AdjointTranspose(x)))); + EXPECT(assert_equal(d.AdjointMap().transpose() * x_dynamic, + d.AdjointTranspose(x_dynamic))); + + std::function + f_adjoint_transpose = [](const ExtendedPose33& g, const Vector12& v) { + return Vector12(g.AdjointTranspose(v)); + }; + Matrix Hf_state, Hf_x; + f.AdjointTranspose(x, Hf_state, Hf_x); + EXPECT(assert_equal(numericalDerivative21(f_adjoint_transpose, f, x), Hf_state, + 1e-8)); + EXPECT( + assert_equal(numericalDerivative22(f_adjoint_transpose, f, x), Hf_x)); + + std::function + d_adjoint_transpose = [](const ExtendedPose3d& g, const Vector& v) { + return g.AdjointTranspose(v); + }; + Matrix Hd_state, Hd_x; + d.AdjointTranspose(x_dynamic, Hd_state, Hd_x); + EXPECT(assert_equal( + numericalDerivative21( + d_adjoint_transpose, d, x_dynamic), + Hd_state, 1e-8)); + EXPECT(assert_equal( + numericalDerivative22( + d_adjoint_transpose, d, x_dynamic), + Hd_x)); +} + +//****************************************************************************** +TEST(ExtendedPose3, adjointTranspose) { + const Vector12 xi = kXi; + const Vector12 y = + (Vector12() << 0.03, -0.07, 0.02, 0.4, 0.1, -0.2, -0.3, 0.5, 0.9, 0.2, + -0.8, 0.6) + .finished(); + const Vector xi_dynamic = xi; + const Vector y_dynamic = y; + + EXPECT(assert_equal(Vector(ExtendedPose33::adjointMap(xi).transpose() * y), + Vector(ExtendedPose33::adjointTranspose(xi, y)))); + EXPECT(assert_equal(ExtendedPose3d::adjointMap(xi_dynamic).transpose() * + y_dynamic, + ExtendedPose3d::adjointTranspose(xi_dynamic, y_dynamic))); + + std::function + f_adjoint_transpose = [](const Vector12& x, const Vector12& v) { + return Vector12(ExtendedPose33::adjointTranspose(x, v)); + }; + Matrix Hf_xi, Hf_y; + EXPECT(assert_equal(Vector(ExtendedPose33::adjointTranspose(xi, y, Hf_xi, Hf_y)), + Vector(f_adjoint_transpose(xi, y)))); + EXPECT( + assert_equal(numericalDerivative21(f_adjoint_transpose, xi, y, 1e-5), Hf_xi, + 1e-5)); + EXPECT( + assert_equal(numericalDerivative22(f_adjoint_transpose, xi, y, 1e-5), Hf_y, + 1e-5)); + + std::function d_adjoint_transpose = + [](const Vector& x, const Vector& v) { + return ExtendedPose3d::adjointTranspose(x, v); + }; + Matrix Hd_xi, Hd_y; + EXPECT(assert_equal(ExtendedPose3d::adjointTranspose(xi_dynamic, y_dynamic, + Hd_xi, Hd_y), + d_adjoint_transpose(xi_dynamic, y_dynamic))); + EXPECT(assert_equal( + numericalDerivative21( + d_adjoint_transpose, xi_dynamic, y_dynamic, 1e-5), + Hd_xi, 1e-5)); + EXPECT(assert_equal( + numericalDerivative22( + d_adjoint_transpose, xi_dynamic, y_dynamic, 1e-5), + Hd_y, 1e-5)); +} + +//****************************************************************************** +TEST(ExtendedPose3, Derivatives) { + const ExtendedPose33 f(kR1, kX1); + const ExtendedPose3d d(kR1, ExtendedPose3d::Matrix3K(kX1)); + const Vector xi = kXi; + + Matrix Hf; + ExtendedPose33::Expmap(kXi, Hf); + const auto f_exp = [](const Vector12& v) { + return ExtendedPose33::Expmap(v); + }; + const Matrix Hf_num = + numericalDerivative11(f_exp, kXi); + EXPECT(assert_equal(Hf_num, Hf, 1e-6)); + + Matrix Hd; + ExtendedPose3d::Expmap(xi, Hd); + const auto d_exp = [](const Vector& v) { return ExtendedPose3d::Expmap(v); }; + const Matrix Hd_num = + numericalDerivative11(d_exp, xi); + EXPECT(assert_equal(Hd_num, Hd, 1e-6)); + + Matrix Lf; + ExtendedPose33::Logmap(f, Lf); + const auto f_log = [](const ExtendedPose33& g) { + return Vector12(ExtendedPose33::Logmap(g)); + }; + const Matrix Lf_num = + numericalDerivative11(f_log, f); + EXPECT(assert_equal(Lf_num, Lf, 1e-6)); + + Matrix Ld; + ExtendedPose3d::Logmap(d, Ld); + const auto d_log = [](const ExtendedPose3d& g) { + return ExtendedPose3d::Logmap(g); + }; + const Matrix Ld_num = + numericalDerivative11(d_log, d); + EXPECT(assert_equal(Ld_num, Ld, 1e-6)); +} + +//****************************************************************************** +TEST(ExtendedPose3, VecJacobian) { + const ExtendedPose33 f(kR1, kX1); + const ExtendedPose3d d(kR1, ExtendedPose3d::Matrix3K(kX1)); + + Matrix Hf; + const Vector vf = f.vec(Hf); + const auto fv = [](const ExtendedPose33& g) { return Vector(g.vec()); }; + const Matrix Hf_num = + numericalDerivative11(fv, f); + EXPECT(assert_equal(Hf_num, Hf, 1e-6)); + EXPECT_LONGS_EQUAL(36, vf.size()); + + Matrix Hd; + const Vector vd = d.vec(Hd); + const auto dv = [](const ExtendedPose3d& g) { return Vector(g.vec()); }; + const Matrix Hd_num = + numericalDerivative11(dv, d); + EXPECT(assert_equal(Hd_num, Hd, 1e-6)); + EXPECT_LONGS_EQUAL(36, vd.size()); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 00a0ef78ee..7282522527 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -1005,6 +1005,40 @@ TEST(Pose2, AdjointMap) { EXPECT(assert_equal(specialized_Adj, generic_Adj, 1e-9)); } +/* ************************************************************************* */ +TEST(Pose2, AdjointTranspose) { + const Pose2 pose(Rot2::fromAngle(0.5), Point2(1.0, 2.0)); + const Vector3 xi(0.2, -0.4, 0.7); + + EXPECT(assert_equal(Vector(pose.AdjointMap().transpose() * xi), + Vector(pose.AdjointTranspose(xi)))); + + Matrix33 actualH1, actualH2; + std::function proxy = + [](const Pose2& g, const Vector3& x) { + return Vector3(g.AdjointTranspose(x)); + }; + pose.AdjointTranspose(xi, actualH1, actualH2); + EXPECT(assert_equal(numericalDerivative21(proxy, pose, xi), actualH1, 1e-8)); + EXPECT(assert_equal(numericalDerivative22(proxy, pose, xi), actualH2)); +} + +/* ************************************************************************* */ +TEST(Pose2, adjointTranspose) { + const Vector3 xi(0.2, -0.4, 0.7); + const Vector3 y(-0.3, 0.5, 0.9); + + Matrix33 Hxi, Hy; + const Vector3 actual = Pose2::adjointTranspose(xi, y, Hxi, Hy); + std::function f = + [](const Vector3& x, const Vector3& v) { + return Pose2::adjointTranspose(x, v); + }; + EXPECT(assert_equal(f(xi, y), actual)); + EXPECT(assert_equal(numericalDerivative21(f, xi, y, 1e-5), Hxi, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 3797e77619..fc3a82f4dc 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1469,21 +1469,6 @@ TEST(Pose3, Vec) { EXPECT(assert_equal(numericalH, actualH, 1e-9)); } -/* ************************************************************************* */ -TEST(Pose3, AdjointMap) { - // Create a non-trivial Pose3 object - const Pose3 pose(Rot3::Rodrigues(0.1, 0.2, 0.3), Point3(1.0, 2.0, 3.0)); - - // Call the specialized AdjointMap - Matrix6 specialized_Adj = pose.AdjointMap(); - - // Call the generic AdjointMap from the base class - Matrix6 generic_Adj = static_cast*>(&pose)->AdjointMap(); - - // Assert that they are equal - EXPECT(assert_equal(specialized_Adj, generic_Adj, 1e-9)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp index 3ad23cac2d..483f64a8ad 100644 --- a/gtsam/geometry/tests/testSimilarity2.cpp +++ b/gtsam/geometry/tests/testSimilarity2.cpp @@ -240,6 +240,60 @@ TEST(Similarity2, AdjointMap) { EXPECT(assert_equal(specialized_Adj, generic_Adj, 1e-9)); } +//****************************************************************************** +TEST(Similarity2, AdjointTranspose) { + const Similarity2 sim(Rot2::fromAngle(-0.7), Point2(1.5, -2.3), 0.5); + const Vector4 xi(0.2, -0.4, 0.7, -0.1); + + EXPECT(assert_equal(Vector(sim.AdjointMap().transpose() * xi), + Vector(sim.AdjointTranspose(xi)))); + + Matrix44 actualH1, actualH2; + std::function + adjointTransposeProxy = [](const Similarity2& g, const Vector4& x) { + return Vector4(g.AdjointTranspose(x)); + }; + sim.AdjointTranspose(xi, actualH1, actualH2); + EXPECT(assert_equal(numericalDerivative21(adjointTransposeProxy, sim, xi), + actualH1, 1e-8)); + EXPECT(assert_equal(numericalDerivative22(adjointTransposeProxy, sim, xi), + actualH2)); +} + +//****************************************************************************** +TEST(Similarity2, adjointTranspose) { + const Vector4 xi(0.2, -0.4, 0.7, -0.1); + const Vector4 y(-0.3, 0.5, 0.9, -0.2); + + std::function f = + [](const Vector4& x, const Vector4& v) { + return Vector4(Similarity2::adjointTranspose(x, v)); + }; + + Matrix44 Hxi, Hy; + const Vector4 actual = Similarity2::adjointTranspose(xi, y, Hxi, Hy); + EXPECT(assert_equal(f(xi, y), actual)); + EXPECT(assert_equal(numericalDerivative21(f, xi, y, 1e-5), Hxi, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); +} + +//****************************************************************************** +TEST(Similarity2, adjoint) { + const Vector4 xi(0.2, -0.4, 0.7, -0.1); + const Vector4 y(-0.3, 0.5, 0.9, -0.2); + + std::function f = + [](const Vector4& x, const Vector4& v) { + return Vector4(Similarity2::adjoint(x, v)); + }; + + Matrix44 Hxi, Hy; + const Vector4 actual = Similarity2::adjoint(xi, y, Hxi, Hy); + EXPECT(assert_equal(f(xi, y), actual)); + EXPECT(assert_equal(numericalDerivative21(f, xi, y, 1e-5), Hxi, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 42998a86b6..9ab493ce49 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -653,10 +653,62 @@ TEST(Similarity3, AdjointMap) { EXPECT(assert_equal(specialized_Adj, generic_Adj, 1e-9)); } +//****************************************************************************** +TEST(Similarity3, AdjointTranspose) { + const Similarity3 sim(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), + 0.8); + const Vector7 xi(0.2, -0.4, 0.7, -0.1, 0.3, -0.6, 0.5); + + EXPECT(assert_equal(Vector(sim.AdjointMap().transpose() * xi), + Vector(sim.AdjointTranspose(xi)))); + + Matrix77 actualH1, actualH2; + std::function proxy = + [](const Similarity3& g, const Vector7& x) { + return Vector7(g.AdjointTranspose(x)); + }; + sim.AdjointTranspose(xi, actualH1, actualH2); + EXPECT(assert_equal(numericalDerivative21(proxy, sim, xi), actualH1, 1e-8)); + EXPECT(assert_equal(numericalDerivative22(proxy, sim, xi), actualH2)); +} + +//****************************************************************************** +TEST(Similarity3, adjointTranspose) { + const Vector7 xi(0.2, -0.4, 0.7, -0.1, 0.3, -0.6, 0.5); + const Vector7 y(-0.3, 0.5, 0.9, -0.2, 0.4, -0.8, 0.1); + + std::function f = + [](const Vector7& x, const Vector7& v) { + return Vector7(Similarity3::adjointTranspose(x, v)); + }; + + Matrix77 Hxi, Hy; + const Vector7 actual = Similarity3::adjointTranspose(xi, y, Hxi, Hy); + EXPECT(assert_equal(f(xi, y), actual)); + EXPECT(assert_equal(numericalDerivative21(f, xi, y, 1e-5), Hxi, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); +} + +//****************************************************************************** +TEST(Similarity3, adjoint) { + const Vector7 xi(0.2, -0.4, 0.7, -0.1, 0.3, -0.6, 0.5); + const Vector7 y(-0.3, 0.5, 0.9, -0.2, 0.4, -0.8, 0.1); + + std::function f = + [](const Vector7& x, const Vector7& v) { + return Vector7(Similarity3::adjoint(x, v)); + }; + + Matrix77 Hxi, Hy; + const Vector7 actual = Similarity3::adjoint(xi, y, Hxi, Hy); + EXPECT(assert_equal(f(xi, y), actual)); + EXPECT(assert_equal(numericalDerivative21(f, xi, y, 1e-5), Hxi, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, xi, y, 1e-5), Hy, 1e-5)); +} + //****************************************************************************** int main() { TestResult tr; return TestRegistry::runAllTests(tr); } //****************************************************************************** - diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 4abe029503..a93bfead45 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -46,29 +46,23 @@ NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel, //------------------------------------------------------------------------------ const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const { - if (H) - *H << I_3x3, Z_3x3, Z_3x3; - return R_; + return Base::rotation(H); } //------------------------------------------------------------------------------ -const Point3& NavState::position(OptionalJacobian<3, 9> H) const { - if (H) - *H << Z_3x3, R(), Z_3x3; - return t_; +Point3 NavState::position(OptionalJacobian<3, 9> H) const { + return Base::x(0, H); } //------------------------------------------------------------------------------ -const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const { - if (H) - *H << Z_3x3, Z_3x3, R(); - return v_; +Vector3 NavState::velocity(OptionalJacobian<3, 9> H) const { + return Base::x(1, H); } //------------------------------------------------------------------------------ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { const Rot3& nRb = R_; - const Vector3& n_v = v_; + const Vector3 n_v = t_.col(1); Matrix3 D_bv_nRb; Vector3 b_v = nRb.unrotate(n_v, H ? &D_bv_nRb : 0); if (H) @@ -79,7 +73,7 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { //------------------------------------------------------------------------------ double NavState::range(const Point3& point, OptionalJacobian<1, 9> Hself, OptionalJacobian<1, 3> Hpoint) const { - const Vector3 delta = point - t_; + const Vector3 delta = point - t_.col(0); const double r = delta.norm(); if (!Hself && !Hpoint) return r; @@ -109,35 +103,6 @@ Unit3 NavState::bearing(const Point3& point, OptionalJacobian<2, 9> Hself, return b; } -//------------------------------------------------------------------------------ -Matrix5 NavState::matrix() const { - Matrix3 R = this->R(); - - Matrix5 T = Matrix5::Identity(); - T.block<3, 3>(0, 0) = R; - T.block<3, 1>(0, 3) = t_; - T.block<3, 1>(0, 4) = v_; - return T; -} - -//------------------------------------------------------------------------------ -NavState::Vector25 NavState::vec(OptionalJacobian<25, 9> H) const { - const Matrix5 T = this->matrix(); - if (H) { - H->setZero(); - auto R = T.block<3, 3>(0, 0); - H->block<3, 1>(0, 1) = -R.col(2); - H->block<3, 1>(0, 2) = R.col(1); - H->block<3, 1>(5, 0) = R.col(2); - H->block<3, 1>(5, 2) = -R.col(0); - H->block<3, 1>(10, 0) = -R.col(1); - H->block<3, 1>(10, 1) = R.col(0); - H->block<3, 3>(15, 3) = R; - H->block<3, 3>(20, 6) = R; - } - return Eigen::Map(T.data()); -} - //------------------------------------------------------------------------------ std::ostream& operator<<(std::ostream& os, const NavState& state) { os << "R: " << state.attitude() << "\n"; @@ -153,218 +118,16 @@ void NavState::print(const std::string& s) const { //------------------------------------------------------------------------------ bool NavState::equals(const NavState& other, double tol) const { - return R_.equals(other.R_, tol) && traits::Equals(t_, other.t_, tol) - && equal_with_abs_tol(v_, other.v_, tol); -} - -//------------------------------------------------------------------------------ -NavState NavState::inverse() const { - Rot3 Rt = R_.inverse(); - return NavState(Rt, Rt * (-t_), Rt * -(v_)); -} - -//------------------------------------------------------------------------------ -// See [this document](doc/Jacobians.md) for details. -NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - // Get angular velocity w and components rho (for t) and nu (for v) from xi - Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>(); - - // Instantiate functor for Dexp-related operations: - const so3::DexpFunctor local(w); - - // Compute rotation using Expmap -#ifdef GTSAM_USE_QUATERNIONS - const Rot3 R = traits::Expmap(w); -#else - const Rot3 R(local.expmap()); -#endif - - // Compute translation and velocity. See Pose3::Expmap - Matrix3 H_t_w, H_v_w; - const Vector3 t = local.Jacobian().applyLeft(rho, Hxi ? &H_t_w : nullptr); - const Vector3 v = local.Jacobian().applyLeft(nu, Hxi ? &H_v_w : nullptr); - - if (Hxi) { - const Matrix3 Jr = local.Jacobian().right(); - // We are creating a NavState, so we still need to chain H_t_w and H_v_w - // with R^T, the Jacobian of Navstate::Create with respect to both t and v. - const Matrix3 Rt = R.transpose(); - *Hxi << Jr, Z_3x3, Z_3x3, // Jr here *is* the Jacobian of expmap - Rt * H_t_w, Jr, Z_3x3, // - Rt * H_v_w, Z_3x3, Jr; - // In the last two rows, Jr = R^T * Jl, see Barfoot eq. (8.83). - // Jl is the left Jacobian of SO(3) at w. - } - - return NavState(R, t, v); -} - -//------------------------------------------------------------------------------ -Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) { - if (Hstate) *Hstate = LogmapDerivative(state); - - const Vector3 phi = Rot3::Logmap(state.rotation()); - const Vector3& p = state.position(); - const Vector3& v = state.velocity(); - const double t = phi.norm(); - if (t < 1e-8) { - Vector9 log; - log << phi, p, v; - 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 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); - const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); - Vector9 log; - // Order is ω, p, v - log << phi, rho, nu; - return log; - } -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::AdjointMap() const { - const Matrix3 R = R_.matrix(); - Matrix3 A = skewSymmetric(t_) * R; - Matrix3 B = skewSymmetric(v_) * R; - // Eqn 2 in Barrau20icra - Matrix9 adj; - adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R; - return adj; -} - -//------------------------------------------------------------------------------ -Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state, - OptionalJacobian<9, 9> H_xib) const { - const Matrix9 Ad = AdjointMap(); - - // Jacobians - if (H_state) *H_state = -Ad * adjointMap(xi_b); - if (H_xib) *H_xib = Ad; - - return Ad * xi_b; -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::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 NavState::adjoint(const Vector9& xi, const Vector9& y, - OptionalJacobian<9, 9> Hxi, - OptionalJacobian<9, 9> H_y) { - 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; - } - } - - const Matrix9& ad_xi = adjointMap(xi); - if (H_y) *H_y = ad_xi; - - return ad_xi * y; -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { - Matrix9 J; - Expmap(xi, J); - return J; -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::LogmapDerivative(const Vector9& xi) { - const Vector3 w = xi.head<3>(); - Vector3 rho = xi.segment<3>(3); - Vector3 nu = xi.tail<3>(); - - // Instantiate functor for Dexp-related operations: - const so3::DexpFunctor local(w); - - // Call Jacobian().applyLeft to get its Jacobians - Matrix3 H_t_w, H_v_w; - local.Jacobian().applyLeft(rho, H_t_w); - local.Jacobian().applyLeft(nu, H_v_w); - - // Multiply with R^T to account for NavState::Create Jacobian. - const Matrix3 Rt = local.expmap().transpose(); - const Matrix3 Qt = Rt * H_t_w; - const Matrix3 Qv = Rt * H_v_w; - - // Now compute the blocks of the LogmapDerivative Jacobian - const Matrix3 Jw = Rot3::LogmapDerivative(w); - const Matrix3 Qt2 = -Jw * Qt * Jw; - const Matrix3 Qv2 = -Jw * Qv * Jw; - - Matrix9 J; - J << Jw, Z_3x3, Z_3x3, - Qt2, Jw, Z_3x3, - Qv2, Z_3x3, Jw; - return J; -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::LogmapDerivative(const NavState& state) { - const Vector9 xi = Logmap(state); - return LogmapDerivative(xi); -} - -//------------------------------------------------------------------------------ -Matrix5 NavState::Hat(const Vector9& xi) { - Matrix5 X; - const double wx = xi(0), wy = xi(1), wz = xi(2); - const double px = xi(3), py = xi(4), pz = xi(5); - const double vx = xi(6), vy = xi(7), vz = xi(8); - X << 0., -wz, wy, px, vx, - wz, 0., -wx, py, vy, - -wy, wx, 0., pz, vz, - 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0.; - return X; -} - -//------------------------------------------------------------------------------ -Vector9 NavState::Vee(const Matrix5& Xi) { - Vector9 xi; - xi << Xi(2, 1), Xi(0, 2), Xi(1, 0), - Xi(0, 3), Xi(1, 3), Xi(2, 3), - Xi(0, 4), Xi(1, 4), Xi(2, 4); - return xi; -} - -//------------------------------------------------------------------------------ -NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, - ChartJacobian Hxi) { - return Expmap(xi, Hxi); -} - -//------------------------------------------------------------------------------ -Vector9 NavState::ChartAtOrigin::Local(const NavState& state, - ChartJacobian Hstate) { - return Logmap(state, Hstate); + return Base::equals(other, tol); } //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + // NOTE: This is an intentional custom chart for NavState manifold updates. + // It differs from the default LieGroup chart based on full Expmap/Logmap. Rot3 nRb = R_; - Point3 n_t = t_, n_v = v_; + Point3 n_t = t_.col(0), n_v = t_.col(1); Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0); const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0); @@ -390,10 +153,11 @@ NavState NavState::retract(const Vector9& xi, // //------------------------------------------------------------------------------ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + // Inverse of the custom component-wise chart used in retract(). 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); + const Point3 dP = R_.unrotate(g.t_.col(0) - t_.col(0), H1 ? &D_dt_R : 0); + const Vector dV = R_.unrotate(g.t_.col(1) - t_.col(1), H1 ? &D_dv_R : 0); Vector9 xi; Matrix3 D_xi_R; @@ -479,7 +243,7 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { Rot3 nRb = R_; - Point3 n_t = t_, n_v = v_; + Point3 n_t = t_.col(0), n_v = t_.col(1); const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); @@ -531,7 +295,7 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt, bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { const Rot3& nRb = R_; - const Velocity3& n_v = v_; // derivative is Ri ! + const Velocity3 n_v = t_.col(1); // derivative is Ri ! const double dt22 = 0.5 * dt * dt; Vector9 xi; diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 8b2a2d9141..03f0eee3a1 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -19,10 +19,15 @@ #pragma once #include +#include #include #include #include +#if GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + namespace gtsam { /// Velocity is currently typedef'd to Vector3 @@ -36,46 +41,37 @@ using Velocity3 = Vector3; * NOTE: While Barrau20icra follow a R,v,t order, * we use a R,t,v order to maintain backwards compatibility. */ -class GTSAM_EXPORT NavState : public MatrixLieGroup { - private: - - // TODO(frank): - // - should we rename t_ to p_? if not, we should rename dP do dT - Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav - Point3 t_; ///< position n_t, in nav frame - Velocity3 v_; ///< velocity n_v in nav frame - +class GTSAM_EXPORT NavState : public ExtendedPose3<2, NavState> { public: + using Base = ExtendedPose3<2, NavState>; using LieAlgebra = Matrix5; using Vector25 = Eigen::Matrix; + inline constexpr static auto dimension = 9; /// @name Constructors /// @{ /// Default constructor - NavState() : - t_(0, 0, 0), v_(Vector3::Zero()) { - } + NavState() : Base() {} + + NavState(const Base& other) : Base(other) {} /// Construct from attitude, position, velocity - NavState(const Rot3& R, const Point3& t, const Velocity3& v) : - R_(R), t_(t), v_(v) { - } + NavState(const Rot3& R, const Point3& t, const Velocity3& v) + : Base(R, (Eigen::Matrix() << t.x(), v.x(), t.y(), v.y(), + t.z(), v.z()) + .finished()) {} /// Construct from pose and velocity - NavState(const Pose3& pose, const Velocity3& v) : - R_(pose.rotation()), t_(pose.translation()), v_(v) { - } + NavState(const Pose3& pose, const Velocity3& v) + : NavState(pose.rotation(), pose.translation(), v) {} /// Construct from SO(3) and R^6 - NavState(const Matrix3& R, const Vector6& tv) : - R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { - } + NavState(const Matrix3& R, const Vector6& tv) + : NavState(Rot3(R), tv.head<3>(), tv.tail<3>()) {} /// Construct from Matrix5 - NavState(const Matrix5& T) : - R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 3)), v_(T.block<3, 1>(0, 4)) { - } + NavState(const Matrix5& T) : Base(T) {} /// Named constructor with derivatives static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, @@ -93,8 +89,8 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup { /// @{ const Rot3& attitude(OptionalJacobian<3, 9> H = {}) const; - const Point3& position(OptionalJacobian<3, 9> H = {}) const; - const Velocity3& velocity(OptionalJacobian<3, 9> H = {}) const; + Point3 position(OptionalJacobian<3, 9> H = {}) const; + Velocity3 velocity(OptionalJacobian<3, 9> H = {}) const; const Pose3 pose() const { return Pose3(attitude(), position()); @@ -130,22 +126,15 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup { } /// Return position as Vector3 Vector3 t() const { - return t_; + return t_.col(0); } - /// Return velocity as Vector3. Computation-free. - const Vector3& v() const { - return v_; + /// Return velocity as Vector3. + Vector3 v() const { + return velocity(); } // Return velocity in body frame Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const; - /// Return matrix group representation, in MATLAB notation: - /// nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1] - Matrix5 matrix() const; - - /// Vectorize 5x5 matrix into a 25-dim vector. - Vector25 vec(OptionalJacobian<25, 9> H = {}) const; - /// @} /// @name Testable /// @{ @@ -164,21 +153,6 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup { /// @name Group /// @{ - /// identity for group operation - static NavState Identity() { - return NavState(); - } - - /// inverse transformation with derivatives - NavState inverse() const; - - using LieGroup::inverse; // version with derivative - - /// compose syntactic sugar - NavState operator*(const NavState& T) const { - return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_); - } - /// Syntactic sugar const Rot3& rotation() const { return attitude(); }; @@ -203,82 +177,29 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup { return v.segment<3>(6); } - /// retract with optional derivatives + /** + * Manifold retract used by optimization. + * This intentionally uses a component-wise chart (R via Expmap, and p/v via + * world-frame rotation of the tangent increments), not the default LieGroup + * chart based on full Expmap/Logmap. + */ NavState retract(const Vector9& v, // OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {}) const; - /// localCoordinates with optional derivatives + /** + * Inverse of the custom manifold chart used by retract. + * Kept consistent with retract for optimization; Lie expmap/logmap remain + * available separately for group operations. + */ Vector9 localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {}) const; /// @} - /// @name Lie Group + /// @name Lie Group (all Lie group operations are implemented in ExtendedPose3) /// @{ - /** - * Exponential map at identity - create a NavState from canonical coordinates - * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ - */ - static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {}); - - /** - * Log map at identity - return the canonical coordinates \f$ - * [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ of this NavState - */ - static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {}); - - /** - * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) - * frame to the world spatial frame. - */ - Matrix9 AdjointMap() const; - - /** - * Apply this NavState'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$ - * Note that H_xib = AdjointMap() - */ - Vector9 Adjoint(const Vector9& xi_b, - OptionalJacobian<9, 9> H_this = {}, - OptionalJacobian<9, 9> H_xib = {}) const; - - /** - * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 - * but for the NavState [ad(w,v)] = [w^, zero3; v^, w^] - */ - 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 = {}, - OptionalJacobian<9, 9> H_y = {}); - - /// Derivative of Expmap - static Matrix9 ExpmapDerivative(const Vector9& xi); - - /// Derivative of Logmap - static Matrix9 LogmapDerivative(const Vector9& xi); - - /// Derivative of Logmap, NavState version - static Matrix9 LogmapDerivative(const NavState& xi); - - // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP - struct GTSAM_EXPORT ChartAtOrigin { - static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {}); - static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); - }; - - /// Hat maps from tangent vector to Lie algebra - static Matrix5 Hat(const Vector9& xi); - - /// Vee maps from Lie algebra to tangent vector - static Vector9 Vee(const Matrix5& X); - /// @} /// @name Dynamics /// @{ @@ -328,9 +249,7 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(R_); - ar & BOOST_SERIALIZATION_NVP(t_); - ar & BOOST_SERIALIZATION_NVP(v_); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } #endif /// @} diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 665abb0842..3530d0e62c 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -45,8 +45,9 @@ class ConstantBias { class NavState { // Constructors NavState(); - NavState(const gtsam::Rot3& R, const gtsam::Point3& t, gtsam::Vector v); - NavState(const gtsam::Pose3& pose, gtsam::Vector v); + NavState(const gtsam::Rot3& R, const gtsam::Point3& t, + const gtsam::Vector3& v); + NavState(const gtsam::Pose3& pose, const gtsam::Vector3& v); // Testable void print(string s = "") const; @@ -55,8 +56,8 @@ class NavState { // Access gtsam::Rot3 attitude() const; gtsam::Point3 position() const; - gtsam::Vector velocity() const; - gtsam::Vector bodyVelocity() const; + gtsam::Vector3 velocity() const; + gtsam::Vector3 bodyVelocity() const; gtsam::Pose3 pose() const; // Standard Interface @@ -89,10 +90,14 @@ class NavState { gtsam::NavState expmap(gtsam::Vector v, Eigen::Ref H1, Eigen::Ref H2); gtsam::Vector logmap(const gtsam::NavState& p); gtsam::Vector logmap(const gtsam::NavState& p, Eigen::Ref H1, Eigen::Ref H2); - gtsam::Matrix AdjointMap() const; - gtsam::Vector Adjoint(gtsam::Vector xi_b) const; // Matrix Lie Group + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi_b) const; + gtsam::Vector AdjointTranspose(gtsam::Vector x) const; + static gtsam::Matrix adjointMap(gtsam::Vector xi); + static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y); + static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); gtsam::Vector vec() const; gtsam::Matrix matrix() const; static gtsam::Matrix Hat(const gtsam::Vector& xi); diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 0e19d83eff..3a6b06134e 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -256,7 +256,7 @@ TEST(NavState, Compose) { } /* ************************************************************************* */ -// Check compose and its push-forward, another case +// Check compose and its pushforward, another case TEST(NavState, Compose2) { const NavState& T1 = T; Matrix actual = (T1 * T2).matrix(); @@ -385,7 +385,7 @@ 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 + * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', traveling 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 * second order Coriolis corrections are as expected. @@ -553,6 +553,15 @@ Point3 expectedP(0.29552, 0.0446635, 1); NavState expected(expectedR, expectedV, expectedP); } // namespace screwNavState +/* ************************************************************************* */ +// Checks correct exponential map (Expmap) with brute force matrix exponential +TEST(NavState, Expmap_c_full) { + EXPECT(assert_equal(screwNavState::expected, + expm(screwNavState::xi), 1e-6)); + EXPECT(assert_equal(screwNavState::expected, + NavState::Expmap(screwNavState::xi), 1e-6)); +} + /* ************************************************************************* */ // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) TEST(NavState, Adjoint_full) { @@ -574,14 +583,42 @@ TEST(NavState, Adjoint_compose_full) { // To debug derivatives of compose, assert that // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 const NavState& 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(); + Vector9 x; + x << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8; NavState expected = T1 * NavState::Expmap(x) * T2; Vector y = T2.inverse().Adjoint(x); NavState actual = T1 * T2 * NavState::Expmap(y); EXPECT(assert_equal(expected, actual, 1e-6)); } +/* ************************************************************************* */ +TEST(NavState, ExpmapsGaloreFull) { + Vector xi; + NavState actual; + xi = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + actual = NavState::Expmap(xi); + EXPECT(assert_equal(expm(xi), actual, 1e-6)); + EXPECT(assert_equal(xi, NavState::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 = NavState::Expmap(txi); + EXPECT(assert_equal(expm(txi, 30), actual, 1e-6)); + Vector log = NavState::Logmap(actual); + EXPECT(assert_equal(actual, NavState::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 = NavState::Expmap(xi); + EXPECT(assert_equal(expm(xi, 10), actual, 1e-5)); + EXPECT(assert_equal(xi, NavState::Logmap(actual), 1e-9)); +} + /* ************************************************************************* */ TEST(NavState, HatAndVee) { // Create a few test vectors @@ -692,8 +729,8 @@ TEST(NavState, manifold_expmap) { TEST(NavState, 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(); + Vector9 d; + d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9; // exp(-d)=inverse(exp(d)) EXPECT(assert_equal(NavState::Expmap(-d), NavState::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -803,21 +840,6 @@ TEST(NavState, Vec) { EXPECT(assert_equal(numericalH, actualH, 1e-9)); } -/* ************************************************************************* */ -TEST(NavState, AdjointMap_GenericVsSpecialized) { - // Create a non-trivial NavState object - const NavState navState(Rot3::Rodrigues(0.1, 0.2, 0.3), Point3(1.0, 2.0, 3.0), Velocity3(0.4, 0.5, 0.6)); - - // Call the specialized AdjointMap - Matrix9 specialized_Adj = navState.AdjointMap(); - - // Call the generic AdjointMap from the base class - Matrix9 generic_Adj = static_cast*>(&navState)->AdjointMap(); - - // Assert that they are equal - EXPECT(assert_equal(specialized_Adj, generic_Adj, 1e-9)); -} - /* ************************************************************************* */ TEST(NavState, AutonomousFlow) { const double dt = 0.1; diff --git a/python/gtsam/tests/test_ExtendedPose3.py b/python/gtsam/tests/test_ExtendedPose3.py new file mode 100644 index 0000000000..a1f8523a44 --- /dev/null +++ b/python/gtsam/tests/test_ExtendedPose3.py @@ -0,0 +1,110 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +ExtendedPose3 wrapper unit tests. +""" + +import unittest + +import numpy as np + +import gtsam +from gtsam import Rot3 +from gtsam.utils.test_case import GtsamTestCase + + +class TestExtendedPose3(GtsamTestCase): + """Test wrapped ExtendedPose3 static instantiations.""" + + @staticmethod + def class_for_k(k: int): + return getattr(gtsam, f"ExtendedPose3{k}") + + def test_constructors_static_k(self): + """All requested static-K classes are wrapped and default-constructible.""" + for k in (2, 3, 4, 6): + cls = self.class_for_k(k) + pose = cls() + self.assertEqual(pose.k(), k) + self.gtsamAssertEquals(pose, cls.Identity(), 1e-12) + + def test_k6_lie_group_and_matrix_lie_group(self): + """Test selected MatrixLieGroup operations on K=6.""" + cls = self.class_for_k(6) + + xi = np.array([ + 0.11, -0.07, 0.05, + 0.30, -0.40, 0.10, + -0.20, 0.60, -0.50, + 0.70, -0.10, 0.20, + -0.40, 0.30, 0.80, + 0.50, -0.20, -0.30, + 0.10, 0.20, -0.60, + ]) + yi = np.array([ + -0.06, 0.04, 0.02, + 0.10, -0.15, 0.05, + -0.12, 0.18, 0.07, + 0.22, -0.09, 0.03, + -0.14, 0.11, 0.08, + 0.06, 0.13, -0.17, + -0.05, 0.09, 0.04, + ]) + + hat = cls.Hat(xi) + np.testing.assert_allclose(cls.Vee(hat), xi, atol=1e-9) + + p = cls.Expmap(xi) + q = cls.Expmap(yi) + np.testing.assert_allclose(cls.Logmap(p), xi, atol=1e-9) + + composed = p * q + np.testing.assert_allclose(composed.matrix(), p.matrix() @ q.matrix(), atol=1e-9) + self.gtsamAssertEquals(composed, p.compose(q), 1e-9) + self.gtsamAssertEquals(p.between(q), p.inverse().compose(q), 1e-9) + self.gtsamAssertEquals(p.inverse() * p, cls.Identity(), 1e-9) + + self.assertEqual(cls.Dim(), 21) + self.assertEqual(p.dim(), 21) + self.assertEqual(p.k(), 6) + self.assertEqual(p.vec().shape[0], 81) + + def test_k6_constructor_with_components(self): + """Construct K=6 from (Rot3, x) and access x(i)/xMatrix().""" + cls = self.class_for_k(6) + x = np.array([ + [1.0, 4.0, -1.0, 0.5, 2.1, -0.3], + [2.0, 5.0, 0.5, -1.2, 3.3, 0.8], + [3.0, 6.0, 2.0, 1.4, -2.7, 0.6], + ]) + pose = cls(Rot3(), x) + + np.testing.assert_allclose(pose.xMatrix(), x, atol=1e-12) + for i in range(6): + np.testing.assert_allclose(pose.x(i), x[:, i], atol=1e-12) + + @unittest.skipUnless(hasattr(gtsam.ExtendedPose36, "serialize"), "Serialization not enabled") + def test_serialization_k6(self): + """Serialization works when boost serialization is enabled.""" + cls = self.class_for_k(6) + expected = cls.Expmap(np.array([ + 0.01, -0.02, 0.03, + 0.04, -0.05, 0.06, + 0.07, -0.08, 0.09, + -0.10, 0.11, -0.12, + 0.13, -0.14, 0.15, + -0.16, 0.17, -0.18, + 0.19, -0.20, 0.21, + ])) + actual = cls() + serialized = expected.serialize() + actual.deserialize(serialized) + self.gtsamAssertEquals(expected, actual, 1e-10) + + +if __name__ == "__main__": + unittest.main()