diff --git a/gtsam/base/GroupAction.h b/gtsam/base/GroupAction.h index 348fdf89ac..cd0fe4750e 100644 --- a/gtsam/base/GroupAction.h +++ b/gtsam/base/GroupAction.h @@ -213,6 +213,15 @@ struct InducedVectorField : public Action { * OptionalJacobian Hg = {}) const; * (for Right action) * + * For ProductLieGroup semidirect products where M is a vector space (Eigen + * column vector), provide the static method below so ProductLieGroup can + * derive Expmap and Logmap automatically via the φ₁ kernel: + * + * static Eigen::Matrix generator(const TangentVector_G& u); + * + * where generator(u)·h = d/dt φ(expG(t·u), h)|_{t=0} is the infinitesimal + * generator of the representation (a DimM×DimM matrix, linear in u). + * * @tparam Derived The user's action functor. * @tparam G The group type. * @tparam M The manifold type. diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 42a0681459..cd69a5622c 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -18,164 +18,164 @@ #pragma once +#include + namespace gtsam { -template -ProductLieGroup ProductLieGroup::operator*( +// --------------------------------------------------------------------------- +// phi1FrechetBlock: compute φ₀(A), φ₁(A), and L_{φ₁}(A, B) from one block +// matrix exponential. +// +// Identity: +// exp([[0, I, 0], +// [0, A, B], +// [0, 0, A]]) +// = [[I, φ₁(A), L_{φ₁}(A, B)], +// [0, exp(A), L_exp(A, B) ], +// [0, 0, exp(A) ]] +// --------------------------------------------------------------------------- +template +typename ProductLieGroup::Phi1FrechetResult +ProductLieGroup::phi1FrechetBlock(const Jacobian2& A, + const Jacobian2& B) { + const int r = static_cast(A.rows()); + Eigen::MatrixXd M = Eigen::MatrixXd::Zero(3 * r, 3 * r); + M.block(0, r, r, r) = Eigen::MatrixXd::Identity(r, r); + M.block(r, r, r, r) = A; + M.block(r, 2 * r, r, r) = B; + M.block(2 * r, 2 * r, r, r) = A; + const Eigen::MatrixXd expM = M.exp(); + + Phi1FrechetResult result; + if constexpr (secondDynamic) { + result.phi0.resize(r, r); + result.phi1.resize(r, r); + result.Lphi1.resize(r, r); + } + result.phi1 = expM.block(0, r, r, r); + result.Lphi1 = expM.block(0, 2 * r, r, r); + result.phi0 = expM.block(r, r, r, r); + return result; +} + + + +template +ProductLieGroup ProductLieGroup::operator*( const ProductLieGroup& other) const { checkMatchingDimensions(other, "operator*"); - return ProductLieGroup(traits::Compose(this->first, other.first), - traits::Compose(this->second, other.second)); + if constexpr (isDirectProduct) { + // Direct product: (g₁,h₁)·(g₂,h₂) = (g₁g₂, h₁h₂) + return ProductLieGroup(traits::Compose(this->first, other.first), + traits::Compose(this->second, other.second)); + } else { + // Semidirect product: (g₁,h₁)·(g₂,h₂) = (g₁g₂, h₁·φ(g₁,h₂)). + // The LHS G-component acts on the RHS H-component before composing in H. + const H actedSecond = Action{}(this->first, other.second); + return ProductLieGroup(traits::Compose(this->first, other.first), + traits::Compose(this->second, actedSecond)); + } } -template -ProductLieGroup ProductLieGroup::inverse() const { - return ProductLieGroup(traits::Inverse(this->first), - traits::Inverse(this->second)); +template +ProductLieGroup ProductLieGroup::inverse() const { + const G gInv = traits::Inverse(this->first); + const H hInv = traits::Inverse(this->second); + if constexpr (isDirectProduct) { + // Direct product: (g,h)⁻¹ = (g⁻¹, h⁻¹) + return ProductLieGroup(gInv, hInv); + } else { + // Semidirect product: (g,h)⁻¹ = (g⁻¹, φ(g⁻¹, h⁻¹)). + // g⁻¹ must also un-rotate h⁻¹ to undo the action baked into the group law. + return ProductLieGroup(gInv, Action{}(gInv, hInv)); + } } -template -ProductLieGroup ProductLieGroup::retract(const TangentVector& v, - ChartJacobian H1, - ChartJacobian H2) const { - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - if (static_cast(v.size()) != - productDimension) { +template +ProductLieGroup ProductLieGroup::retract( + const TangentVector& v, ChartJacobian H1, ChartJacobian H2) const { + const size_t d1 = firstDim(); + const size_t d2 = secondDim(); + const size_t d = combinedDimension(d1, d2); + if (static_cast(v.size()) != d) { throw std::invalid_argument( "ProductLieGroup::retract tangent dimension does not match product " "dimension"); } - Jacobian1 D_g_first; - Jacobian1 D_g_second; - Jacobian2 D_h_first; - Jacobian2 D_h_second; - G g = traits::Retract(this->first, tangentSegment(v, 0, firstDimension), - H1 ? &D_g_first : nullptr, - H2 ? &D_g_second : nullptr); - H h = traits::Retract( - this->second, tangentSegment(v, firstDimension, secondDimension), - H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_first; - } + // Expmap-based retract works for both direct and semidirect products: + // retract(p, v) = p · Expmap(v) + // The chart Jacobians are H1 = Ad(Expmap(v)⁻¹) and H2 = D_Expmap(v). + // For a direct product these are block-diagonal and match the componentwise + // formula; for a semidirect product they carry the full coupled structure. + // + // We pre-split v here so the 2-arg Expmap overload is used, which handles + // the dynamic-dynamic case correctly (the 1-arg form can't infer the split + // without an instance to query d1 and d2 from). + Matrix D_v1, D_v2; + using DynJ = OptionalJacobian; + const ProductLieGroup delta = + Expmap(tangentSegment(v, 0, d1), tangentSegment(v, d1, d2), + H2 ? DynJ(D_v1) : DynJ(), H2 ? DynJ(D_v2) : DynJ()); + const ProductLieGroup result = compose(delta); + if (H1) *H1 = delta.inverse().AdjointMap(); if (H2) { - *H2 = zeroJacobian(productDimension); - H2->block(0, 0, firstDimension, firstDimension) = D_g_second; - H2->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; + *H2 = zeroJacobian(d); + H2->leftCols(d1) = D_v1; + H2->rightCols(d2) = D_v2; } - return ProductLieGroup(g, h); + return result; } -template -typename ProductLieGroup::TangentVector -ProductLieGroup::localCoordinates(const ProductLieGroup& g, - ChartJacobian H1, - ChartJacobian H2) const { +template +typename ProductLieGroup::TangentVector +ProductLieGroup::localCoordinates(const ProductLieGroup& g, + ChartJacobian H1, + ChartJacobian H2) const { checkMatchingDimensions(g, "localCoordinates"); - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian1 D_g_second; - Jacobian2 D_h_first; - Jacobian2 D_h_second; - typename traits::TangentVector v1 = - traits::Local(this->first, g.first, H1 ? &D_g_first : nullptr, - H2 ? &D_g_second : nullptr); - typename traits::TangentVector v2 = traits::Local( - this->second, g.second, H1 ? &D_h_first : nullptr, - H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_first; - } - if (H2) { - *H2 = zeroJacobian(productDimension); - H2->block(0, 0, firstDimension, firstDimension) = D_g_second; - H2->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - return makeTangentVector(v1, v2, firstDimension, secondDimension); + // Logmap-based local coordinates work for both direct and semidirect + // products: + // localCoordinates(p, q) = Logmap(p⁻¹·q) + // For a direct product Logmap is componentwise; for a semidirect product it + // carries the full coupled Lie algebra structure from Action::Logmap. + const ProductLieGroup relative = between(g); + Jacobian D_logmap; + TangentVector v = + Logmap(relative, H1 || H2 ? ChartJacobian(&D_logmap) : ChartJacobian()); + if (H1) *H1 = -D_logmap * relative.inverse().AdjointMap(); + if (H2) *H2 = D_logmap; + return v; } -template -ProductLieGroup ProductLieGroup::compose( +template +ProductLieGroup ProductLieGroup::compose( const ProductLieGroup& other, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(other, "compose"); - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Compose(this->first, other.first, H1 ? &D_g_first : nullptr); - H h = traits::Compose(this->second, other.second, - H1 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - if (H2) *H2 = identityJacobian(productDimension); - return ProductLieGroup(g, h); -} - -template -ProductLieGroup ProductLieGroup::between( + const ProductLieGroup result = (*this) * other; + if (H1) *H1 = other.inverse().AdjointMap(); + if (H2) *H2 = identityJacobian(dim()); + return result; +} + +template +ProductLieGroup ProductLieGroup::between( const ProductLieGroup& other, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(other, "between"); - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Between(this->first, other.first, H1 ? &D_g_first : nullptr); - H h = traits::Between(this->second, other.second, - H1 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - if (H2) *H2 = identityJacobian(productDimension); - return ProductLieGroup(g, h); -} - -template -ProductLieGroup ProductLieGroup::inverse(ChartJacobian D) const { - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Inverse(this->first, D ? &D_g_first : nullptr); - H h = traits::Inverse(this->second, D ? &D_h_second : nullptr); - if (D) { - *D = zeroJacobian(productDimension); - D->block(0, 0, firstDimension, firstDimension) = D_g_first; - D->block(firstDimension, firstDimension, secondDimension, secondDimension) = - D_h_second; - } - return ProductLieGroup(g, h); + const ProductLieGroup result = this->inverse() * other; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = identityJacobian(dim()); + return result; } -template -ProductLieGroup ProductLieGroup::Expmap(const TangentVector& v, - ChartJacobian Hv) { +template +ProductLieGroup ProductLieGroup::inverse( + ChartJacobian D) const { + if (D) *D = -AdjointMap(); + return inverse(); +} + +template +ProductLieGroup ProductLieGroup::Expmap( + const TangentVector& v, ChartJacobian Hv) { size_t firstDimension = 0; size_t secondDimension = 0; if constexpr (firstDynamic && secondDynamic) { @@ -212,6 +212,7 @@ ProductLieGroup ProductLieGroup::Expmap(const TangentVector& v, "ProductLieGroup::Expmap tangent dimension does not match product " "dimension"); } + Matrix D_g_first; Matrix D_h_second; const auto v1 = tangentSegment(v, 0, firstDimension); @@ -233,76 +234,248 @@ ProductLieGroup ProductLieGroup::Expmap(const TangentVector& v, return result; } -template -ProductLieGroup ProductLieGroup::Expmap( +template +ProductLieGroup ProductLieGroup::Expmap( const Eigen::Ref::TangentVector>& v1, const Eigen::Ref::TangentVector>& v2, OptionalJacobian H1, OptionalJacobian H2) { - const size_t firstDimension = static_cast(v1.size()); - const size_t secondDimension = static_cast(v2.size()); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Expmap(v1, H1 ? &D_g_first : nullptr); - H h = traits::Expmap(v2, H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = Matrix::Zero(productDimension, firstDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + if constexpr (isDirectProduct) { + // Direct product: the two Lie algebras are independent, so the exponential + // is componentwise: exp(v₁,v₂) = (exp_G(v₁), exp_H(v₂)). + // The Jacobian columns split cleanly between the G and H blocks. + const size_t d1 = static_cast(v1.size()); + const size_t d2 = static_cast(v2.size()); + const size_t d = combinedDimension(d1, d2); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Expmap(v1, H1 ? &D_g_first : nullptr); + H h = traits::Expmap(v2, H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = Matrix::Zero(d, d1); + H1->block(0, 0, d1, d1) = D_g_first; + } + if (H2) { + *H2 = Matrix::Zero(d, d2); + H2->block(d1, 0, d2, d2) = D_h_second; + } + return ProductLieGroup(g, h); + } else if constexpr (hasGenerator) { + // Generic semidirect Expmap for vector-space H via the φ₁ kernel: + // Expmap(u, v) = (expG(u), φ₁(Aφ(u)) · v) + // where Aφ(u) = Action::generator(u) is the infinitesimal generator. + const size_t d1 = static_cast(v1.size()); + const size_t d2 = static_cast(v2.size()); + const size_t d = combinedDimension(d1, d2); + + const Jacobian2 A = Action::generator(v1); + Jacobian2 zeroB = A; + zeroB.setZero(); + const auto kernels = phi1FrechetBlock(A, zeroB); + + Jacobian1 D_G; + const G g = traits::Expmap(v1, H1 ? &D_G : nullptr); + const H h = kernels.phi1 * v2; + + if (H1 || H2) { + const auto phi0Solver = kernels.phi0.lu(); + + if (H1) { + // Top rows: D Exp_G(u) in the output chart (from traits::Expmap). + // Bottom rows: D(φ₁(Aφ(u))·v) pulled back by φ₀(A)⁻¹, because + // Expmap Jacobians are expressed in local coordinates at Expmap(u,v). + // Here dφ₁(A) is evaluated analytically via its Fréchet derivative. + *H1 = Matrix::Zero(d, d1); + H1->topRows(d1) = D_G; + typename traits::TangentVector ej; + if constexpr (firstDynamic) ej.resize(static_cast(d1)); + ej.setZero(); + for (Eigen::Index j = 0; j < static_cast(d1); ++j) { + ej(j) = 1.0; + const Jacobian2 Bj = Action::generator(ej); + const typename traits::TangentVector dh = + phi1FrechetBlock(A, Bj).Lphi1 * v2; + H1->col(j).tail(d2) = phi0Solver.solve(dh); + ej(j) = 0.0; + } + } + if (H2) { + // ∂(φ₁(A)·v)/∂v = φ₁(A) in coordinates, then pulled back by φ₀(A)⁻¹ + // to match the output chart. For SO(3) this is Rᵀ J_l = J_r. + *H2 = Matrix::Zero(d, d2); + H2->bottomRows(d2) = phi0Solver.solve(kernels.phi1); + } + } + return ProductLieGroup(g, h); + } else { + static_assert(hasGenerator, + "ProductLieGroup semidirect Expmap requires H to be a " + "fixed-size Eigen column vector and Action::generator(u)."); } - if (H2) { - *H2 = Matrix::Zero(productDimension, secondDimension); - H2->block(firstDimension, 0, secondDimension, secondDimension) = D_h_second; - } - return ProductLieGroup(g, h); -} - -template -typename ProductLieGroup::TangentVector ProductLieGroup::Logmap( - const ProductLieGroup& p, ChartJacobian Hp) { - const size_t firstDimension = p.firstDim(); - const size_t secondDimension = p.secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - typename traits::TangentVector v1 = - traits::Logmap(p.first, Hp ? &D_g_first : nullptr); - typename traits::TangentVector v2 = - traits::Logmap(p.second, Hp ? &D_h_second : nullptr); - TangentVector v = makeTangentVector(v1, v2, firstDimension, secondDimension); - if (Hp) { - *Hp = zeroJacobian(productDimension); - Hp->block(0, 0, firstDimension, firstDimension) = D_g_first; - Hp->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; +} + +template +typename ProductLieGroup::TangentVector +ProductLieGroup::Logmap(const ProductLieGroup& p, + ChartJacobian Hp) { + if constexpr (isDirectProduct) { + // Direct product: the inverse of a componentwise Expmap is componentwise, + // so log(g,h) = (log_G(g), log_H(h)) with a block-diagonal Jacobian. + const size_t firstDimension = p.firstDim(); + const size_t secondDimension = p.secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + typename traits::TangentVector v1 = + traits::Logmap(p.first, Hp ? &D_g_first : nullptr); + typename traits::TangentVector v2 = + traits::Logmap(p.second, Hp ? &D_h_second : nullptr); + TangentVector v = + makeTangentVector(v1, v2, firstDimension, secondDimension); + if (Hp) { + *Hp = zeroJacobian(productDimension); + Hp->block(0, 0, firstDimension, firstDimension) = D_g_first; + Hp->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + return v; + } else if constexpr (hasGenerator) { + // Generic semidirect Logmap for vector-space H via the φ₁ kernel: + // Logmap(g, h) = (logG(g), φ₁(Aφ(logG(g)))⁻¹ · h) + // This is the exact inverse of the Expmap formula above. + const size_t d1 = p.firstDim(); + const size_t d2 = p.secondDim(); + const size_t d = combinedDimension(d1, d2); + + Jacobian1 D_G; + const auto v1 = traits::Logmap(p.first, Hp ? &D_G : nullptr); + const Jacobian2 A = Action::generator(v1); + Jacobian2 zeroB = A; + zeroB.setZero(); + const auto kernels = phi1FrechetBlock(A, zeroB); + const auto phi1Solver = kernels.phi1.lu(); + const typename traits::TangentVector v2 = phi1Solver.solve(p.second); + TangentVector v = makeTangentVector(v1, v2, d1, d2); + + if (Hp) { + *Hp = zeroJacobian(d); + // Top-left: ∂logG(g)/∂g — analytic. + Hp->topLeftCorner(d1, d1) = D_G; + // Top-right: 0 — logG doesn't depend on h (already zeroed). + // Bottom-right: ∂v₂/∂(δh) = φ₁(A)⁻¹ · φ₀(A). + // Perturbing h by δh in the semidirect frame moves h by φ₀(A)·δh + // (because Expmap(0, δh) = (I, δh) and (g,h)*(I,δh) → h + φ₀(A)·δh), + // so ∂v₂/∂(δh) = φ₁⁻¹ · φ₀. + Hp->bottomRightCorner(d2, d2) = phi1Solver.solve(kernels.phi0); + // Bottom-left: ∂v₂/∂(δg) follows from dφ₁(A) = L_{φ₁}(A, dA) + // with dA = generator(du) and du = D Log_G(g) · δg. + for (Eigen::Index j = 0; j < static_cast(d1); ++j) { + const typename traits::TangentVector du = D_G.col(j); + const Jacobian2 Bj = Action::generator(du); + Hp->col(j).tail(d2) = + -phi1Solver.solve(phi1FrechetBlock(A, Bj).Lphi1 * v2); + } + } + return v; + } else { + static_assert(hasGenerator, + "ProductLieGroup semidirect Logmap requires H to be a " + "fixed-size Eigen column vector and Action::generator(u)."); } - return v; } -template -ProductLieGroup ProductLieGroup::expmap( +template +ProductLieGroup ProductLieGroup::expmap( const TangentVector& v) const { return compose(ProductLieGroup::Expmap(v)); } -template -typename ProductLieGroup::Jacobian ProductLieGroup::AdjointMap() - const { - const auto adjG = traits::AdjointMap(this->first); - const auto adjH = traits::AdjointMap(this->second); - const size_t d1 = static_cast(adjG.rows()); - const size_t d2 = static_cast(adjH.rows()); - Jacobian adj = zeroJacobian(d1 + d2); - adj.block(0, 0, d1, d1) = adjG; - adj.block(d1, d1, d2, d2) = adjH; - return adj; -} - -template +template +typename ProductLieGroup::Jacobian +ProductLieGroup::AdjointMap() const { + if constexpr (isDirectProduct) { + // Direct product: the two algebras are independent, so the adjoint is + // block-diagonal: Ad_(g,h) = diag(Ad_G(g), Ad_H(h)). + const auto adjG = traits::AdjointMap(this->first); + const auto adjH = traits::AdjointMap(this->second); + const size_t d1 = static_cast(adjG.rows()); + const size_t d2 = static_cast(adjH.rows()); + Jacobian adj = zeroJacobian(d1 + d2); + adj.block(0, 0, d1, d1) = adjG; + adj.block(d1, d1, d2, d2) = adjH; + return adj; + } else { + // Semidirect product: the action couples the algebra blocks, producing an + // off-diagonal term. The formula is assembled from the action's Jacobians: + // + // Ad_(g,h) = [[ Ad_G(g), 0 ], + // [ -Jg * Ad_G(g), Jh ]] + // + // where Jg = D_g φ(e_G, h) (action Jacobian w.r.t. G at identity) + // Jh = D_h φ(g, e_H) (action Jacobian w.r.t. H at identity) + // + // Both come directly from the action's operator() Jacobian arguments. + const size_t d1 = firstDim(); + const size_t d2 = secondDim(); + const size_t d = combinedDimension(d1, d2); + + const Action action{}; + + // Jg: DimH × DimG — evaluated at (e_G, h) + ActionJacobianG Jg; + action(defaultIdentity(), this->second, &Jg, {}); + + // Jh: DimH × DimH — evaluated at (g, e_H) + Jacobian2 Jh; + action(this->first, defaultIdentity(), {}, &Jh); + + const auto adjG = traits::AdjointMap(this->first); + + Jacobian adj = zeroJacobian(d); + adj.block(0, 0, d1, d1) = adjG; + adj.block(d1, d1, d2, d2) = Jh; + adj.block(d1, 0, d2, d1) = -Jg * adjG; + return adj; + } +} + +template +typename ProductLieGroup::Jacobian +ProductLieGroup::adjointMap(const TangentVector& xi) { + static_assert(hasGenerator, + "ProductLieGroup::adjointMap (algebra ad) is only defined for " + "semidirect products with Action::generator."); + // Semidirect product adjoint: + // + // ad_(a,b) = [[ ad^G_a, 0 ], + // [ -M(b), generator(a) ]] + // + // where M(b)·c = generator(c)·b. + constexpr int d1 = dimension1; + constexpr int d2 = dimension2; + + const auto a = xi.template head(); + const auto b = xi.template tail(); + + // Bottom-left -M(b): column j = -generator(e_j)·b. + Eigen::Matrix negM; + for (int i = 0; i < d1; ++i) { + const typename traits::TangentVector ei = + traits::TangentVector::Unit(i); + negM.col(i) = -(Action::generator(ei) * b); + } + + Jacobian ad = Jacobian::Zero(); + ad.template topLeftCorner() = G::adjointMap(a); + ad.template bottomRightCorner() = Action::generator(a); + ad.template bottomLeftCorner() = negM; + return ad; +} + +template template -T ProductLieGroup::defaultIdentity() { +T ProductLieGroup::defaultIdentity() { if constexpr (traits::dimension == Eigen::Dynamic) { return T(); } else { @@ -310,9 +483,9 @@ T ProductLieGroup::defaultIdentity() { } } -template +template template -typename traits::TangentVector ProductLieGroup::tangentSegment( +typename traits::TangentVector ProductLieGroup::tangentSegment( const TangentVector& v, size_t start, size_t runtimeDimension) { const int startIndex = static_cast(start); const int runtimeIndex = static_cast(runtimeDimension); @@ -324,9 +497,9 @@ typename traits::TangentVector ProductLieGroup::tangentSegment( } } -template -typename ProductLieGroup::TangentVector -ProductLieGroup::makeTangentVector( +template +typename ProductLieGroup::TangentVector +ProductLieGroup::makeTangentVector( const typename traits::TangentVector& v1, const typename traits::TangentVector& v2, size_t firstDimension, size_t secondDimension) { @@ -346,9 +519,9 @@ ProductLieGroup::makeTangentVector( } } -template -typename ProductLieGroup::Jacobian ProductLieGroup::zeroJacobian( - size_t productDimension) { +template +typename ProductLieGroup::Jacobian +ProductLieGroup::zeroJacobian(size_t productDimension) { if constexpr (dimension == Eigen::Dynamic) { return Jacobian::Zero(productDimension, productDimension); } else { @@ -357,9 +530,9 @@ typename ProductLieGroup::Jacobian ProductLieGroup::zeroJacobian( } } -template -typename ProductLieGroup::Jacobian -ProductLieGroup::identityJacobian(size_t productDimension) { +template +typename ProductLieGroup::Jacobian +ProductLieGroup::identityJacobian(size_t productDimension) { if constexpr (dimension == Eigen::Dynamic) { return Jacobian::Identity(productDimension, productDimension); } else { @@ -368,8 +541,8 @@ ProductLieGroup::identityJacobian(size_t productDimension) { } } -template -void ProductLieGroup::checkMatchingDimensions( +template +void ProductLieGroup::checkMatchingDimensions( const ProductLieGroup& other, const char* operation) const { if (firstDim() != other.firstDim() || secondDim() != other.secondDim()) { throw std::invalid_argument(std::string("ProductLieGroup::") + operation + @@ -377,8 +550,8 @@ void ProductLieGroup::checkMatchingDimensions( } } -template -void ProductLieGroup::print(const std::string& s) const { +template +void ProductLieGroup::print(const std::string& s) const { std::cout << s << "ProductLieGroup" << std::endl; traits::Print(this->first, " first"); traits::Print(this->second, " second"); diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 297652204a..3688ebb1c1 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -26,16 +27,55 @@ #include #include #include +#include #include // pair #include namespace gtsam { +/// Detects whether Action has a static generator() method. +/// Used to enable the generic semidirect Expmap/Logmap via the φ₁ kernel. +template +struct HasGenerator : std::false_type {}; +template +struct HasGenerator()))>> + : std::true_type {}; + +/// Detects vector-space Lie groups (Eigen column vectors, group law = addition). +template +struct IsVectorGroup : std::false_type {}; +template +struct IsVectorGroup> : std::true_type {}; + /** - * @brief Template to construct the product Lie group of two other Lie groups - * Assumes Lie group structure for G and H + * @brief Product Lie group of G and H, optionally with a semidirect structure. + * + * When Action is omitted (default void), this is the direct product G × H: + * operations are componentwise and tangent vectors concatenate independently. + * + * When Action is provided, this is the left semidirect product G ⋉ H, where G + * acts on H via the left group action φ: G × H → H. The group law becomes: + * + * (g₁, h₁) · (g₂, h₂) = (g₁g₂, h₁ · φ(g₁, h₂)) + * (g, h)⁻¹ = (g⁻¹, φ(g⁻¹, h⁻¹)) + * + * Action must derive from GroupAction and implement: + * - operator()(g, h, Hg={}, Hh={}): the group action with optional + * Jacobians w.r.t. g (DimH×DimG) and h (DimH×DimH). These Jacobians are + * used to automatically derive the AdjointMap. + * + * Semidirect products require H to be a fixed-size vector-space Lie group + * (Eigen column vector) and the action to supply the infinitesimal generator: + * - static Jacobian_H generator(const TangentVector_G& u): returns the + * DimH×DimH matrix Aφ(u) = d/dt φ(expG(t·u), ·)|_{t=0} (linear in u). + * Expmap and Logmap are derived automatically via φ₁(Aφ(u)). + * + * Example: SE(3) as a semidirect product: + * using SE3 = ProductLieGroup; + * where Rot3VectorAction implements φ(R, t) = R·t. */ -template +template class ProductLieGroup : public std::pair { GTSAM_CONCEPT_ASSERT(IsLieGroup); GTSAM_CONCEPT_ASSERT(IsLieGroup); @@ -52,6 +92,14 @@ class ProductLieGroup : public std::pair { inline constexpr static int dimension2 = traits::dimension; inline constexpr static bool firstDynamic = dimension1 == Eigen::Dynamic; inline constexpr static bool secondDynamic = dimension2 == Eigen::Dynamic; + inline constexpr static bool isDirectProduct = std::is_void_v; + /// True when the semidirect product has the required generator support. + inline constexpr static bool hasGenerator = + !isDirectProduct && IsVectorGroup::value && !secondDynamic && + HasGenerator::TangentVector>::value; + static_assert(isDirectProduct || hasGenerator, + "ProductLieGroup semidirect products require H to be a " + "fixed-size Eigen column vector and Action::generator(u)."); public: /// Manifold dimension @@ -74,6 +122,11 @@ class ProductLieGroup : public std::pair { Eigen::Matrix>; using Jacobian1 = typename traits::Jacobian; using Jacobian2 = typename traits::Jacobian; + /// Jacobian of the action w.r.t. G (DimH × DimG), used in the generic + /// semidirect AdjointMap formula. + using ActionJacobianG = + std::conditional_t>; public: /// @name Standard Constructors @@ -180,6 +233,9 @@ class ProductLieGroup : public std::pair { /// Adjoint map Jacobian AdjointMap() const; + /// Static algebra adjoint ad_xi (semidirect products only). + static Jacobian adjointMap(const TangentVector& xi); + /// @} protected: @@ -214,6 +270,18 @@ class ProductLieGroup : public std::pair { void checkMatchingDimensions(const ProductLieGroup& other, const char* operation) const; + /// Result of the analytic Fréchet derivative helper for φ₁. + struct Phi1FrechetResult { + Jacobian2 phi0; ///< exp(A) + Jacobian2 phi1; ///< φ₁(A) + Jacobian2 Lphi1; ///< Fréchet derivative L_{φ₁}(A, B) + }; + + /// Compute exp(A), φ₁(A), and the Fréchet derivative L_{φ₁}(A, B) from a + /// single block exponential. Only valid/called when hasGenerator is true. + static Phi1FrechetResult phi1FrechetBlock(const Jacobian2& A, + const Jacobian2& B); + public: /// @name Testable interface /// @{ @@ -523,9 +591,9 @@ class PowerLieGroup }; /// Traits specialization for ProductLieGroup -template -struct traits> - : internal::LieGroup> {}; +template +struct traits> + : internal::LieGroup> {}; /// Traits specialization for PowerLieGroup template diff --git a/gtsam/base/TangentLieGroup.h b/gtsam/base/TangentLieGroup.h new file mode 100644 index 0000000000..75a3c0b2b1 --- /dev/null +++ b/gtsam/base/TangentLieGroup.h @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + * 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 TangentLieGroup.h + * @date June, 2026 + * @author Alessandro Fornasier + * @brief Tangent group TG = G ⋉ 𝔤: the semidirect product of a Lie group G with + * its own Lie algebra under the adjoint action φ(g,ξ)=Ad_g·ξ. + * This class builds on the semidirect ProductLieGroup. + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * @brief Adjoint action of a Lie group G on its Lie algebra 𝔤 ≅ ℝⁿ. + * + * φ(g, ξ) = Ad_g · ξ , + * + * where generator(u) = ad_u = G::adjointMap(u). + * + * Requires G to expose a static adjointMap. + */ +template +struct AdjointAction : public GroupAction, G, + typename traits::TangentVector> { + static constexpr ActionType type = ActionType::Left; + using TangentVector = typename traits::TangentVector; + static constexpr int n = traits::dimension; + + TangentVector operator()(const G& g, const TangentVector& xi, + OptionalJacobian Hg = {}, + OptionalJacobian Hxi = {}) const { + const typename traits::Jacobian Ad = traits::AdjointMap(g); + if (Hxi) *Hxi = Ad; // ∂(Ad_g ξ)/∂ξ = Ad_g + if (Hg) *Hg = -(Ad * G::adjointMap(xi)); // ∂/∂g = -Ad_g · ad_ξ + return Ad * xi; + } + + /// Infinitesimal generator Aφ(u) = ad_u. + static Eigen::Matrix generator(const TangentVector& u) { + return G::adjointMap(u); + } +}; + +/** + * @brief Tangent Lie group TG = G ⋉ 𝔤 (dimension 2·dim(G)). + * + * Element is a pair: .first ∈ G, .second ∈ 𝔤 ≅ ℝⁿ. + * Group law: (g₁,ξ₁)·(g₂,ξ₂) = (g₁g₂, ξ₁ + Ad_{g₁}ξ₂). + * + * Example: using TGSE3 = TangentLieGroup; // dimension 12 + */ +template +using TangentLieGroup = + ProductLieGroup::TangentVector, AdjointAction>; + +} // namespace gtsam diff --git a/tests/testActionProductLieGroup.cpp b/tests/testActionProductLieGroup.cpp new file mode 100644 index 0000000000..ccdefa0a86 --- /dev/null +++ b/tests/testActionProductLieGroup.cpp @@ -0,0 +1,653 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- + */ + +/** + * @file testActionProductLieGroup.cpp + * @date April, 2026 + * @author Rohan Bansal + * @author Jennifer Oum + * @brief unit tests for action-parameterized product Lie groups + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +constexpr double kTol = 1e-9; + +using Vector10 = Eigen::Matrix; + +// Rot2 acting on Point2 by rotation: φ(R, t) = R·t. +// The infinitesimal generator is Aφ(u) = Hat(u), a 2x2 skew matrix. +struct Rot2PointAction : public GroupAction { + static constexpr ActionType type = ActionType::Left; + + Point2 operator()(const Rot2& R, const Point2& t, + OptionalJacobian<2, 1> HR = {}, + OptionalJacobian<2, 2> Ht = {}) const { + return R.rotate(t, HR, Ht); + } + + static Matrix2 generator(const Vector1& u) { return Rot2::Hat(u); } +}; + +// Rot3 acting on Vector3 by rotation: φ(R, t) = R·t. +// The infinitesimal generator Aφ(u)·t = d/dt(exp(tu∧)·t)|₀ = u∧·t, +// so generator(u) = u∧ (skew-symmetric matrix). ProductLieGroup derives +// Expmap and Logmap automatically via φ₁(u∧) = SO(3) left Jacobian. +struct Rot3VectorAction : public GroupAction { + static constexpr ActionType type = ActionType::Left; + + Vector3 operator()(const Rot3& R, const Vector3& t, + OptionalJacobian<3, 3> HR = {}, + OptionalJacobian<3, 3> Ht = {}) const { + return R.rotate(t, HR, Ht); + } + + static Matrix3 generator(const Vector3& u) { return skewSymmetric(u); } +}; + +// SE(3) acting on ℝ⁴=(position p, time s): φ((R,ν),[p;s]) = [R·p+ν·s; s], the +// 4×4 homogeneous matrix M(g)=[[R,ν],[0,1]] times [p;s]. The Pose3 translation +// slot holds the velocity-boost ν. Reconstructs Gal(3) = SE(3) ⋉ ℝ⁴. +struct SE3Vector4Action : public GroupAction { + static constexpr ActionType type = ActionType::Left; + + Vector4 operator()(const Pose3& g, const Vector4& h, + OptionalJacobian<4, 6> Hg = {}, + OptionalJacobian<4, 4> Hh = {}) const { + const Rot3& R = g.rotation(); + const Point3 nu = g.translation(); // velocity-boost + const Vector3 p = h.head<3>(); + const double s = h(3); + + Matrix3 D_Rp_R, D_Rp_p; + const Point3 Rp = + R.rotate(p, Hg ? &D_Rp_R : nullptr, Hh ? &D_Rp_p : nullptr); + + Vector4 out; + out << (Rp + s * nu), s; + + if (Hg) { + // g·Exp([ω;ρ]) ≈ (R·Exp(ω^), ν + R·ρ): ∂/∂ω = -R·p^ (=D_Rp_R), ∂/∂ρ = s·R. + Hg->setZero(); + Hg->topLeftCorner<3, 3>() = D_Rp_R; + Hg->block<3, 3>(0, 3) = s * R.matrix(); + } + if (Hh) { + // ∂φ/∂h = M(g) = [[R, ν],[0,1]]. + Hh->setZero(); + Hh->topLeftCorner<3, 3>() = D_Rp_p; // ∂/∂p = R + Hh->block<3, 1>(0, 3) = nu; // ∂/∂s = ν + (*Hh)(3, 3) = 1.0; + } + return out; + } + + /// Generator Aφ([ω;ρ]) = 4×4 se(3) hat = [[ω^, ρ],[0,0]]. + static Matrix4 generator(const Vector6& u) { + Matrix4 A = Matrix4::Zero(); + A.topLeftCorner<3, 3>() = skewSymmetric(u.head<3>()); + A.block<3, 1>(0, 3) = u.tail<3>(); + return A; + } +}; + +using Semidirect2 = ProductLieGroup; +using Semidirect = ProductLieGroup; +using SemidirectGal3 = ProductLieGroup; + +namespace { + +Semidirect2 semidirect2State() { + return Semidirect2(Rot2::fromAngle(0.35), Point2(0.4, -0.6)); +} + +Vector3 semidirect2Xi() { + Vector3 xi; + xi << 0.25, 0.3, -0.2; + return xi; +} + +Vector3 pose2XiFromSemidirect2(const Vector3& xi) { + Vector3 pose2Xi; + pose2Xi << xi(1), xi(2), xi(0); + return pose2Xi; +} + +Vector3 semidirect2XiFromPose2(const Vector3& xi) { + Vector3 semidirectXi; + semidirectXi << xi(2), xi(0), xi(1); + return semidirectXi; +} + +Semidirect2 expmapSemidirect2Proxy(const Vector3& vec) { + return Semidirect2::Expmap(vec); +} + +Vector3 logmapSemidirect2Proxy(const Semidirect2& p) { + return Semidirect2::Logmap(p); +} + +Pose2 asPose2(const Semidirect2& state) { + return Pose2(state.first, state.second); +} + +Semidirect semidirectState1() { + return Semidirect(Rot3::RzRyRx(0.1, 0.2, -0.3), Vector3(1.0, -0.5, 0.25)); +} + +Semidirect semidirectState2() { + return Semidirect(Rot3::RzRyRx(-0.2, 0.1, 0.15), Vector3(-0.75, 0.4, 1.2)); +} + +Semidirect semidirectState3() { + return Semidirect(Rot3::RzRyRx(0.2, -0.1, 0.05), Vector3(0.3, -0.6, 0.8)); +} + +Semidirect semidirectState4() { + return Semidirect(Rot3::RzRyRx(0.1, -0.2, 0.3), Vector3(0.4, -0.1, 0.2)); +} + +Vector6 semidirectXi() { + Vector6 xi; + xi << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2; + return xi; +} + +Vector6 retractDelta() { + Vector6 delta; + delta << 0.05, -0.04, 0.03, 0.1, -0.2, 0.05; + return delta; +} + +Semidirect composeSemidirectProxy(const Semidirect& A, const Semidirect& B) { + return A.compose(B); +} + +Semidirect betweenSemidirectProxy(const Semidirect& A, const Semidirect& B) { + return A.between(B); +} + +Semidirect inverseSemidirectProxy(const Semidirect& A) { return A.inverse(); } + +Semidirect expmapSemidirectProxy(const Vector6& vec) { + return Semidirect::Expmap(vec); +} + +Vector6 logmapSemidirectProxy(const Semidirect& p) { + return Semidirect::Logmap(p); +} + +Semidirect retractSemidirectProxy(const Semidirect& X, const Vector6& v) { + return X.retract(v); +} + +Vector6 localCoordinatesSemidirectProxy(const Semidirect& X, + const Semidirect& Y) { + return X.localCoordinates(Y); +} + +Pose3 asPose3(const Semidirect& state) { + return Pose3(state.first, state.second); +} + +Gal3 asGal3(const SemidirectGal3& s) { + // Pose3.translation() holds the velocity-boost; H.head<3>() is the position. + return Gal3(s.first.rotation(), s.second.head<3>(), s.first.translation(), + s.second(3)); +} + +Vector10 gal3Xi() { + Vector10 xi; + xi << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2, 0.15, -0.25, 0.3, 0.2; + return xi; +} + +SemidirectGal3 expmapSGal3(const Vector10& v) { + return SemidirectGal3::Expmap(v); +} + +Vector10 logmapSGal3(const SemidirectGal3& s) { + return SemidirectGal3::Logmap(s); +} + +SemidirectGal3 gal3State1() { + return SemidirectGal3( + Pose3(Rot3::RzRyRx(0.1, 0.2, -0.3), Point3(1.0, -0.5, 0.25)), + (Vector4() << 0.4, -0.2, 0.7, 0.3).finished()); +} + +SemidirectGal3 gal3State2() { + return SemidirectGal3( + Pose3(Rot3::RzRyRx(-0.2, 0.1, 0.15), Point3(-0.75, 0.4, 1.2)), + (Vector4() << -0.3, 0.5, -0.1, 0.6).finished()); +} + +SemidirectGal3 gal3State3() { + return SemidirectGal3( + Pose3(Rot3::RzRyRx(0.2, -0.1, 0.05), Point3(0.3, -0.6, 0.8)), + (Vector4() << 0.2, 0.1, -0.4, 0.2).finished()); +} + +SemidirectGal3 gal3State4() { + return SemidirectGal3( + Pose3(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(0.4, -0.1, 0.2)), + (Vector4() << 0.15, -0.25, 0.3, 0.2).finished()); +} + +Vector10 gal3RetractDelta() { + Vector10 delta; + delta << 0.05, -0.04, 0.03, 0.1, -0.2, 0.05, 0.02, -0.03, 0.04, 0.01; + return delta; +} + +SemidirectGal3 composeSGal3(const SemidirectGal3& A, const SemidirectGal3& B) { + return A.compose(B); +} + +SemidirectGal3 betweenSGal3(const SemidirectGal3& A, const SemidirectGal3& B) { + return A.between(B); +} + +SemidirectGal3 inverseSGal3(const SemidirectGal3& A) { return A.inverse(); } + +SemidirectGal3 retractSGal3(const SemidirectGal3& X, const Vector10& v) { + return X.retract(v); +} + +Vector10 localCoordinatesSGal3(const SemidirectGal3& X, + const SemidirectGal3& Y) { + return X.localCoordinates(Y); +} + +} // namespace + +/* ************************************************************************* */ +// A second semidirect instance, Rot2 ⋉ R², checks the generic kernel path in +// a different dimension with Pose2 as the oracle. +TEST(Lie, ProductLieGroupSemidirectAction2D) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + + const Rot2PointAction action; + const Rot2 R1 = Rot2::fromAngle(0.3); + const Rot2 R2 = Rot2::fromAngle(-0.2); + const Point2 t(0.4, -0.5); + EXPECT_LEFT_ACTION(action, R1, R2, t); + + const Vector1 u = (Vector1() << 0.35).finished(); + const Point2 p(0.2, -0.7); + const double eps = 1e-7; + const Point2 generatorAction = Rot2PointAction::generator(u) * p; + const Point2 generatorFiniteDifference = + (Rot2::Expmap((Vector1() << eps * u(0)).finished()).rotate(p) - p) / eps; + EXPECT(assert_equal(generatorAction, generatorFiniteDifference, 1e-6)); + + const Vector3 xi = semidirect2Xi(); + const Semidirect2 actual = Semidirect2::Expmap(xi); + EXPECT(assert_equal(Pose2::Expmap(pose2XiFromSemidirect2(xi)), asPose2(actual), + kTol)); + EXPECT(assert_equal( + xi, semidirect2XiFromPose2(Pose2::Logmap(asPose2(actual))), kTol)); +} + +/* ************************************************************************* */ +// Check Expmap/Logmap values and Jacobians for a second semidirect product, +// independent of the Pose3-specific ordering conventions. +TEST(testActionProduct, ExpmapLogmap2D) { + const Vector3 xi = semidirect2Xi(); + + Matrix expH; + const Semidirect2 actual = Semidirect2::Expmap(xi, expH); + const Matrix numericExpH = numericalDerivative11(expmapSemidirect2Proxy, xi); + + EXPECT(assert_equal(Pose2::Expmap(pose2XiFromSemidirect2(xi)), asPose2(actual), + kTol)); + EXPECT(assert_equal(numericExpH, expH, 1e-6)); + + const Semidirect2 state = semidirect2State(); + Matrix logH; + const Vector3 actualLog = Semidirect2::Logmap(state, logH); + const Matrix numericLogH = numericalDerivative11(logmapSemidirect2Proxy, state); + + EXPECT(assert_equal( + semidirect2XiFromPose2(Pose2::Logmap(asPose2(state))), actualLog, kTol)); + EXPECT(assert_equal(numericLogH, logH, 1e-6)); +} + +/* ************************************************************************* */ +// Verify the semidirect product obeys the left action law and matches Pose3 +// behavior. +TEST(Lie, ProductLieGroupSemidirectAction) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + + const Rot3VectorAction action; + const Rot3 R1 = Rot3::RzRyRx(0.1, -0.2, 0.3); + const Rot3 R2 = Rot3::RzRyRx(-0.2, 0.05, 0.1); + const Vector3 t(0.4, -0.5, 0.6); + EXPECT_LEFT_ACTION(action, R1, R2, t); + + const Semidirect identity; + const Vector6 xi = semidirectXi(); + const Semidirect actual = identity.expmap(xi); + EXPECT(assert_equal(Pose3::Expmap(xi), asPose3(actual), kTol)); + EXPECT(assert_equal(xi, identity.logmap(actual), kTol)); + + const Semidirect a = + Semidirect(Rot3::RzRyRx(0.1, 0.2, -0.1), Vector3(1.0, -2.0, 0.5)); + const Semidirect b = + Semidirect(Rot3::RzRyRx(-0.3, 0.15, 0.2), Vector3(-0.25, 0.4, 1.5)); + const Semidirect c = semidirectState3(); + EXPECT(assert_equal(asPose3((a * b) * c), asPose3(a * (b * c)), kTol)); + EXPECT(assert_equal(asPose3(a * a.inverse()), Pose3(), kTol)); + EXPECT(assert_equal(asPose3(a * b), asPose3(a) * asPose3(b), kTol)); +} + +/* ************************************************************************* */ +// Check semidirect compose values and Jacobians against Pose3 and numerical +// derivatives. +TEST(testActionProduct, compose) { + const Semidirect state1 = semidirectState1(); + const Semidirect state2 = semidirectState2(); + + Matrix actH1, actH2; + const Semidirect actual = state1.compose(state2, actH1, actH2); + const Matrix numericH1 = + numericalDerivative21(composeSemidirectProxy, state1, state2); + const Matrix numericH2 = + numericalDerivative22(composeSemidirectProxy, state1, state2); + + EXPECT( + assert_equal(asPose3(actual), asPose3(state1) * asPose3(state2), kTol)); + EXPECT(assert_equal(numericH1, actH1, 1e-6)); + EXPECT(assert_equal(numericH2, actH2, 1e-6)); +} + +/* ************************************************************************* */ +// Check semidirect between values and Jacobians against Pose3 and numerical +// derivatives. +TEST(testActionProduct, between) { + const Semidirect state1 = semidirectState1(); + const Semidirect state2 = semidirectState2(); + + Matrix actH1, actH2; + const Semidirect actual = state1.between(state2, actH1, actH2); + const Matrix numericH1 = + numericalDerivative21(betweenSemidirectProxy, state1, state2); + const Matrix numericH2 = + numericalDerivative22(betweenSemidirectProxy, state1, state2); + + EXPECT(assert_equal(asPose3(actual), asPose3(state1).between(asPose3(state2)), + kTol)); + EXPECT(assert_equal(numericH1, actH1, 1e-6)); + EXPECT(assert_equal(numericH2, actH2, 1e-6)); +} + +/* ************************************************************************* */ +// Check the semidirect inverse and its Jacobian against Pose3 and numerical +// derivatives. +TEST(testActionProduct, inverse) { + const Semidirect state = + Semidirect(Rot3::RzRyRx(0.2, -0.1, 0.05), Vector3(0.5, -1.2, 0.8)); + + Matrix actH; + const Semidirect actual = state.inverse(actH); + const Matrix numericH = numericalDerivative11(inverseSemidirectProxy, state); + + EXPECT(assert_equal(asPose3(actual), asPose3(state).inverse(), kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +// Check the semidirect Expmap and its Jacobian against Pose3 and numerical +// derivatives. +TEST(testActionProduct, Expmap) { + const Vector6 xi = semidirectXi(); + + Matrix actH; + const Semidirect actual = Semidirect::Expmap(xi, actH); + const Matrix numericH = numericalDerivative11(expmapSemidirectProxy, xi); + + EXPECT(assert_equal(Pose3::Expmap(xi), asPose3(actual), kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +// Check the semidirect Logmap and its Jacobian against Pose3 and numerical +// derivatives. +TEST(testActionProduct, Logmap) { + const Semidirect state = semidirectState4(); + + Matrix actH; + const Vector6 actual = Semidirect::Logmap(state, actH); + const Matrix numericH = numericalDerivative11(logmapSemidirectProxy, state); + + EXPECT(assert_equal(Pose3::Logmap(asPose3(state)), actual, kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +// Check that the semidirect adjoint matches the Pose3 adjoint. +TEST(testActionProduct, AdjointMap) { + const Semidirect state = semidirectState4(); + + EXPECT(assert_equal(asPose3(state).AdjointMap(), state.AdjointMap(), kTol)); +} + +/* ************************************************************************* */ +// Check Expmap-based retract/localCoordinates consistency and both Jacobians +// against numerical derivatives. +TEST(testActionProduct, retractAndLocalCoordinates) { + const Semidirect state = semidirectState4(); + const Vector6 delta = retractDelta(); + + Matrix retractH1, retractH2, localH1, localH2; + const Semidirect updated = state.retract(delta, retractH1, retractH2); + const Vector6 recovered = state.localCoordinates(updated, localH1, localH2); + + EXPECT(assert_equal(asPose3(updated), + asPose3(state).compose(Pose3::Expmap(delta)), kTol)); + EXPECT(assert_equal(delta, recovered, kTol)); + + const Matrix numericRetractH1 = + numericalDerivative21(retractSemidirectProxy, state, delta); + const Matrix numericRetractH2 = + numericalDerivative22(retractSemidirectProxy, state, delta); + const Matrix numericLocalH1 = + numericalDerivative21(localCoordinatesSemidirectProxy, state, updated); + const Matrix numericLocalH2 = + numericalDerivative22(localCoordinatesSemidirectProxy, state, updated); + + EXPECT(assert_equal(numericRetractH1, retractH1, 1e-6)); + EXPECT(assert_equal(numericRetractH2, retractH2, 1e-6)); + EXPECT(assert_equal(numericLocalH1, localH1, 1e-6)); + EXPECT(assert_equal(numericLocalH2, localH2, 1e-6)); +} + +/* ************************************************************************* */ +// A third semidirect instance, SE(3) ⋉ ℝ⁴, reconstructs Gal(3) and exercises a +// non-trivial (Pose3) base with a 4-D action, using the native Gal3 as oracle. +TEST(Lie, ProductLieGroupSemidirectActionGal3) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + EXPECT_LONGS_EQUAL(10, SemidirectGal3::dimension); + + const SE3Vector4Action action; + const Pose3 g1 = Pose3(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(0.4, -0.5, 0.6)); + const Pose3 g2 = Pose3(Rot3::RzRyRx(-0.2, 0.05, 0.1), Point3(-0.3, 0.2, 0.1)); + const Vector4 h = (Vector4() << 0.4, -0.5, 0.6, 0.3).finished(); + EXPECT_LEFT_ACTION(action, g1, g2, h); + + const Vector6 u = (Vector6() << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6).finished(); + const Vector4 p = (Vector4() << 0.2, -0.7, 0.5, 0.4).finished(); + const double eps = 1e-7; + const Vector4 generatorAction = SE3Vector4Action::generator(u) * p; + const Vector4 generatorFiniteDifference = + (action(Pose3::Expmap(eps * u), p) - p) / eps; + EXPECT(assert_equal(generatorAction, generatorFiniteDifference, 1e-6)); + + const Vector10 xi = gal3Xi(); + const SemidirectGal3 actual = SemidirectGal3::Expmap(xi); + EXPECT(assert_equal(Gal3::Expmap(xi), asGal3(actual), kTol)); + EXPECT(assert_equal(xi, SemidirectGal3::Logmap(actual), kTol)); + + const SemidirectGal3 a = gal3State1(); + const SemidirectGal3 b = gal3State2(); + const SemidirectGal3 c = gal3State3(); + EXPECT(assert_equal(asGal3((a * b) * c), asGal3(a * (b * c)), kTol)); + EXPECT(assert_equal(asGal3(a * a.inverse()), Gal3(), kTol)); + EXPECT(assert_equal(asGal3(a * b), asGal3(a) * asGal3(b), kTol)); +} + +/* ************************************************************************* */ +// Check Gal(3) compose values and Jacobians against native Gal3 and numerical +// derivatives. +TEST(testActionProduct, composeGal3) { + const SemidirectGal3 state1 = gal3State1(); + const SemidirectGal3 state2 = gal3State2(); + + Matrix actH1, actH2; + const SemidirectGal3 actual = state1.compose(state2, actH1, actH2); + const Matrix numericH1 = numericalDerivative21(composeSGal3, state1, state2); + const Matrix numericH2 = numericalDerivative22(composeSGal3, state1, state2); + + EXPECT(assert_equal(asGal3(actual), asGal3(state1) * asGal3(state2), kTol)); + EXPECT(assert_equal(numericH1, actH1, 1e-6)); + EXPECT(assert_equal(numericH2, actH2, 1e-6)); +} + +/* ************************************************************************* */ +// Check Gal(3) between values and Jacobians against native Gal3 and numerical +// derivatives. +TEST(testActionProduct, betweenGal3) { + const SemidirectGal3 state1 = gal3State1(); + const SemidirectGal3 state2 = gal3State2(); + + Matrix actH1, actH2; + const SemidirectGal3 actual = state1.between(state2, actH1, actH2); + const Matrix numericH1 = numericalDerivative21(betweenSGal3, state1, state2); + const Matrix numericH2 = numericalDerivative22(betweenSGal3, state1, state2); + + EXPECT(assert_equal(asGal3(actual), asGal3(state1).between(asGal3(state2)), + kTol)); + EXPECT(assert_equal(numericH1, actH1, 1e-6)); + EXPECT(assert_equal(numericH2, actH2, 1e-6)); +} + +/* ************************************************************************* */ +// Check the Gal(3) inverse and its Jacobian against native Gal3 and numerical +// derivatives. +TEST(testActionProduct, inverseGal3) { + const SemidirectGal3 state = gal3State4(); + + Matrix actH; + const SemidirectGal3 actual = state.inverse(actH); + const Matrix numericH = numericalDerivative11(inverseSGal3, state); + + EXPECT(assert_equal(asGal3(actual), asGal3(state).inverse(), kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +// Check the Gal(3) Expmap and its Jacobian against native Gal3 and numerical +// derivatives. +TEST(testActionProduct, ExpmapGal3) { + const Vector10 xi = gal3Xi(); + + Matrix actH; + const SemidirectGal3 actual = SemidirectGal3::Expmap(xi, actH); + const Matrix numericH = numericalDerivative11(expmapSGal3, xi); + + EXPECT(assert_equal(Gal3::Expmap(xi), asGal3(actual), kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +// Check the Gal(3) Logmap and its Jacobian against native Gal3 and numerical +// derivatives. +TEST(testActionProduct, LogmapGal3) { + const SemidirectGal3 state = gal3State4(); + + Matrix actH; + const Vector10 actual = SemidirectGal3::Logmap(state, actH); + const Matrix numericH = numericalDerivative11(logmapSGal3, state); + + EXPECT(assert_equal(Gal3::Logmap(asGal3(state)), actual, kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +// Check that the Gal(3) adjoint matches the native Gal3 adjoint. +TEST(testActionProduct, AdjointMapGal3) { + const SemidirectGal3 state = gal3State4(); + + EXPECT(assert_equal(Matrix(asGal3(state).AdjointMap()), + Matrix(state.AdjointMap()), kTol)); +} + +/* ************************************************************************* */ +// Static algebra adjoint of the semidirect product matches native Gal3::ad. +TEST(testActionProduct, AlgebraAdjointGal3) { + const Vector10 xi = gal3Xi(); + EXPECT(assert_equal(Matrix(Gal3::adjointMap(xi)), + Matrix(SemidirectGal3::adjointMap(xi)), kTol)); +} + +/* ************************************************************************* */ +// Check Expmap-based retract/localCoordinates consistency and both Jacobians +// against numerical derivatives. +TEST(testActionProduct, retractAndLocalCoordinatesGal3) { + const SemidirectGal3 state = gal3State4(); + const Vector10 delta = gal3RetractDelta(); + + Matrix retractH1, retractH2, localH1, localH2; + const SemidirectGal3 updated = state.retract(delta, retractH1, retractH2); + const Vector10 recovered = state.localCoordinates(updated, localH1, localH2); + + EXPECT(assert_equal(asGal3(updated), + asGal3(state).compose(Gal3::Expmap(delta)), kTol)); + EXPECT(assert_equal(delta, recovered, kTol)); + + const Matrix numericRetractH1 = + numericalDerivative21(retractSGal3, state, delta); + const Matrix numericRetractH2 = + numericalDerivative22(retractSGal3, state, delta); + const Matrix numericLocalH1 = + numericalDerivative21(localCoordinatesSGal3, state, updated); + const Matrix numericLocalH2 = + numericalDerivative22(localCoordinatesSGal3, state, updated); + + EXPECT(assert_equal(numericRetractH1, retractH1, 1e-6)); + EXPECT(assert_equal(numericRetractH2, retractH2, 1e-6)); + EXPECT(assert_equal(numericLocalH1, localH1, 1e-6)); + EXPECT(assert_equal(numericLocalH2, localH2, 1e-6)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/tests/testTangentLieGroup.cpp b/tests/testTangentLieGroup.cpp new file mode 100644 index 0000000000..970b649d16 --- /dev/null +++ b/tests/testTangentLieGroup.cpp @@ -0,0 +1,213 @@ +/* ---------------------------------------------------------------------------- + * 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 testTangentLieGroup.cpp + * @date June, 2026 + * @author Alessandro Fornasier + * @brief Tangent Lie group TG = G ⋉ 𝔤 via the semidirect ProductLieGroup. + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +constexpr double kTol = 1e-9; +constexpr double kNumTol = 1e-6; + +// GTSAM provides Vector4/Vector6 but not these fixed sizes; define locally. +using Vector12 = Eigen::Matrix; +using Vector20 = Eigen::Matrix; + +using TGSO3 = TangentLieGroup; +using TGSE3 = TangentLieGroup; +using TGGal3 = TangentLieGroup; + +namespace { + +TGSE3 tgse3State() { + return TGSE3(Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1.0, 2.0, 3.0)), + (Vector6() << 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished()); +} +TGSE3 tgse3State2() { + return TGSE3(Pose3(Rot3::RzRyRx(-0.2, 0.1, 0.15), Point3(-0.75, 0.4, 1.2)), + (Vector6() << -0.3, 0.2, 0.1, 0.5, -0.4, 0.25).finished()); +} +Vector6 tgso3Xi() { + return (Vector6() << 0.1, -0.2, 0.3, 0.4, -0.5, 0.6).finished(); +} +Vector12 tgse3Xi() { + Vector12 v; + v << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2, 0.05, 0.15, -0.25, 0.3, -0.35, 0.1; + return v; +} +TGSE3 expmapTGSE3(const Vector12& v) { return TGSE3::Expmap(v); } +Vector12 logmapTGSE3(const TGSE3& x) { return TGSE3::Logmap(x); } +TGSE3 composeTGSE3(const TGSE3& a, const TGSE3& b) { return a.compose(b); } +TGSE3 betweenTGSE3(const TGSE3& a, const TGSE3& b) { return a.between(b); } +TGSE3 inverseTGSE3(const TGSE3& a) { return a.inverse(); } + +Vector20 tggal3Xi() { + Vector20 v; + v << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2, 0.15, -0.25, 0.3, 0.2, // base (gal3) + 0.05, 0.1, -0.15, 0.2, -0.05, 0.1, 0.12, -0.08, 0.18, -0.1; // algebra + return v; +} +TGGal3 expmapTGGal3(const Vector20& v) { return TGGal3::Expmap(v); } +Vector20 logmapTGGal3(const TGGal3& x) { return TGGal3::Logmap(x); } + +} // namespace + +/* ************************************************************************* */ +TEST(TangentLieGroup, Concepts) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + EXPECT_LONGS_EQUAL(12, TGSE3::dimension); + EXPECT_LONGS_EQUAL(6, TGSO3::dimension); +} + +/* ************************************************************************* */ +TEST(TangentLieGroup, IdentityComposeInverse) { + const TGSE3 x = tgse3State(); + EXPECT(assert_equal(TGSE3::Identity(), x * x.inverse(), kTol)); + EXPECT(assert_equal(TGSE3::Identity(), x.inverse() * x, kTol)); + // group law: (g1,v1)*(g2,v2) = (g1 g2, v1 + Ad_{g1} v2) + const TGSE3 a(Pose3(Rot3::Rz(0.2), Point3(1, 0, 0)), + (Vector6() << 0, 0, 0.1, 0.2, 0, 0).finished()); + const TGSE3 b(Pose3(Rot3::Ry(0.1), Point3(0, 1, 0)), + (Vector6() << 0.05, 0, 0, 0, 0.3, 0).finished()); + const TGSE3 ab = a * b; + EXPECT(assert_equal(a.first * b.first, ab.first, kTol)); + EXPECT(assert_equal(Vector(a.second + a.first.AdjointMap() * b.second), + Vector(ab.second), kTol)); +} + +/* ************************************************************************* */ +// AdjointAction obeys the left-action law and its generator matches a finite +// difference of Ad_{Exp(εu)}·ξ. +TEST(TangentLieGroup, AdjointActionLawAndGenerator) { + const AdjointAction phi; + const Pose3 g1 = Pose3(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(0.4, -0.5, 0.6)); + const Pose3 g2 = Pose3(Rot3::RzRyRx(-0.2, 0.05, 0.1), Point3(-0.3, 0.2, 0.1)); + const Vector6 xi = (Vector6() << 0.2, -0.1, 0.3, 0.4, 0.5, -0.6).finished(); + EXPECT_LEFT_ACTION(phi, g1, g2, xi); + + const Vector6 u = (Vector6() << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6).finished(); + const double eps = 1e-7; + const Vector6 genFd = (Pose3::Expmap(eps * u).AdjointMap() * xi - xi) / eps; + EXPECT(assert_equal(Vector(AdjointAction::generator(u) * xi), + Vector(genFd), 1e-6)); +} + +/* ************************************************************************* */ +TEST(TangentLieGroup, ExpLogRoundTrip) { + const Vector6 xi = tgso3Xi(); + EXPECT(assert_equal(xi, TGSO3::Logmap(TGSO3::Expmap(xi)), kTol)); + const TGSE3 x = tgse3State(); + EXPECT(assert_equal(x, TGSE3::Expmap(TGSE3::Logmap(x)), kTol)); +} + +/* ************************************************************************* */ +// Expmap([u; xi]).second == J_l^G(u) * xi, with J_l(u) = Ad_{Exp(u)} * J_r(u) +// and J_r(u) = the Jacobian returned by Expmap. +TEST(TangentLieGroup, TransportBlockIsLeftJacobian) { + const Vector6 u = (Vector6() << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6).finished(); + const Vector6 xi = (Vector6() << -0.2, 0.1, 0.05, 0.3, -0.1, 0.2).finished(); + Vector12 tv; + tv << u, xi; + Matrix6 Jr; + const Pose3 g = Pose3::Expmap(u, Jr); + const Matrix6 Jl = g.AdjointMap() * Jr; + const TGSE3 x = TGSE3::Expmap(tv); + EXPECT(assert_equal(g, x.first, kTol)); + EXPECT(assert_equal(Vector(Jl * xi), Vector(x.second), kTol)); +} + +/* ************************************************************************* */ +TEST(TangentLieGroup, ExpmapLogmapJacobians) { + const Vector12 xi = tgse3Xi(); + Matrix expH; + TGSE3::Expmap(xi, expH); + EXPECT(assert_equal(numericalDerivative11(expmapTGSE3, xi), expH, kNumTol)); + + const TGSE3 state = tgse3State2(); + Matrix logH; + TGSE3::Logmap(state, logH); + EXPECT(assert_equal(numericalDerivative11(logmapTGSE3, state), logH, kNumTol)); +} + +/* ************************************************************************* */ +TEST(TangentLieGroup, ComposeBetweenInverseJacobians) { + const TGSE3 a = tgse3State(), b = tgse3State2(); + Matrix H1, H2; + + a.compose(b, H1, H2); + EXPECT(assert_equal(numericalDerivative21(composeTGSE3, a, b), H1, kNumTol)); + EXPECT(assert_equal(numericalDerivative22(composeTGSE3, a, b), H2, kNumTol)); + + a.between(b, H1, H2); + EXPECT(assert_equal(numericalDerivative21(betweenTGSE3, a, b), H1, kNumTol)); + EXPECT(assert_equal(numericalDerivative22(betweenTGSE3, a, b), H2, kNumTol)); + + Matrix H; + a.inverse(H); + EXPECT(assert_equal(numericalDerivative11(inverseTGSE3, a), H, kNumTol)); +} + +/* ************************************************************************* */ +// Ad_{(g,ξ)} = [[Ad_g, 0], [ad_ξ·Ad_g, Ad_g]]. +TEST(TangentLieGroup, AdjointMap) { + const TGSE3 x = tgse3State(); + const Matrix6 Ad = x.first.AdjointMap(); + const Matrix6 adXi = Pose3::adjointMap(x.second); + Eigen::Matrix expected = Eigen::Matrix::Zero(); + expected.topLeftCorner<6, 6>() = Ad; + expected.bottomRightCorner<6, 6>() = Ad; + expected.bottomLeftCorner<6, 6>() = adXi * Ad; + EXPECT(assert_equal(Matrix(expected), Matrix(x.AdjointMap()), kTol)); +} + +/* ************************************************************************* */ +TEST(TangentLieGroup, TGGal3Concepts) { + GTSAM_CONCEPT_ASSERT(IsLieGroup); + EXPECT_LONGS_EQUAL(20, TGGal3::dimension); +} + +/* ************************************************************************* */ +TEST(TangentLieGroup, TGGal3RoundTripAndAxioms) { + const Vector20 xi = tggal3Xi(); + const TGGal3 x = TGGal3::Expmap(xi); + EXPECT(assert_equal(xi, TGGal3::Logmap(x), kTol)); + EXPECT(assert_equal(TGGal3::Identity(), x * x.inverse(), kTol)); +} + +/* ************************************************************************* */ +TEST(TangentLieGroup, TGGal3Jacobians) { + const Vector20 xi = tggal3Xi(); + Matrix expH; + TGGal3::Expmap(xi, expH); + EXPECT(assert_equal(numericalDerivative11(expmapTGGal3, xi), expH, kNumTol)); + + const TGGal3 state = TGGal3::Expmap(tggal3Xi() * 0.5); + Matrix logH; + TGGal3::Logmap(state, logH); + EXPECT(assert_equal(numericalDerivative11(logmapTGGal3, state), logH, kNumTol)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +}