From 72734d184b28953a14bf150ea13450fad5bba218 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Jun 2023 12:12:48 -0400 Subject: [PATCH 1/4] overload += operator in Ordering --- gtsam/inference/Ordering.cpp | 12 ++++++++++++ gtsam/inference/Ordering.h | 20 +++++++------------- gtsam/inference/tests/testOrdering.cpp | 14 ++++++++++++++ 3 files changed, 33 insertions(+), 13 deletions(-) diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 2956c7575c..cb2ca752d4 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -281,6 +281,18 @@ void Ordering::print(const std::string& str, cout.flush(); } +/* ************************************************************************* */ +Ordering::This& Ordering::operator+=(Key key) { + this->push_back(key); + return *this; +} + +/* ************************************************************************* */ +Ordering::This& Ordering::operator,(Key key) { + this->push_back(key); + return *this; +} + /* ************************************************************************* */ Ordering::This& Ordering::operator+=(KeyVector& keys) { this->insert(this->end(), keys.begin(), keys.end()); diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 884a93f0d8..86d44e0721 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -25,10 +25,6 @@ #include #include -#ifdef GTSAM_USE_BOOST_FEATURES -#include -#endif - #include #include @@ -61,15 +57,13 @@ class Ordering: public KeyVector { Base(keys.begin(), keys.end()) { } -#ifdef GTSAM_USE_BOOST_FEATURES - /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling - /// push_back. - boost::assign::list_inserter > operator+=( - Key key) { - return boost::assign::make_list_inserter( - boost::assign_detail::call_push_back(*this))(key); - } -#endif + /// Add new variables to the ordering as + /// `ordering += key1, key2, ...`. + This& operator+=(Key key); + + /// Overloading the comma operator allows for chaining appends + // e.g. keys += key1, key2 + This& operator,(Key key); /** * @brief Append new keys to the ordering as `ordering += keys`. diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 761c330b46..328d383d86 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -196,6 +196,20 @@ TEST(Ordering, csr_format_3) { EXPECT(adjExpected == adjAcutal); } +/* ************************************************************************* */ +TEST(Ordering, AppendKey) { + using symbol_shorthand::X; + Ordering actual; + actual += X(0); + + Ordering expected1{X(0)}; + EXPECT(assert_equal(expected1, actual)); + + actual += X(1), X(2), X(3); + Ordering expected2{X(0), X(1), X(2), X(3)}; + EXPECT(assert_equal(expected2, actual)); +} + /* ************************************************************************* */ TEST(Ordering, AppendVector) { using symbol_shorthand::X; From 9abf6c39c2fae08f574377e99bfdc6736c007967 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Jun 2023 12:13:16 -0400 Subject: [PATCH 2/4] overload += operator in FactorGraph --- gtsam/inference/FactorGraph.h | 77 +++++-------------- .../linear/tests/testGaussianFactorGraph.cpp | 22 ++++++ 2 files changed, 42 insertions(+), 57 deletions(-) diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index f1dc37c376..327fca49a6 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -29,10 +29,6 @@ #include // for Eigen::aligned_allocator -#ifdef GTSAM_USE_BOOST_FEATURES -#include -#endif - #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include #include @@ -53,45 +49,6 @@ class BayesTree; class HybridValues; -/** Helper */ -template -class CRefCallPushBack { - C& obj; - - public: - explicit CRefCallPushBack(C& obj) : obj(obj) {} - template - void operator()(const A& a) { - obj.push_back(a); - } -}; - -/** Helper */ -template -class RefCallPushBack { - C& obj; - - public: - explicit RefCallPushBack(C& obj) : obj(obj) {} - template - void operator()(A& a) { - obj.push_back(a); - } -}; - -/** Helper */ -template -class CRefCallAddCopy { - C& obj; - - public: - explicit CRefCallAddCopy(C& obj) : obj(obj) {} - template - void operator()(const A& a) { - obj.addCopy(a); - } -}; - /** * A factor graph is a bipartite graph with factor nodes connected to variable * nodes. In this class, however, only factor nodes are kept around. @@ -215,17 +172,26 @@ class FactorGraph { push_back(factor); } -#ifdef GTSAM_USE_BOOST_FEATURES - /// `+=` works well with boost::assign list inserter. + /// Append factor to factor graph template - typename std::enable_if< - std::is_base_of::value, - boost::assign::list_inserter>>::type + typename std::enable_if::value, + This>::type& operator+=(std::shared_ptr factor) { - return boost::assign::make_list_inserter(RefCallPushBack(*this))( - factor); + push_back(factor); + return *this; + } + + /** + * @brief Overload comma operator to allow for append chaining. + * + * E.g. fg += factor1, factor2, ... + */ + template + typename std::enable_if::value, This>::type& operator,( + std::shared_ptr factor) { + push_back(factor); + return *this; } -#endif /// @} /// @name Adding via iterators @@ -276,18 +242,15 @@ class FactorGraph { push_back(factorOrContainer); } -#ifdef GTSAM_USE_BOOST_FEATURES /** * Add a factor or container of factors, including STL collections, * BayesTrees, etc. */ template - boost::assign::list_inserter> operator+=( - const FACTOR_OR_CONTAINER& factorOrContainer) { - return boost::assign::make_list_inserter(CRefCallPushBack(*this))( - factorOrContainer); + This& operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) { + push_back(factorOrContainer); + return *this; } -#endif /// @} /// @name Specialized versions diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index e9e6262962..41ee9471ee 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -70,6 +70,28 @@ TEST(GaussianFactorGraph, initialization) { EQUALITY(expectedIJS, actualIJS); } +/* ************************************************************************* */ +TEST(GaussianFactorGraph, Append) { + // Create empty graph + GaussianFactorGraph fg; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + + auto f1 = + make_shared(0, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); + auto f2 = make_shared(0, -10 * I_2x2, 1, 10 * I_2x2, + Vector2(2.0, -1.0), unit2); + auto f3 = make_shared(0, -5 * I_2x2, 2, 5 * I_2x2, + Vector2(0.0, 1.0), unit2); + + fg += f1; + fg += f2; + EXPECT_LONGS_EQUAL(2, fg.size()); + + fg = GaussianFactorGraph(); + fg += f1, f2, f3; + EXPECT_LONGS_EQUAL(3, fg.size()); +} + /* ************************************************************************* */ TEST(GaussianFactorGraph, sparseJacobian) { // Create factor graph: From 0971c75a61381e8232f16690b7d5d910685d6ae8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Jun 2023 15:27:23 -0400 Subject: [PATCH 3/4] remove unnecessary headers --- gtsam/base/Group.h | 7 ------- gtsam/base/types.h | 7 +------ 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index e76a273da5..09534efc37 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -22,13 +22,6 @@ #include -#ifdef GTSAM_USE_BOOST_FEATURES -#include -#include -#include -#include -#endif - #include namespace gtsam { diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 3f1c1cd3d1..c734678d24 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -19,16 +19,11 @@ #pragma once +#include // for GTSAM_USE_TBB #include -#ifdef GTSAM_USE_BOOST_FEATURES -#include -#include -#endif -#include // for GTSAM_USE_TBB #include #include - #include #include From 9c0caf60bc6d48cc6d93a6cfa4194b9ed843fa1f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Jun 2023 16:39:08 -0400 Subject: [PATCH 4/4] add header to timing.cpp --- gtsam/base/timing.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 154a564db6..b435950669 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -19,9 +19,10 @@ #include #include +#include +#include #include #include -#include #include #include #include