diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index aa6ef1be89..e76bfafbf3 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -136,6 +136,7 @@ else() -Wall # Enable common warnings -Wpedantic # Enable pedantic warnings $<$:-Wextra -Wno-unused-parameter> # Enable extra warnings, but ignore no-unused-parameter (as we ifdef out chunks of code) + $<$:-Wno-psabi> # GCC 10 emits non-actionable psABI notes for some value-returning wrappers under C++17 $<$:-Wreturn-local-addr> # Error: return local address $<$:-Wreturn-stack-address> # Error: return local address $<$:-Wno-weak-template-vtables> # TODO(dellaert): don't know how to resolve diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 51b453678c..f48ae88b56 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -74,11 +74,11 @@ template struct GetDimensionImpl { // Get dimension at compile-time for fixed-size manifolds, and at // run-time for dynamic-size manifolds. - static int GetDimension(const Class& m) { + static size_t GetDimension(const Class& m) { if constexpr (N == Eigen::Dynamic) { return m.dim(); } else { - return N; + return static_cast(N); } } }; diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h new file mode 100644 index 0000000000..905e104724 --- /dev/null +++ b/gtsam/base/ProductLieGroup-inl.h @@ -0,0 +1,663 @@ +/* ---------------------------------------------------------------------------- + + * 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 ProductLieGroup-inl.h + * @date March, 2026 + * @author Frank Dellaert + * @brief Internals for ProductLieGroup.h, not for general consumption + */ + +#pragma once + +namespace gtsam { + +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)); +} + +template +ProductLieGroup ProductLieGroup::inverse() const { + return ProductLieGroup(traits::Inverse(this->first), + traits::Inverse(this->second)); +} + +template +ProductLieGroup ProductLieGroup::retract(const TangentVector& v, + ChartJacobian H1, + ChartJacobian H2) const { + if (H1 || H2) { + throw std::runtime_error( + "ProductLieGroup::retract derivatives not implemented yet"); + } + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + if (static_cast(v.size()) != + combinedDimension(firstDimension, secondDimension)) { + throw std::invalid_argument( + "ProductLieGroup::retract tangent dimension does not match product " + "dimension"); + } + G g = + traits::Retract(this->first, tangentSegment(v, 0, firstDimension)); + H h = traits::Retract( + this->second, tangentSegment(v, firstDimension, secondDimension)); + return ProductLieGroup(g, h); +} + +template +typename ProductLieGroup::TangentVector +ProductLieGroup::localCoordinates(const ProductLieGroup& g, + ChartJacobian H1, + ChartJacobian H2) const { + if (H1 || H2) { + throw std::runtime_error( + "ProductLieGroup::localCoordinates derivatives not implemented yet"); + } + checkMatchingDimensions(g, "localCoordinates"); + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + typename traits::TangentVector v1 = traits::Local(this->first, g.first); + typename traits::TangentVector v2 = + traits::Local(this->second, g.second); + return makeTangentVector(v1, v2, firstDimension, secondDimension); +} + +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& 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); +} + +template +ProductLieGroup ProductLieGroup::Expmap(const TangentVector& v, + ChartJacobian Hv) { + size_t firstDimension = 0; + size_t secondDimension = 0; + if constexpr (firstDynamic && secondDynamic) { + if (v.size() == 0) { + if (Hv) *Hv = Matrix::Zero(0, 0); + return ProductLieGroup(); + } + throw std::invalid_argument( + "ProductLieGroup::Expmap requires split tangent vectors when both " + "factors are dynamic"); + } else if constexpr (firstDynamic) { + if (v.size() < dimension2) { + throw std::invalid_argument( + "ProductLieGroup::Expmap tangent dimension is too small for the " + "fixed second factor"); + } + firstDimension = static_cast(v.size() - dimension2); + secondDimension = static_cast(dimension2); + } else if constexpr (secondDynamic) { + if (v.size() < dimension1) { + throw std::invalid_argument( + "ProductLieGroup::Expmap tangent dimension is too small for the " + "fixed first factor"); + } + firstDimension = static_cast(dimension1); + secondDimension = static_cast(v.size() - dimension1); + } else { + firstDimension = static_cast(dimension1); + secondDimension = static_cast(dimension2); + } + if (static_cast(v.size()) != + combinedDimension(firstDimension, secondDimension)) { + throw std::invalid_argument( + "ProductLieGroup::Expmap tangent dimension does not match product " + "dimension"); + } + Matrix D_g_first; + Matrix D_h_second; + const auto v1 = tangentSegment(v, 0, firstDimension); + const auto v2 = tangentSegment(v, firstDimension, secondDimension); + ProductLieGroup result = + Expmap(v1, v2, + Hv ? OptionalJacobian(D_g_first) + : OptionalJacobian(), + Hv ? OptionalJacobian(D_h_second) + : OptionalJacobian()); + if (Hv) { + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + *Hv = zeroJacobian(productDimension); + Hv->block(0, 0, productDimension, firstDimension) = D_g_first; + Hv->block(0, firstDimension, productDimension, secondDimension) = + D_h_second; + } + return result; +} + +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 (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; + } + return v; +} + +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 +T ProductLieGroup::defaultIdentity() { + if constexpr (traits::dimension == Eigen::Dynamic) { + return T(); + } else { + return traits::Identity(); + } +} + +template +template +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); + if constexpr (Dim == Eigen::Dynamic) { + return v.segment(startIndex, runtimeIndex); + } else { + static_cast(runtimeDimension); + return v.template segment(startIndex); + } +} + +template +typename ProductLieGroup::TangentVector +ProductLieGroup::makeTangentVector( + const typename traits::TangentVector& v1, + const typename traits::TangentVector& v2, size_t firstDimension, + size_t secondDimension) { + const int firstIndex = static_cast(firstDimension); + const int secondIndex = static_cast(secondDimension); + if constexpr (dimension == Eigen::Dynamic) { + TangentVector v(combinedDimension(firstDimension, secondDimension)); + v.segment(0, firstIndex) = v1; + v.segment(firstIndex, secondIndex) = v2; + return v; + } else { + static_cast(firstDimension); + static_cast(secondDimension); + TangentVector v; + v << v1, v2; + return v; + } +} + +template +typename ProductLieGroup::Jacobian ProductLieGroup::zeroJacobian( + size_t productDimension) { + if constexpr (dimension == Eigen::Dynamic) { + return Jacobian::Zero(productDimension, productDimension); + } else { + static_cast(productDimension); + return Jacobian::Zero(); + } +} + +template +typename ProductLieGroup::Jacobian +ProductLieGroup::identityJacobian(size_t productDimension) { + if constexpr (dimension == Eigen::Dynamic) { + return Jacobian::Identity(productDimension, productDimension); + } else { + static_cast(productDimension); + return Jacobian::Identity(); + } +} + +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 + + " requires matching component dimensions"); + } +} + +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"); +} + +template +void PowerLieGroupBase::checkDynamicTangentSize( + const TangentVector& v, size_t count, const char* operation) { + if constexpr (isDynamic) { + if (static_cast(v.size()) != totalDimension(count)) { + throw std::invalid_argument(std::string("PowerLieGroup::") + operation + + " tangent dimension does not match group " + "dimension"); + } + } else { + static_cast(v); + static_cast(count); + static_cast(operation); + } +} + +template +void PowerLieGroupBase::checkMatchingCounts( + const Derived& other, const char* operation) const { + if constexpr (isDynamic) { + if (derived().size() != other.size()) { + throw std::invalid_argument(std::string("PowerLieGroup::") + operation + + " requires matching component counts"); + } + } else { + static_cast(other); + static_cast(operation); + } +} + +template +typename traits::TangentVector +PowerLieGroupBase::tangentSegment(const TangentVector& v, + size_t i) { + if constexpr (isDynamic) { + return v.segment(offset(i), baseDimension); + } else { + return v.template segment(i * baseDimension); + } +} + +template +Derived PowerLieGroupBase::makeResult(size_t count) { + if constexpr (isDynamic) { + return Derived(count); + } else { + static_cast(count); + return Derived(); + } +} + +template +typename PowerLieGroupBase::JacobianStorage +PowerLieGroupBase::makeJacobianStorage(size_t count) { + if constexpr (isDynamic) { + return JacobianStorage(count); + } else { + static_cast(count); + return JacobianStorage(); + } +} + +template +void PowerLieGroupBase::assignTangentSegment( + TangentVector& v, size_t i, const typename traits::TangentVector& vi) { + if constexpr (isDynamic) { + v.segment(offset(i), baseDimension) = vi; + } else { + v.template segment(i * baseDimension) = vi; + } +} + +template +template +void PowerLieGroupBase::assignJacobianBlock( + MatrixType& H, size_t i, const BaseJacobian& block) { + if constexpr (isDynamic) { + H.block(offset(i), offset(i), baseDimension, baseDimension) = block; + } else { + H.template block(i * baseDimension, + i * baseDimension) = block; + } +} + +template +void PowerLieGroupBase::fillJacobianBlocks( + ChartJacobian H, const JacobianStorage& jacobians, size_t count) { + if (!H) return; + *H = zeroJacobian(count); + for (size_t i = 0; i < count; ++i) { + assignJacobianBlock(*H, i, jacobians[i]); + } +} + +template +Derived PowerLieGroupBase::operator*( + const Derived& other) const { + checkMatchingCounts(other, "operator*"); + Derived result = makeResult(componentCount()); + for (size_t i = 0; i < componentCount(); ++i) { + result[i] = traits::Compose(derived()[i], other[i]); + } + return result; +} + +template +Derived PowerLieGroupBase::inverse() const { + Derived result = makeResult(componentCount()); + for (size_t i = 0; i < componentCount(); ++i) { + result[i] = traits::Inverse(derived()[i]); + } + return result; +} + +template +Derived PowerLieGroupBase::retract(const TangentVector& v, + ChartJacobian H1, + ChartJacobian H2) const { + if (H1 || H2) { + throw std::runtime_error( + "PowerLieGroup::retract derivatives not implemented yet"); + } + const size_t count = componentCount(); + checkDynamicTangentSize(v, count, "retract"); + Derived result = makeResult(count); + for (size_t i = 0; i < count; ++i) { + result[i] = traits::Retract(derived()[i], tangentSegment(v, i)); + } + return result; +} + +template +typename PowerLieGroupBase::TangentVector +PowerLieGroupBase::localCoordinates(const Derived& g, + ChartJacobian H1, + ChartJacobian H2) const { + if (H1 || H2) { + throw std::runtime_error( + "PowerLieGroup::localCoordinates derivatives not implemented yet"); + } + checkMatchingCounts(g, "localCoordinates"); + TangentVector v = zeroTangent(componentCount()); + for (size_t i = 0; i < componentCount(); ++i) { + assignTangentSegment(v, i, traits::Local(derived()[i], g[i])); + } + return v; +} + +template +Derived PowerLieGroupBase::compose(const Derived& other, + ChartJacobian H1, + ChartJacobian H2) const { + checkMatchingCounts(other, "compose"); + const size_t count = componentCount(); + JacobianStorage jacobians = makeJacobianStorage(count); + Derived result = makeResult(count); + for (size_t i = 0; i < count; ++i) { + result[i] = traits::Compose(derived()[i], other[i], + H1 ? &jacobians[i] : nullptr); + } + fillJacobianBlocks(H1, jacobians, count); + if (H2) *H2 = identityJacobian(count); + return result; +} + +template +Derived PowerLieGroupBase::between(const Derived& other, + ChartJacobian H1, + ChartJacobian H2) const { + checkMatchingCounts(other, "between"); + const size_t count = componentCount(); + JacobianStorage jacobians = makeJacobianStorage(count); + Derived result = makeResult(count); + for (size_t i = 0; i < count; ++i) { + result[i] = traits::Between(derived()[i], other[i], + H1 ? &jacobians[i] : nullptr); + } + fillJacobianBlocks(H1, jacobians, count); + if (H2) *H2 = identityJacobian(count); + return result; +} + +template +Derived PowerLieGroupBase::inverse(ChartJacobian D) const { + const size_t count = componentCount(); + JacobianStorage jacobians = makeJacobianStorage(count); + Derived result = makeResult(count); + for (size_t i = 0; i < count; ++i) { + result[i] = traits::Inverse(derived()[i], D ? &jacobians[i] : nullptr); + } + fillJacobianBlocks(D, jacobians, count); + return result; +} + +template +Derived PowerLieGroupBase::Expmap(const TangentVector& v, + ChartJacobian Hv) { + size_t count = 0; + if constexpr (isDynamic) { + if (v.size() % baseDimension != 0) { + throw std::invalid_argument( + "PowerLieGroup::Expmap tangent dimension must be divisible by base " + "group dimension"); + } + count = static_cast(v.size() / + static_cast(baseDimension)); + } else { + count = N; + } + JacobianStorage jacobians = makeJacobianStorage(count); + Derived result = makeResult(count); + for (size_t i = 0; i < count; ++i) { + result[i] = + traits::Expmap(tangentSegment(v, i), Hv ? &jacobians[i] : nullptr); + } + fillJacobianBlocks(Hv, jacobians, count); + return result; +} + +template +typename PowerLieGroupBase::TangentVector +PowerLieGroupBase::Logmap(const Derived& p, ChartJacobian Hp) { + const size_t count = isDynamic ? p.size() : N; + TangentVector v = zeroTangent(count); + JacobianStorage jacobians = makeJacobianStorage(count); + for (size_t i = 0; i < count; ++i) { + assignTangentSegment(v, i, + traits::Logmap(p[i], Hp ? &jacobians[i] : nullptr)); + } + fillJacobianBlocks(Hp, jacobians, count); + return v; +} + +template +typename PowerLieGroupBase::Jacobian +PowerLieGroupBase::AdjointMap() const { + Jacobian adj = zeroJacobian(componentCount()); + for (size_t i = 0; i < componentCount(); ++i) { + assignJacobianBlock(adj, i, traits::AdjointMap(derived()[i])); + } + return adj; +} + +template +void PowerLieGroupBase::print(const std::string& s) const { + std::cout << s << "PowerLieGroup" << std::endl; + for (size_t i = 0; i < componentCount(); ++i) { + traits::Print(derived()[i], " component[" + std::to_string(i) + "]"); + } +} + +template +bool PowerLieGroupBase::equals(const Derived& other, + double tol) const { + if constexpr (isDynamic) { + if (derived().size() != other.size()) { + return false; + } + } + for (size_t i = 0; i < componentCount(); ++i) { + if (!traits::Equals(derived()[i], other[i], tol)) { + return false; + } + } + return true; +} + +template +typename PowerLieGroupBase::TangentVector +PowerLieGroupBase::zeroTangent(size_t count) { + if constexpr (isDynamic) { + return TangentVector::Zero(totalDimension(count)); + } else { + static_cast(count); + return TangentVector::Zero(); + } +} + +template +typename PowerLieGroupBase::Jacobian +PowerLieGroupBase::zeroJacobian(size_t count) { + if constexpr (isDynamic) { + return Jacobian::Zero(totalDimension(count), totalDimension(count)); + } else { + static_cast(count); + return Jacobian::Zero(); + } +} + +template +typename PowerLieGroupBase::Jacobian +PowerLieGroupBase::identityJacobian(size_t count) { + if constexpr (isDynamic) { + return Jacobian::Identity(totalDimension(count), totalDimension(count)); + } else { + static_cast(count); + return Jacobian::Identity(); + } +} + +template +PowerLieGroup::PowerLieGroup(const std::initializer_list& elements) { + if (elements.size() != N) { + throw std::invalid_argument( + "PowerLieGroup: initializer list size must equal N"); + } + std::copy(elements.begin(), elements.end(), this->begin()); +} + +} // namespace gtsam diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 2f925c7039..297652204a 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -21,10 +21,13 @@ #include #include +#include #include #include +#include #include #include // pair +#include namespace gtsam { @@ -45,15 +48,39 @@ class ProductLieGroup : public std::pair { protected: /// Dimensions of the two subgroups - static constexpr size_t dimension1 = traits::dimension; - static constexpr size_t dimension2 = traits::dimension; + inline constexpr static int dimension1 = traits::dimension; + inline constexpr static int dimension2 = traits::dimension; + inline constexpr static bool firstDynamic = dimension1 == Eigen::Dynamic; + inline constexpr static bool secondDynamic = dimension2 == Eigen::Dynamic; + + public: + /// Manifold dimension + inline constexpr static int dimension = + firstDynamic || secondDynamic ? Eigen::Dynamic : dimension1 + dimension2; + + /// Tangent vector type + using TangentVector = std::conditional_t>; + + /// Chart Jacobian type + using ChartJacobian = + std::conditional_t, + OptionalJacobian>; + + /// Jacobian types for internal use + using Jacobian = + std::conditional_t>; + using Jacobian1 = typename traits::Jacobian; + using Jacobian2 = typename traits::Jacobian; public: /// @name Standard Constructors /// @{ /// Default constructor yields identity - ProductLieGroup() : Base(traits::Identity(), traits::Identity()) {} + ProductLieGroup() : Base(defaultIdentity(), defaultIdentity()) {} /// Construct from two subgroup elements ProductLieGroup(const G& g, const H& h) : Base(g, h) {} @@ -71,16 +98,10 @@ class ProductLieGroup : public std::pair { static ProductLieGroup Identity() { return ProductLieGroup(); } /// Group multiplication - ProductLieGroup operator*(const ProductLieGroup& other) const { - return ProductLieGroup(traits::Compose(this->first, other.first), - traits::Compose(this->second, other.second)); - } + ProductLieGroup operator*(const ProductLieGroup& other) const; /// Group inverse - ProductLieGroup inverse() const { - return ProductLieGroup(traits::Inverse(this->first), - traits::Inverse(this->second)); - } + ProductLieGroup inverse() const; /// Compose with another element (same as operator*) ProductLieGroup compose(const ProductLieGroup& g) const { @@ -97,137 +118,50 @@ class ProductLieGroup : public std::pair { /// @{ /// Manifold dimension - static constexpr size_t dimension = dimension1 + dimension2; + inline constexpr static int manifoldDimension = dimension; /// Return manifold dimension - static size_t Dim() { return dimension; } + static constexpr int Dim() { return manifoldDimension; } /// Return manifold dimension - size_t dim() const { return dimension; } - - /// Tangent vector type - using TangentVector = Eigen::Matrix(dimension), 1>; - - /// Chart Jacobian type - using ChartJacobian = OptionalJacobian; + size_t dim() const { return combinedDimension(firstDim(), secondDim()); } /// Retract to manifold ProductLieGroup retract(const TangentVector& v, ChartJacobian H1 = {}, - ChartJacobian H2 = {}) const { - if (H1 || H2) { - throw std::runtime_error( - "ProductLieGroup::retract derivatives not implemented yet"); - } - G g = traits::Retract(this->first, v.template head()); - H h = traits::Retract(this->second, v.template tail()); - return ProductLieGroup(g, h); - } + ChartJacobian H2 = {}) const; /// Local coordinates on manifold TangentVector localCoordinates(const ProductLieGroup& g, ChartJacobian H1 = {}, - ChartJacobian H2 = {}) const { - if (H1 || H2) { - throw std::runtime_error( - "ProductLieGroup::localCoordinates derivatives not implemented yet"); - } - typename traits::TangentVector v1 = - traits::Local(this->first, g.first); - typename traits::TangentVector v2 = - traits::Local(this->second, g.second); - TangentVector v; - v << v1, v2; - return v; - } + ChartJacobian H2 = {}) const; /// @} /// @name Lie Group Operations /// @{ - public: - /// Jacobian types for internal use - using Jacobian = Eigen::Matrix(dimension), static_cast(dimension)>; - using Jacobian1 = Eigen::Matrix(dimension1), static_cast(dimension1)>; - using Jacobian2 = Eigen::Matrix(dimension2), static_cast(dimension2)>; - /// Compose with Jacobians ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1, - ChartJacobian H2 = {}) const { - Jacobian1 D_g_first = Jacobian1::Zero(); - Jacobian2 D_h_second; - G g = traits::Compose(this->first, other.first, H1 ? &D_g_first : 0); - H h = traits::Compose(this->second, other.second, H1 ? &D_h_second : 0); - if (H1) { - H1->setZero(); - H1->template topLeftCorner() = D_g_first; - H1->template bottomRightCorner() = D_h_second; - } - if (H2) *H2 = Jacobian::Identity(); - return ProductLieGroup(g, h); - } + ChartJacobian H2 = {}) const; /// Between with Jacobians ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1, - ChartJacobian H2 = {}) const { - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Between(this->first, other.first, H1 ? &D_g_first : 0); - H h = traits::Between(this->second, other.second, H1 ? &D_h_second : 0); - if (H1) { - H1->setZero(); - H1->template topLeftCorner() = D_g_first; - H1->template bottomRightCorner() = D_h_second; - } - if (H2) *H2 = Jacobian::Identity(); - return ProductLieGroup(g, h); - } + ChartJacobian H2 = {}) const; /// Inverse with Jacobian - ProductLieGroup inverse(ChartJacobian D) const { - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Inverse(this->first, D ? &D_g_first : 0); - H h = traits::Inverse(this->second, D ? &D_h_second : 0); - if (D) { - D->setZero(); - D->template topLeftCorner() = D_g_first; - D->template bottomRightCorner() = D_h_second; - } - return ProductLieGroup(g, h); - } + ProductLieGroup inverse(ChartJacobian D) const; /// Exponential map - static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = {}) { - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Expmap(v.template head(), Hv ? &D_g_first : 0); - H h = - traits::Expmap(v.template tail(), Hv ? &D_h_second : 0); - if (Hv) { - Hv->setZero(); - Hv->template topLeftCorner() = D_g_first; - Hv->template bottomRightCorner() = D_h_second; - } - return ProductLieGroup(g, h); - } + static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = {}); + + /// Exponential map from subgroup tangent vectors + static ProductLieGroup Expmap( + const Eigen::Ref::TangentVector>& v1, + const Eigen::Ref::TangentVector>& v2, + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}); /// Logarithmic map - static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {}) { - Jacobian1 D_g_first; - Jacobian2 D_h_second; - typename traits::TangentVector v1 = - traits::Logmap(p.first, Hp ? &D_g_first : 0); - typename traits::TangentVector v2 = - traits::Logmap(p.second, Hp ? &D_h_second : 0); - TangentVector v; - v << v1, v2; - if (Hp) { - Hp->setZero(); - Hp->template topLeftCorner() = D_g_first; - Hp->template bottomRightCorner() = D_h_second; - } - return v; - } + static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {}); /// Local coordinates (same as Logmap) static TangentVector LocalCoordinates(const ProductLieGroup& p, @@ -236,9 +170,7 @@ class ProductLieGroup : public std::pair { } /// Right multiplication by exponential map - ProductLieGroup expmap(const TangentVector& v) const { - return compose(ProductLieGroup::Expmap(v)); - } + ProductLieGroup expmap(const TangentVector& v) const; /// Logarithmic map for relative transformation TangentVector logmap(const ProductLieGroup& g) const { @@ -246,25 +178,46 @@ class ProductLieGroup : public std::pair { } /// Adjoint map - Jacobian AdjointMap() const { - const auto& adjG = traits::AdjointMap(this->first); - const auto& adjH = traits::AdjointMap(this->second); - size_t d1 = adjG.rows(), d2 = adjH.rows(); - Matrix adj = Matrix::Zero(d1 + d2, d1 + d2); - adj.block(0, 0, d1, d1) = adjG; - adj.block(d1, d1, d2, d2) = adjH; - return adj; - } + Jacobian AdjointMap() const; /// @} + protected: + /// Return default identity for fixed-size factors and a placeholder for + /// dynamic ones. + template + static T defaultIdentity(); + + size_t firstDim() const { return traits::GetDimension(this->first); } + size_t secondDim() const { return traits::GetDimension(this->second); } + + static size_t combinedDimension(size_t d1, size_t d2) { return d1 + d2; } + + /// Extract a tangent segment for one factor. + template ::dimension> + static typename traits::TangentVector tangentSegment( + const TangentVector& v, size_t start, size_t runtimeDimension); + + /// Concatenate subgroup tangent vectors into the product tangent. + static TangentVector makeTangentVector( + const typename traits::TangentVector& v1, + const typename traits::TangentVector& v2, size_t firstDimension, + size_t secondDimension); + + /// Create a zero Jacobian with the requested runtime size. + static Jacobian zeroJacobian(size_t productDimension); + + /// Create an identity Jacobian with the requested runtime size. + static Jacobian identityJacobian(size_t productDimension); + + /// Check that another product has matching runtime dimensions. + void checkMatchingDimensions(const ProductLieGroup& other, + const char* operation) const; + + public: /// @name Testable interface /// @{ - void print(const std::string& s = "") const { - std::cout << s << "ProductLieGroup" << std::endl; - traits::Print(this->first, " first"); - traits::Print(this->second, " second"); - } + void print(const std::string& s = "") const; bool equals(const ProductLieGroup& other, double tol = 1e-9) const { return traits::Equals(this->first, other.first, tol) && @@ -274,275 +227,298 @@ class ProductLieGroup : public std::pair { }; /** - * @brief Template to construct the N-fold power of a Lie group - * Represents the group G^N = G x G x ... x G (N times) - * Assumes Lie group structure for G and N >= 2 + * @brief Shared implementation for fixed-size and dynamic-count PowerLieGroup */ -template -class PowerLieGroup : public std::array { - static_assert(N >= 1, "PowerLieGroup requires N >= 1"); - GTSAM_CONCEPT_ASSERT(IsLieGroup); - GTSAM_CONCEPT_ASSERT(IsTestable); +template +struct PowerLieGroupJacobianStorage { + /// Container type for per-component Jacobians. + using type = std::array; +}; - public: - /// Base array type - typedef std::array Base; +template +struct PowerLieGroupJacobianStorage { + /// Container type for per-component Jacobians. + using type = std::vector; +}; +template +class PowerLieGroupBase { protected: - /// Dimension of the base group - static constexpr size_t baseDimension = traits::dimension; + static constexpr bool isDynamic = (N == Eigen::Dynamic); + static constexpr int baseDimension = traits::dimension; public: - /// @name Standard Constructors - /// @{ + typedef multiplicative_group_tag group_flavor; - /// Default constructor yields identity - PowerLieGroup() { this->fill(traits::Identity()); } + static constexpr int dimension = + isDynamic ? Eigen::Dynamic : N * baseDimension; - /// Construct from array of group elements - PowerLieGroup(const Base& elements) : Base(elements) {} + typedef std::conditional_t> + TangentVector; - /// Construct from initializer list - PowerLieGroup(const std::initializer_list& elements) { - if (elements.size() != N) { - throw std::invalid_argument( - "PowerLieGroup: initializer list size must equal N"); - } - std::copy(elements.begin(), elements.end(), this->begin()); - } + typedef std::conditional_t, + OptionalJacobian> + ChartJacobian; - /// @} - /// @name Group Operations - /// @{ + typedef std::conditional_t> + Jacobian; - typedef multiplicative_group_tag group_flavor; + using BaseJacobian = typename traits::Jacobian; + using JacobianStorage = + typename PowerLieGroupJacobianStorage::type; - /// Identity element - static PowerLieGroup Identity() { return PowerLieGroup(); } + protected: + /// Downcast to the derived storage type. + const Derived& derived() const { return static_cast(*this); } - /// Group multiplication - PowerLieGroup operator*(const PowerLieGroup& other) const { - PowerLieGroup result; - for (size_t i = 0; i < N; ++i) { - result[i] = traits::Compose((*this)[i], other[i]); - } - return result; + /// Downcast to the derived storage type. + Derived& derived() { return static_cast(*this); } + + /// Total tangent dimension for a given component count. + static size_t totalDimension(size_t count) { + return count * static_cast(baseDimension); } - /// Group inverse - PowerLieGroup inverse() const { - PowerLieGroup result; - for (size_t i = 0; i < N; ++i) { - result[i] = traits::Inverse((*this)[i]); + /// Starting offset of one component inside the concatenated tangent. + static Eigen::Index offset(size_t i) { + return static_cast(i * static_cast(baseDimension)); + } + + /// Runtime component count. + size_t componentCount() const { + if constexpr (isDynamic) { + return derived().size(); + } else { + return N; } - return result; } - /// Compose with another element (same as operator*) - PowerLieGroup compose(const PowerLieGroup& g) const { return (*this) * g; } + /// Validate tangent size for dynamic-count groups. + static void checkDynamicTangentSize(const TangentVector& v, size_t count, + const char* operation); - /// Calculate relative transformation - PowerLieGroup between(const PowerLieGroup& g) const { - return this->inverse() * g; - } + /// Validate matching component counts for binary operations. + void checkMatchingCounts(const Derived& other, const char* operation) const; - /// @} - /// @name Manifold Operations - /// @{ + /// Extract one component tangent from the concatenated tangent. + static typename traits::TangentVector tangentSegment( + const TangentVector& v, size_t i); - /// Manifold dimension - static constexpr size_t dimension = N * baseDimension; + /// Create a result object with the requested component count. + static Derived makeResult(size_t count); + + /// Create per-component Jacobian storage. + static JacobianStorage makeJacobianStorage(size_t count); + + /// Write one component tangent into the concatenated tangent. + static void assignTangentSegment(TangentVector& v, size_t i, + const typename traits::TangentVector& vi); + + /// Write one component block into a block-diagonal Jacobian. + template + static void assignJacobianBlock(MatrixType& H, size_t i, + const BaseJacobian& block); + /// Assemble a block-diagonal Jacobian from per-component blocks. + static void fillJacobianBlocks(ChartJacobian H, + const JacobianStorage& jacobians, + size_t count); + + public: /// Return manifold dimension - static size_t Dim() { return dimension; } + static constexpr int Dim() { return dimension; } /// Return manifold dimension - size_t dim() const { return dimension; } + size_t dim() const { return totalDimension(componentCount()); } - /// Tangent vector type - typedef Eigen::Matrix TangentVector; + /// Group multiplication + Derived operator*(const Derived& other) const; - /// Chart Jacobian type - typedef OptionalJacobian ChartJacobian; + /// Group inverse + Derived inverse() const; - /// Retract to manifold - PowerLieGroup retract(const TangentVector& v, ChartJacobian H1 = {}, - ChartJacobian H2 = {}) const { - if (H1 || H2) { - throw std::runtime_error( - "PowerLieGroup::retract derivatives not implemented yet"); - } - PowerLieGroup result; - for (size_t i = 0; i < N; ++i) { - const auto vi = v.template segment(i * baseDimension); - result[i] = traits::Retract((*this)[i], vi); - } - return result; - } + /// Compose with another element (same as operator*) + Derived compose(const Derived& g) const { return (*this) * g; } - /// Local coordinates on manifold - TangentVector localCoordinates(const PowerLieGroup& g, ChartJacobian H1 = {}, - ChartJacobian H2 = {}) const { - if (H1 || H2) { - throw std::runtime_error( - "PowerLieGroup::localCoordinates derivatives not implemented yet"); - } - TangentVector v; - for (size_t i = 0; i < N; ++i) { - const auto vi = traits::Local((*this)[i], g[i]); - v.template segment(i * baseDimension) = vi; - } - return v; - } + /// Calculate relative transformation + Derived between(const Derived& g) const { return this->inverse() * g; } - /// @} - /// @name Lie Group Operations - /// @{ + /// Retract to manifold + Derived retract(const TangentVector& v, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const; - public: - /// Jacobian types for internal use - typedef Eigen::Matrix Jacobian; - typedef Eigen::Matrix BaseJacobian; + /// Local coordinates on manifold + TangentVector localCoordinates(const Derived& g, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const; /// Compose with Jacobians - PowerLieGroup compose(const PowerLieGroup& other, ChartJacobian H1, - ChartJacobian H2 = {}) const { - std::array jacobians; - PowerLieGroup result; - for (size_t i = 0; i < N; ++i) { - result[i] = traits::Compose((*this)[i], other[i], - H1 ? &jacobians[i] : nullptr); - } - if (H1) { - H1->setZero(); - for (size_t i = 0; i < N; ++i) { - H1->template block( - i * baseDimension, i * baseDimension) = jacobians[i]; - } - } - if (H2) *H2 = Jacobian::Identity(); - return result; - } + Derived compose(const Derived& other, ChartJacobian H1, + ChartJacobian H2 = {}) const; /// Between with Jacobians - PowerLieGroup between(const PowerLieGroup& other, ChartJacobian H1, - ChartJacobian H2 = {}) const { - std::array jacobians; - PowerLieGroup result; - for (size_t i = 0; i < N; ++i) { - result[i] = traits::Between((*this)[i], other[i], - H1 ? &jacobians[i] : nullptr); - } - if (H1) { - H1->setZero(); - for (size_t i = 0; i < N; ++i) { - H1->template block( - i * baseDimension, i * baseDimension) = jacobians[i]; - } - } - if (H2) *H2 = Jacobian::Identity(); - return result; - } + Derived between(const Derived& other, ChartJacobian H1, + ChartJacobian H2 = {}) const; /// Inverse with Jacobian - PowerLieGroup inverse(ChartJacobian D) const { - std::array jacobians; - PowerLieGroup result; - for (size_t i = 0; i < N; ++i) { - result[i] = traits::Inverse((*this)[i], D ? &jacobians[i] : nullptr); - } - if (D) { - D->setZero(); - for (size_t i = 0; i < N; ++i) { - D->template block( - i * baseDimension, i * baseDimension) = jacobians[i]; - } - } - return result; - } + Derived inverse(ChartJacobian D) const; /// Exponential map - static PowerLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = {}) { - std::array jacobians; - PowerLieGroup result; - for (size_t i = 0; i < N; ++i) { - const auto vi = v.template segment(i * baseDimension); - result[i] = traits::Expmap(vi, Hv ? &jacobians[i] : nullptr); - } - if (Hv) { - Hv->setZero(); - for (size_t i = 0; i < N; ++i) { - Hv->template block( - i * baseDimension, i * baseDimension) = jacobians[i]; - } - } - return result; - } + static Derived Expmap(const TangentVector& v, ChartJacobian Hv = {}); /// Logarithmic map - static TangentVector Logmap(const PowerLieGroup& p, ChartJacobian Hp = {}) { - std::array jacobians; - TangentVector v; - for (size_t i = 0; i < N; ++i) { - const auto vi = traits::Logmap(p[i], Hp ? &jacobians[i] : nullptr); - v.template segment(i * baseDimension) = vi; - } - if (Hp) { - Hp->setZero(); - for (size_t i = 0; i < N; ++i) { - Hp->template block( - i * baseDimension, i * baseDimension) = jacobians[i]; - } - } - return v; - } + static TangentVector Logmap(const Derived& p, ChartJacobian Hp = {}); /// Local coordinates (same as Logmap) - static TangentVector LocalCoordinates(const PowerLieGroup& p, + static TangentVector LocalCoordinates(const Derived& p, ChartJacobian Hp = {}) { return Logmap(p, Hp); } /// Right multiplication by exponential map - PowerLieGroup expmap(const TangentVector& v) const { - return compose(PowerLieGroup::Expmap(v)); - } + Derived expmap(const TangentVector& v) const { return compose(Expmap(v)); } /// Logarithmic map for relative transformation - TangentVector logmap(const PowerLieGroup& g) const { - return PowerLieGroup::Logmap(between(g)); - } + TangentVector logmap(const Derived& g) const { return Logmap(between(g)); } /// Adjoint map - Jacobian AdjointMap() const { - Jacobian adj = Jacobian::Zero(); - for (size_t i = 0; i < N; ++i) { - const auto adjGi = traits::AdjointMap((*this)[i]); - adj.template block( - i * baseDimension, i * baseDimension) = adjGi; - } - return adj; - } + Jacobian AdjointMap() const; + + /// Print for debugging + void print(const std::string& s = "") const; + + /// Equality with tolerance + bool equals(const Derived& other, double tol = 1e-9) const; + + protected: + /// Create a zero tangent with the requested runtime size. + static TangentVector zeroTangent(size_t count); + + /// Create a zero Jacobian with the requested runtime size. + static Jacobian zeroJacobian(size_t count); + + /// Create an identity Jacobian with the requested runtime size. + static Jacobian identityJacobian(size_t count); +}; + +/** + * @brief Template to construct the N-fold power of a Lie group + * Represents the group G^N = G x G x ... x G (N times) + * Assumes Lie group structure for fixed-size G and fixed N >= 1 + */ +template +class PowerLieGroup : public std::array, + public PowerLieGroupBase> { + static_assert(N >= 1, "PowerLieGroup requires N >= 1"); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsTestable); + static_assert(traits::dimension != Eigen::Dynamic, + "PowerLieGroup requires a fixed-size base group"); + + public: + /// Base array type + typedef std::array Base; + typedef PowerLieGroupBase Helper; + using typename Helper::BaseJacobian; + using typename Helper::ChartJacobian; + using typename Helper::Jacobian; + using typename Helper::TangentVector; + static constexpr int dimension = Helper::dimension; + + public: + /// @name Standard Constructors + /// @{ + + /// Default constructor yields identity + PowerLieGroup() { this->fill(traits::Identity()); } + + /// Construct from array of group elements + PowerLieGroup(const Base& elements) : Base(elements) {} + + /// Construct from initializer list + PowerLieGroup(const std::initializer_list& elements); /// @} + /// @name Group Operations + /// @{ - /// @name Testable interface + /// Identity element + static PowerLieGroup Identity() { return PowerLieGroup(); } + + /// @} + /// @name Manifold Operations + /// @{ + + /// Return manifold dimension + static constexpr int Dim() { return dimension; } + + /// @} + /// @name Lie Group Operations + /// @{ + + /// @} +}; + +/** + * @brief Dynamic-count specialization of PowerLieGroup + * Represents G^N for runtime-sized N while keeping G fixed-size + */ +template +class PowerLieGroup + : public std::vector, + public PowerLieGroupBase> { + GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsTestable); + static_assert(traits::dimension != Eigen::Dynamic, + "PowerLieGroup requires a fixed-size base group"); + + public: + /// Base vector type + typedef std::vector Base; + typedef PowerLieGroupBase Helper; + using typename Helper::BaseJacobian; + using typename Helper::ChartJacobian; + using typename Helper::Jacobian; + using typename Helper::TangentVector; + static constexpr int dimension = Helper::dimension; + + public: + /// @name Standard Constructors + /// @{ + + /// Default constructor yields a zero-length placeholder identity + PowerLieGroup() = default; + + /// Construct a runtime-sized identity element + explicit PowerLieGroup(size_t count) : Base(count, traits::Identity()) {} + + /// Construct from vector of group elements + PowerLieGroup(const Base& elements) : Base(elements) {} + + /// Construct from initializer list + PowerLieGroup(const std::initializer_list& elements) : Base(elements) {} + + /// @} + /// @name Group Operations + /// @{ + + /// Identity element + static PowerLieGroup Identity() { return PowerLieGroup(); } + + /// @} + /// @name Manifold Operations + /// @{ + + /// Return manifold dimension + static constexpr int Dim() { return dimension; } + + /// @} + /// @name Lie Group Operations /// @{ - void print(const std::string& s = "") const { - std::cout << s << "PowerLieGroup" << std::endl; - for (size_t i = 0; i < N; ++i) { - traits::Print((*this)[i], " component[" + std::to_string(i) + "]"); - } - } - bool equals(const PowerLieGroup& other, double tol = 1e-9) const { - for (size_t i = 0; i < N; ++i) { - if (!traits::Equals((*this)[i], other[i], tol)) { - return false; - } - } - return true; - } /// @} }; @@ -552,7 +528,9 @@ struct traits> : internal::LieGroup> {}; /// Traits specialization for PowerLieGroup -template +template struct traits> : internal::LieGroup> {}; } // namespace gtsam + +#include diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index b81b19ac33..4b4673b716 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -29,7 +29,7 @@ struct VectorSpaceImpl { typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; typedef Eigen::Matrix Jacobian; - static int GetDimension(const Class&) { return N;} + static size_t GetDimension(const Class&) { return static_cast(N);} static TangentVector Local(const Class& origin, const Class& other, ChartJacobian H1 = {}, ChartJacobian H2 = {}) { @@ -107,10 +107,10 @@ struct VectorSpaceImpl { /// @{ typedef Eigen::VectorXd TangentVector; typedef OptionalJacobian ChartJacobian; - static int GetDimension(const Class& m) { return m.dim();} + static size_t GetDimension(const Class& m) { return m.dim();} static Eigen::MatrixXd Eye(const Class& m) { - int dim = GetDimension(m); + size_t dim = GetDimension(m); return Eigen::MatrixXd::Identity(dim, dim); } @@ -141,7 +141,7 @@ struct VectorSpaceImpl { static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) { Class result(v); - if (Hv) *Hv = Eye(v); + if (Hv) *Hv = Eye(result); return result; } @@ -395,12 +395,12 @@ struct DynamicTraits { typedef OptionalJacobian ChartJacobian; typedef Dynamic ManifoldType; - static int GetDimension(const Dynamic& m) { - return m.rows() * m.cols(); + static size_t GetDimension(const Dynamic& m) { + return static_cast(m.rows() * m.cols()); } static Jacobian Eye(const Dynamic& m) { - int dim = GetDimension(m); + size_t dim = GetDimension(m); return Eigen::Matrix::Identity(dim, dim); } @@ -432,9 +432,23 @@ struct DynamicTraits { return result; } - static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = {}) { - static_cast(H); - throw std::runtime_error("Expmap not defined for dynamic types"); + static Dynamic Expmap(const TangentVector& v, ChartJacobian H = {}) { + if constexpr (M == Eigen::Dynamic && N == Eigen::Dynamic) { + static_cast(v); + static_cast(H); + throw std::runtime_error("Expmap not defined for fully dynamic matrices"); + } else { + const int rows = (M == Eigen::Dynamic) ? v.size() / N : M; + const int cols = (N == Eigen::Dynamic) ? v.size() / M : N; + if (rows * cols != v.size()) { + throw std::invalid_argument( + "Dynamic Expmap tangent dimension does not match matrix shape"); + } + Dynamic result(rows, cols); + result = Eigen::Map(v.data(), rows, cols); + if (H) *H = Jacobian::Identity(v.size(), v.size()); + return result; + } } static Dynamic Inverse(const Dynamic& m, ChartJacobian H = {}) { diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 0e6bb895f4..6876b123c0 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -160,8 +160,7 @@ namespace gtsam { static_assert(IsManifold::value, "noiseModel::matchesDimension requires a manifold type."); if constexpr (traits::dimension == Eigen::Dynamic) { - return model.dim() == - static_cast(traits::GetDimension(measured)); + return model.dim() == traits::GetDimension(measured); } else { return model.dim() == static_cast(traits::dimension); } @@ -673,7 +672,7 @@ namespace gtsam { static_assert(IsManifold::value, "noiseModel::Unit::Create requires a manifold type."); if constexpr (traits::dimension == Eigen::Dynamic) { - return Create(static_cast(traits::GetDimension(measured))); + return Create(traits::GetDimension(measured)); } else { static const shared_ptr kDefault = Create(static_cast(traits::dimension)); diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h index b809622b94..af521cfc7d 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -73,7 +73,7 @@ class ManifoldEKF { n_ = traits::GetDimension(X0); if constexpr (Dim == Eigen::Dynamic) { // Validate dimensions of initial covariance P0. - if (P0.rows() != n_ || P0.cols() != n_) { + if (!isMatrixOfSize(P0, n_, n_)) { throw std::invalid_argument( "ManifoldEKF: Initial covariance P0 dimensions (" + std::to_string(P0.rows()) + "x" + std::to_string(P0.cols()) + @@ -97,7 +97,7 @@ class ManifoldEKF { const Covariance& covariance() const { return P_; } /// @return runtime dimension of the manifold. - int dimension() const { return n_; } + size_t dimension() const { return n_; } /** * Basic predict step: Updates state and covariance given the predicted next @@ -115,8 +115,7 @@ class ManifoldEKF { */ void predict(const M& X_next, const Jacobian& F, const Covariance& Q) { if constexpr (Dim == Eigen::Dynamic) { - if (F.rows() != n_ || F.cols() != n_ || Q.rows() != n_ || - Q.cols() != n_) { + if (!isMatrixOfSize(F, n_, n_) || !isMatrixOfSize(Q, n_, n_)) { throw std::invalid_argument( "ManifoldEKF::predict: Dynamic F/Q dimensions must match state " "dimension " + @@ -257,29 +256,37 @@ class ManifoldEKF { /// Validate inputs to update. void validateInputs(const gtsam::Vector& prediction, const Matrix& H, const gtsam::Vector& z, const Matrix& R) { - const int m = static_cast(prediction.size()); - if (static_cast(z.size()) != m) { + const size_t m = static_cast(prediction.size()); + if (static_cast(z.size()) != m) { throw std::invalid_argument( "ManifoldEKF::updateWithVector: prediction and z must have same " "length."); } - if (H.rows() != m || H.cols() != n_) { + if (!isMatrixOfSize(H, m, n_)) { throw std::invalid_argument( "ManifoldEKF::updateWithVector: H must be m x n where m = " "measurement size and n = state dimension."); } - if (R.rows() != m || R.cols() != m) { + if (!isMatrixOfSize(R, m, m)) { throw std::invalid_argument( "ManifoldEKF::updateWithVector: R must be m x m where m = " "measurement size."); } } + /// Check whether a matrix has the expected runtime dimensions. + template + static bool isMatrixOfSize(const MatrixType& matrix, size_t rows, + size_t cols) { + return static_cast(matrix.rows()) == rows && + static_cast(matrix.cols()) == cols; + } + protected: M X_; ///< Manifold state estimate. Covariance P_; ///< Covariance (Eigen::Matrix). Jacobian I_; ///< Identity matrix sized to the state dimension. - int n_; ///< Runtime tangent space dimension of M. + size_t n_; ///< Runtime tangent space dimension of M. private: // Detection helper: check if traits::Retract(x, v, Jacobian*) is valid. diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 3530d0e62c..12209fb925 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -676,7 +676,7 @@ virtual class ManifoldEKF { // Accessors M state() const; gtsam::Matrix covariance() const; - int dimension() const; + size_t dimension() const; // Predict with provided next state and Jacobian void predict(const M& X_next, gtsam::Matrix F, gtsam::Matrix Q); diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 072f40b58c..56f309f652 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -70,7 +70,7 @@ namespace gtsam { // Create a Jacobian Prior Factor directly P_initial. // Since x0 is set to the provided mean, the b vector in the prior will be zero // TODO(Frank): is there a reason why noiseModel is not simply P_initial? - int n = traits::GetDimension(x_initial); + size_t n = traits::GetDimension(x_initial); priorFactor_ = JacobianFactor::shared_ptr( new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n), noiseModel::Unit::Create(n))); diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 53f06af156..9e0e14d20d 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -66,8 +66,8 @@ class PoseTranslationPrior : public NoiseModelFactorN { Vector evaluateError(const Pose& pose, OptionalMatrixType H) const override { const Translation& newTrans = pose.translation(); const Rotation& R = pose.rotation(); - const int tDim = traits::GetDimension(newTrans); - const int xDim = traits::GetDimension(pose); + const size_t tDim = traits::GetDimension(newTrans); + const size_t xDim = traits::GetDimension(pose); if (H) { *H = Matrix::Zero(tDim, xDim); std::pair transInterval = POSE::translationInterval(); @@ -109,4 +109,3 @@ class PoseTranslationPrior : public NoiseModelFactorN { - diff --git a/gtsam_unstable/geometry/ABC.h b/gtsam_unstable/geometry/ABC.h index ca25764a59..9315a0faca 100644 --- a/gtsam_unstable/geometry/ABC.h +++ b/gtsam_unstable/geometry/ABC.h @@ -62,7 +62,7 @@ inline Vector6 toInputVector(const Vector3& w) { /// Bundle of calibration rotations modeled as a Lie group template -using Calibrations = PowerLieGroup; +using Calibrations = PowerLieGroup(N)>; //======================================================================== // State Manifold @@ -78,7 +78,7 @@ struct State { Vector3 b; // Gyroscope bias b Calibrations S; // Sensor calibrations S - static constexpr int dimension = 6 + 3 * N; + static constexpr int dimension = 6 + 3 * static_cast(N); using TangentVector = Eigen::Matrix; /// Constructor @@ -481,10 +481,4 @@ template struct traits> : public internal::Manifold> { }; -template -struct traits> : internal::LieGroup> {}; - -template -struct traits> : internal::LieGroup> {}; - } // namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testABC.cpp b/gtsam_unstable/geometry/tests/testABC.cpp index 6790c71c81..c2de1e5619 100644 --- a/gtsam_unstable/geometry/tests/testABC.cpp +++ b/gtsam_unstable/geometry/tests/testABC.cpp @@ -410,7 +410,7 @@ TEST(ABC, LiftEquivariance) { TEST(ABC, InputAction_stateMatrixA) { using namespace abc_input_action_example; - Matrix A_matrix = abc::stateMatrixA(psi_u, X_hat); + Matrix A_matrix = abc::stateMatrixA<2>(psi_u, X_hat); Matrix3 W0 = Rot3::Hat(psi_u(X_hat.inverse()).head<3>()); Matrix expected_A1 = Matrix::Zero(6, 6); @@ -441,7 +441,7 @@ TEST(ABC, ComputeErrorDynamicsMatrixMatchesLegacy) { // A_provided should now be the Manifold-space matrix // stateMatrixA returns the correct Manifold-space matrix (DimM x DimM) InputOrbit psi_u(u); - Matrix expected_A = abc::stateMatrixA(psi_u, X_hat); + Matrix expected_A = abc::stateMatrixA<2>(psi_u, X_hat); // A_computed is now computed on Manifold (D_act * D_lift) Matrix A_computed = filter.computeErrorDynamicsMatrix(psi_u); @@ -574,7 +574,7 @@ TEST(ABC, InputAction_stateTransitionMatchesLieGroupEKF) { Matrix Phi_expected = stateTransitionMatrix<2>(psi_u, X_hat, dt); Group::Jacobian ad_xi = adjointMap(xi); - Group::Jacobian Df = abc::stateMatrixA(psi_u, X_hat) + ad_xi; + Group::Jacobian Df = abc::stateMatrixA<2>(psi_u, X_hat) + ad_xi; Group::Jacobian P0 = Group::Jacobian::Identity(); LieGroupEKF ekf(X_hat, P0); @@ -598,7 +598,7 @@ TEST(ABC, InputAction_stateTransitionMatchesLieGroupEKF_K1) { double dt = 1e-4; - Group::Jacobian Df = abc::stateMatrixA(psi_u, X_hat) + adjointMap(xi); + Group::Jacobian Df = abc::stateMatrixA<2>(psi_u, X_hat) + adjointMap(xi); Group::Jacobian P0 = Group::Jacobian::Identity(); LieGroupEKF ekf(X_hat, P0); @@ -619,7 +619,7 @@ TEST(ABC, InputAction_inputMatrix) { using abc_examples::A1; using abc_examples::B1; - Matrix input_matrix = abc::inputMatrixB(X_hat); + Matrix input_matrix = abc::inputMatrixB<2>(X_hat); const Matrix3 A = A1.matrix(); Matrix expected_B1 = gtsam::diag({A, A}); @@ -805,7 +805,7 @@ TEST(ABC, EqFilter) { Matrix Sigma = I_6x6; double dt = 0.01; Matrix Q = abc::inputProcessNoise<2>(Sigma); - Matrix B = abc::inputMatrixB(g_0); + Matrix B = abc::inputMatrixB<2>(g_0); Matrix Qc = B * Q * B.transpose(); // manifold continuous-time covariance Lift lift_u(u2); InputOrbit psi_u(u2); @@ -853,12 +853,12 @@ TEST(ABC, EqFilter_BespokeDynamics) { Matrix Sigma = I_6x6; double dt = 0.01; Matrix Q = abc::inputProcessNoise<2>(Sigma); - Matrix B = abc::inputMatrixB(g_0); + Matrix B = abc::inputMatrixB<2>(g_0); Matrix Qc = B * Q * B.transpose(); Lift lift_u(u2); InputOrbit psi_u(u2); - Matrix A = abc::stateMatrixA(psi_u, g_0); + Matrix A = abc::stateMatrixA<2>(psi_u, g_0); filter.predictWithJacobian<2>(lift_u, A, Qc, dt); EXPECT(assert_equal(expected_predict, filter.groupEstimate(), 1e-4)); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index f309f2c421..073501acf7 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -658,7 +658,7 @@ struct traits { return (a - b).array().abs().maxCoeff() < tol; } static void Print(const MyType&, const string&) {} - static int GetDimension(const MyType&) { return dimension; } + static size_t GetDimension(const MyType&) { return dimension; } static MyType Retract(const MyType& a, const TangentVector& v, ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = Matrix3::Identity(); diff --git a/tests/testProductLieGroup.cpp b/tests/testProductLieGroup.cpp index 63a3e762df..78eb97a2dd 100644 --- a/tests/testProductLieGroup.cpp +++ b/tests/testProductLieGroup.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include @@ -31,9 +32,13 @@ using namespace gtsam; constexpr double kTol = 1e-9; using Product = ProductLieGroup; -constexpr size_t kPowerComponents = 2; +using ProductVR = ProductLieGroup; +using ProductVV = ProductLieGroup; +constexpr int kPowerComponents = 2; using Power = PowerLieGroup; using PowerTangent = Power::TangentVector; +using DynamicPower = PowerLieGroup; +using DynamicPowerTangent = DynamicPower::TangentVector; namespace gtsam { template <> @@ -66,9 +71,39 @@ struct traits : internal::LieGroupTraits { return true; } }; + +template <> +struct traits : internal::LieGroupTraits { + static void Print(const DynamicPower& m, const std::string& s = "") { + std::cout << s << "["; + for (size_t i = 0; i < m.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << "Pose2(" << m[i].x() << "," << m[i].y() << "," + << m[i].theta() << ")"; + } + std::cout << "]" << std::endl; + } + static bool Equals(const DynamicPower& m1, const DynamicPower& m2, + double tol = 1e-8) { + if (m1.size() != m2.size()) return false; + for (size_t i = 0; i < m1.size(); ++i) { + if (!m1[i].equals(m2[i], tol)) return false; + } + return true; + } +}; } // namespace gtsam namespace { +Vector makeVector(std::initializer_list values) { + Vector vector(static_cast(values.size())); + Eigen::Index index = 0; + for (double value : values) { + vector(index++) = value; + } + return vector; +} + Product composeProductProxy(const Product& A, const Product& B) { return A.compose(B); } @@ -83,6 +118,34 @@ Product expmapProductProxy(const Vector5& vec) { return Product::Expmap(vec); } Vector5 logmapProductProxy(const Product& p) { return Product::Logmap(p); } +ProductVR composeProductVRProxy(const ProductVR& A, const ProductVR& B) { + return A.compose(B); +} + +ProductVR betweenProductVRProxy(const ProductVR& A, const ProductVR& B) { + return A.between(B); +} + +ProductVR inverseProductVRProxy(const ProductVR& A) { return A.inverse(); } + +ProductVR expmapProductVRProxy(const Vector& vec) { + return ProductVR::Expmap(vec); +} + +Vector logmapProductVRProxy(const ProductVR& p) { return ProductVR::Logmap(p); } + +ProductVV composeProductVVProxy(const ProductVV& A, const ProductVV& B) { + return A.compose(B); +} + +ProductVV betweenProductVVProxy(const ProductVV& A, const ProductVV& B) { + return A.between(B); +} + +ProductVV inverseProductVVProxy(const ProductVV& A) { return A.inverse(); } + +Vector logmapProductVVProxy(const ProductVV& p) { return ProductVV::Logmap(p); } + Power composePowerProxy(const Power& A, const Power& B) { return A.compose(B); } Power betweenPowerProxy(const Power& A, const Power& B) { return A.between(B); } @@ -92,6 +155,28 @@ Power inversePowerProxy(const Power& A) { return A.inverse(); } Power powerExpmapProxy(const PowerTangent& vec) { return Power::Expmap(vec); } PowerTangent powerLogmapProxy(const Power& p) { return Power::Logmap(p); } + +DynamicPower composeDynamicPowerProxy(const DynamicPower& A, + const DynamicPower& B) { + return A.compose(B); +} + +DynamicPower betweenDynamicPowerProxy(const DynamicPower& A, + const DynamicPower& B) { + return A.between(B); +} + +DynamicPower inverseDynamicPowerProxy(const DynamicPower& A) { + return A.inverse(); +} + +DynamicPower dynamicPowerExpmapProxy(const DynamicPowerTangent& vec) { + return DynamicPower::Expmap(vec); +} + +DynamicPowerTangent dynamicPowerLogmapProxy(const DynamicPower& p) { + return DynamicPower::Logmap(p); +} } // namespace /* ************************************************************************* */ @@ -178,6 +263,224 @@ TEST(testProduct, AdjointMap) { EXPECT(assert_equal(expected, actual, kTol)); } +/* ************************************************************************* */ +TEST(Lie, ProductLieGroupDynamicVectorRot3) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + + ProductVR pair1(Vector::Zero(2), Rot3::Identity()); + Vector d = makeVector({1.0, 2.0, 0.1, 0.2, 0.3}); + ProductVR expected(makeVector({1.0, 2.0}), + Rot3::Expmap(Vector3(0.1, 0.2, 0.3))); + ProductVR pair2 = pair1.expmap(d); + + EXPECT_LONGS_EQUAL(Eigen::Dynamic, ProductVR::dimension); + EXPECT_LONGS_EQUAL(5, pair1.dim()); + EXPECT(assert_equal(expected, pair2, kTol)); + EXPECT(assert_equal(d, pair1.logmap(pair2), kTol)); + + const Matrix adj = pair1.AdjointMap(); + EXPECT_LONGS_EQUAL(5, adj.rows()); + EXPECT_LONGS_EQUAL(5, adj.cols()); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVR, compose) { + ProductVR state1(makeVector({1.0, 2.0}), Rot3::RzRyRx(0.1, 0.2, 0.3)); + ProductVR state2(makeVector({-0.5, 4.0}), Rot3::RzRyRx(-0.3, 0.1, -0.2)); + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21( + composeProductVRProxy, state1, state2); + Matrix numericH2 = numericalDerivative22( + composeProductVRProxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, kTol)); + EXPECT(assert_equal(numericH2, actH2, kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVR, between) { + ProductVR state1(makeVector({1.0, 2.0}), Rot3::RzRyRx(0.1, 0.2, 0.3)); + ProductVR state2(makeVector({-0.5, 4.0}), Rot3::RzRyRx(-0.3, 0.1, -0.2)); + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21( + betweenProductVRProxy, state1, state2); + Matrix numericH2 = numericalDerivative22( + betweenProductVRProxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, kTol)); + EXPECT(assert_equal(numericH2, actH2, kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVR, inverse) { + ProductVR state(makeVector({1.0, 2.0}), Rot3::RzRyRx(0.1, 0.2, 0.3)); + + Matrix actH; + state.inverse(actH); + Matrix numericH = numericalDerivative11( + inverseProductVRProxy, state); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVR, Expmap) { + Vector vec = makeVector({1.0, 2.0, 0.1, 0.2, 0.3}); + + Matrix actH; + ProductVR::Expmap(vec, actH); + Matrix numericH = + numericalDerivative11(expmapProductVRProxy, vec); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVR, Logmap) { + ProductVR state(makeVector({1.0, 2.0}), Rot3::RzRyRx(0.1, 0.2, 0.3)); + + Matrix actH; + ProductVR::Logmap(state, actH); + Matrix numericH = + numericalDerivative11(logmapProductVRProxy, state); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVR, AdjointMap) { + ProductVR state(makeVector({1.0, 2.0}), Rot3::RzRyRx(0.1, 0.2, 0.3)); + const Matrix actual = state.AdjointMap(); + + Matrix expected = Matrix::Zero(5, 5); + expected.topLeftCorner(2, 2) = Matrix::Identity(2, 2); + expected.bottomRightCorner(3, 3) = state.second.AdjointMap(); + + EXPECT(assert_equal(expected, actual, kTol)); +} + +/* ************************************************************************* */ +TEST(Lie, ProductLieGroupDynamicVectorVector) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + + ProductVV pair1(makeVector({0.0, 0.0}), makeVector({0.0, 0.0, 0.0})); + Vector d = makeVector({1.0, 2.0, 3.0, 4.0, 5.0}); + ProductVV expected(makeVector({1.0, 2.0}), makeVector({3.0, 4.0, 5.0})); + ProductVV pair2 = ProductVV::Expmap(Vector(d.head(2)), Vector(d.tail(3))); + + EXPECT_LONGS_EQUAL(Eigen::Dynamic, ProductVV::dimension); + EXPECT_LONGS_EQUAL(5, pair1.dim()); + EXPECT(assert_equal(expected, pair2, kTol)); + EXPECT(assert_equal(d, pair1.logmap(pair2), kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVV, compose) { + ProductVV state1(makeVector({1.0, 2.0}), makeVector({3.0, 4.0, 5.0})); + ProductVV state2(makeVector({-0.5, 4.0}), makeVector({-1.0, 2.0, 1.5})); + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21( + composeProductVVProxy, state1, state2); + Matrix numericH2 = numericalDerivative22( + composeProductVVProxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, kTol)); + EXPECT(assert_equal(numericH2, actH2, kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVV, between) { + ProductVV state1(makeVector({1.0, 2.0}), makeVector({3.0, 4.0, 5.0})); + ProductVV state2(makeVector({-0.5, 4.0}), makeVector({-1.0, 2.0, 1.5})); + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21( + betweenProductVVProxy, state1, state2); + Matrix numericH2 = numericalDerivative22( + betweenProductVVProxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, kTol)); + EXPECT(assert_equal(numericH2, actH2, kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVV, inverse) { + ProductVV state(makeVector({1.0, 2.0}), makeVector({3.0, 4.0, 5.0})); + + Matrix actH; + state.inverse(actH); + Matrix numericH = numericalDerivative11( + inverseProductVVProxy, state); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVV, retractAndLocalCoordinates) { + ProductVV state(makeVector({1.0, 2.0}), makeVector({3.0, 4.0, 5.0})); + Vector delta = makeVector({0.1, -0.2, 0.3, -0.4, 0.5}); + + ProductVV updated = state.retract(delta); + EXPECT(assert_equal(delta, state.localCoordinates(updated), kTol)); + EXPECT(assert_equal(updated, + state.compose(ProductVV::Expmap(Vector(delta.head(2)), + Vector(delta.tail(3)))), + kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVV, Expmap) { + Vector vec = makeVector({1.0, 2.0, 3.0, 4.0, 5.0}); + ProductVV expected(makeVector({1.0, 2.0}), makeVector({3.0, 4.0, 5.0})); + + Matrix actH1, actH2; + ProductVV actual = + ProductVV::Expmap(Vector(vec.head(2)), Vector(vec.tail(3)), actH1, actH2); + Matrix expectedH1 = Matrix::Zero(5, 2); + Matrix expectedH2 = Matrix::Zero(5, 3); + expectedH1.block(0, 0, 2, 2).setIdentity(); + expectedH2.block(2, 0, 3, 3).setIdentity(); + EXPECT(assert_equal(expected, actual, kTol)); + EXPECT(assert_equal(vec, ProductVV::Logmap(actual), kTol)); + EXPECT(assert_equal(expectedH1, actH1, kTol)); + EXPECT(assert_equal(expectedH2, actH2, kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVV, Logmap) { + ProductVV state(makeVector({1.0, 2.0}), makeVector({3.0, 4.0, 5.0})); + + Matrix actH; + ProductVV::Logmap(state, actH); + Matrix numericH = + numericalDerivative11(logmapProductVVProxy, state); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVV, AdjointMap) { + ProductVV state(makeVector({1.0, 2.0}), makeVector({3.0, 4.0, 5.0})); + const Matrix actual = state.AdjointMap(); + const Matrix expected = Matrix::Identity(5, 5); + + EXPECT(assert_equal(expected, actual, kTol)); +} + +/* ************************************************************************* */ +TEST(testProductDynamicVV, Exceptions) { + ProductVV state1(makeVector({1.0, 2.0}), makeVector({3.0, 4.0, 5.0})); + ProductVV state2(makeVector({1.0, 2.0, 3.0}), makeVector({4.0, 5.0})); + Vector vec = makeVector({1.0, 2.0, 3.0, 4.0, 5.0}); + + CHECK_EXCEPTION(ProductVV::Expmap(vec), std::invalid_argument); + CHECK_EXCEPTION(state1.compose(state2), std::invalid_argument); + CHECK_EXCEPTION(state1.between(state2), std::invalid_argument); + CHECK_EXCEPTION(state1.localCoordinates(state2), std::invalid_argument); +} + /* ************************************************************************* */ Product interpolate_proxy(const Product& x, const Product& y, double t) { return interpolate(x, y, t); @@ -313,6 +616,142 @@ TEST(testPower, AdjointMap) { EXPECT(assert_equal(expected, actual, kTol)); } +/* ************************************************************************* */ +TEST(Lie, PowerLieGroupDynamicCount) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + + DynamicPower identity; + EXPECT_LONGS_EQUAL(Eigen::Dynamic, DynamicPower::dimension); + EXPECT_LONGS_EQUAL(0, identity.size()); + EXPECT_LONGS_EQUAL(0, identity.dim()); + EXPECT_LONGS_EQUAL(0, DynamicPower::Identity().size()); + EXPECT(assert_equal(Vector::Zero(0), DynamicPower::Logmap(identity), kTol)); + const Matrix emptyAdj = identity.AdjointMap(); + EXPECT_LONGS_EQUAL(0, emptyAdj.rows()); + EXPECT_LONGS_EQUAL(0, emptyAdj.cols()); + + DynamicPower sizedIdentity(2); + EXPECT_LONGS_EQUAL(2, sizedIdentity.size()); + EXPECT(assert_equal(Pose2(), sizedIdentity[0], kTol)); + EXPECT(assert_equal(Pose2(), sizedIdentity[1], kTol)); + + DynamicPowerTangent xi = makeVector({0.1, 0.2, 0.3, 0.4, 0.5, 0.6}); + DynamicPower expected( + {Pose2::Expmap(xi.head<3>()), Pose2::Expmap(xi.tail<3>())}); + DynamicPower actual = DynamicPower::Expmap(xi); + EXPECT(assert_equal(expected, actual, kTol)); + EXPECT(assert_equal(xi, DynamicPower::Logmap(actual), kTol)); + EXPECT_LONGS_EQUAL(6, actual.dim()); +} + +/* ************************************************************************* */ +TEST(testPowerDynamic, compose) { + DynamicPower state1({Pose2(1, 2, 3), Pose2(4, 5, 6)}); + DynamicPower state2({Pose2(-0.5, 4, -1.0), Pose2(2.0, -3.0, 0.25)}); + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = + numericalDerivative21( + composeDynamicPowerProxy, state1, state2); + Matrix numericH2 = + numericalDerivative22( + composeDynamicPowerProxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, kTol)); + EXPECT(assert_equal(numericH2, actH2, kTol)); +} + +/* ************************************************************************* */ +TEST(testPowerDynamic, between) { + DynamicPower state1({Pose2(1, 2, 3), Pose2(4, 5, 6)}); + DynamicPower state2({Pose2(-0.5, 4, -1.0), Pose2(2.0, -3.0, 0.25)}); + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = + numericalDerivative21( + betweenDynamicPowerProxy, state1, state2); + Matrix numericH2 = + numericalDerivative22( + betweenDynamicPowerProxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, kTol)); + EXPECT(assert_equal(numericH2, actH2, kTol)); +} + +/* ************************************************************************* */ +TEST(testPowerDynamic, inverse) { + DynamicPower state({Pose2(1, 2, 3), Pose2(4, 5, 6)}); + + Matrix actH; + state.inverse(actH); + Matrix numericH = numericalDerivative11( + inverseDynamicPowerProxy, state); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testPowerDynamic, retractAndLocalCoordinates) { + DynamicPower state({Pose2(1, 2, 3), Pose2(4, 5, 6)}); + DynamicPowerTangent delta = makeVector({0.1, 0.2, 0.3, -0.2, 0.1, -0.1}); + + DynamicPower updated = state.retract(delta); + DynamicPower expected( + {state[0].retract(delta.head<3>()), state[1].retract(delta.tail<3>())}); + EXPECT(assert_equal(expected, updated, kTol)); + EXPECT(assert_equal(delta, state.localCoordinates(updated), kTol)); +} + +/* ************************************************************************* */ +TEST(testPowerDynamic, Expmap) { + DynamicPowerTangent vec = makeVector({0.1, 0.2, 0.3, 0.4, 0.5, 0.6}); + DynamicPower expected( + {Pose2::Expmap(vec.head<3>()), Pose2::Expmap(vec.tail<3>())}); + + Matrix actH; + DynamicPower actual = DynamicPower::Expmap(vec, actH); + Matrix numericH = numericalDerivative11( + dynamicPowerExpmapProxy, vec); + EXPECT(assert_equal(expected, actual, kTol)); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testPowerDynamic, Logmap) { + DynamicPower state({Pose2(1, 2, 3), Pose2(4, 5, 6)}); + + Matrix actH; + DynamicPower::Logmap(state, actH); + Matrix numericH = numericalDerivative11( + dynamicPowerLogmapProxy, state); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testPowerDynamic, AdjointMap) { + DynamicPower state({Pose2(1, 2, 3), Pose2(4, 5, 6)}); + const Matrix actual = state.AdjointMap(); + + Matrix expected = Matrix::Zero(6, 6); + expected.block<3, 3>(0, 0) = state[0].AdjointMap(); + expected.block<3, 3>(3, 3) = state[1].AdjointMap(); + + EXPECT(assert_equal(expected, actual, kTol)); +} + +/* ************************************************************************* */ +TEST(testPowerDynamic, Exceptions) { + DynamicPower state1({Pose2(1, 2, 3), Pose2(4, 5, 6)}); + DynamicPower state2({Pose2(1, 2, 3)}); + CHECK_EXCEPTION(state1.compose(state2), std::invalid_argument); + CHECK_EXCEPTION(state1.between(state2), std::invalid_argument); + CHECK_EXCEPTION(state1.localCoordinates(state2), std::invalid_argument); + + DynamicPowerTangent vec = makeVector({0.1, 0.2, 0.3, 0.4, 0.5}); + CHECK_EXCEPTION(DynamicPower::Expmap(vec), std::invalid_argument); +} + //****************************************************************************** int main() { TestResult tr;