From 2fe4c83680156dcbff95b6c2cabf78c3dac7b084 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 10:11:49 -0800 Subject: [PATCH 01/37] Make base class constructors available (including initializer lists). --- gtsam/base/FastSet.h | 10 +--------- gtsam/inference/Ordering.h | 10 +++------- 2 files changed, 4 insertions(+), 16 deletions(-) diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 906412f4d2..e93f056f67 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -56,15 +56,7 @@ class FastSet: public std::set, typedef std::set, typename internal::FastDefaultAllocator::type> Base; - /** Default constructor */ - FastSet() { - } - - /** Constructor from a range, passes through to base class */ - template - explicit FastSet(INPUTITERATOR first, INPUTITERATOR last) : - Base(first, last) { - } + using Base::Base; // Inherit the set constructors /** Constructor from a iterable container, passes through to base class */ template diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index c9c6a61765..e875ed961e 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -50,18 +50,14 @@ class Ordering: public KeyVector { Ordering() { } + using KeyVector::KeyVector; // Inherit the KeyVector's constructors + /// Create from a container template explicit Ordering(const KEYS& keys) : Base(keys.begin(), keys.end()) { } - /// Create an ordering using iterators over keys - template - Ordering(ITERATOR firstKey, ITERATOR lastKey) : - Base(firstKey, lastKey) { - } - /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling /// push_back. boost::assign::list_inserter > operator+=( @@ -195,7 +191,7 @@ class Ordering: public KeyVector { KeySet src = fg.keys(); KeyVector keys(src.begin(), src.end()); std::stable_sort(keys.begin(), keys.end()); - return Ordering(keys); + return Ordering(keys.begin(), keys.end()); } /// METIS Formatting function From c4fb76429909feef526f6d5d72463595b5462547 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 10:19:52 -0800 Subject: [PATCH 02/37] Removed boost headers in DecisionTree-inl.h that are no longer needed. --- gtsam/discrete/DecisionTree-inl.h | 11 ++--------- gtsam_unstable/discrete/Domain.cpp | 3 +-- gtsam_unstable/discrete/SingleValue.cpp | 3 +-- gtsam_unstable/discrete/tests/testSudoku.cpp | 6 +++--- 4 files changed, 7 insertions(+), 16 deletions(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index d01c918401..9f3d5e8f95 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -22,14 +22,10 @@ #include #include -#include #include #include -#include #include -#include -#include -#include + #include #include #include @@ -41,8 +37,6 @@ namespace gtsam { - using boost::assign::operator+=; - /****************************************************************************/ // Node /****************************************************************************/ @@ -535,8 +529,7 @@ namespace gtsam { template DecisionTree::DecisionTree(const L& label, const DecisionTree& f0, const DecisionTree& f1) { - std::vector functions; - functions += f0, f1; + const std::vector functions{f0, f1}; root_ = compose(functions.begin(), functions.end(), label); } diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index 7acc10cb4a..cf0da42e77 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -37,8 +37,7 @@ double Domain::operator()(const DiscreteValues& values) const { /* ************************************************************************* */ DecisionTreeFactor Domain::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(key(), cardinality_); + const DiscreteKeys keys{DiscreteKey(key(), cardinality_)}; vector table; for (size_t i1 = 0; i1 < cardinality_; ++i1) table.push_back(contains(i1)); DecisionTreeFactor converted(keys, table); diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 6dd81a7dc6..fc5fab83f6 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -29,8 +29,7 @@ double SingleValue::operator()(const DiscreteValues& values) const { /* ************************************************************************* */ DecisionTreeFactor SingleValue::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(keys_[0], cardinality_); + const DiscreteKeys keys{DiscreteKey(keys_[0], cardinality_)}; vector table; for (size_t i1 = 0; i1 < cardinality_; i1++) table.push_back(i1 == value_); DecisionTreeFactor converted(keys, table); diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index 9796c25f1a..a476112222 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -58,14 +58,14 @@ class Sudoku : public CSP { // add row constraints for (size_t i = 0; i < n; i++) { DiscreteKeys dkeys; - for (size_t j = 0; j < n; j++) dkeys += dkey(i, j); + for (size_t j = 0; j < n; j++) dkeys.push_back(dkey(i, j)); addAllDiff(dkeys); } // add col constraints for (size_t j = 0; j < n; j++) { DiscreteKeys dkeys; - for (size_t i = 0; i < n; i++) dkeys += dkey(i, j); + for (size_t i = 0; i < n; i++) dkeys.push_back(dkey(i, j)); addAllDiff(dkeys); } @@ -77,7 +77,7 @@ class Sudoku : public CSP { // Box I,J DiscreteKeys dkeys; for (size_t i = i0; i < i0 + N; i++) - for (size_t j = j0; j < j0 + N; j++) dkeys += dkey(i, j); + for (size_t j = j0; j < j0 + N; j++) dkeys.push_back(dkey(i, j)); addAllDiff(dkeys); j0 += N; } From d2f0cf5cc7149a2ee096021b6ea1288d47245348 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 11:02:26 -0800 Subject: [PATCH 03/37] Inherit constructors for CameraSets to switch to initializer lists. --- gtsam/geometry/CameraSet.h | 94 ++++++------ gtsam/geometry/tests/testTriangulation.cpp | 165 ++++++++------------- 2 files changed, 106 insertions(+), 153 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 143d4bc3c3..0fbb50f021 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -35,56 +35,56 @@ template class CameraSet : public std::vector > { protected: - - /** - * 2D measurement and noise model for each of the m views - * The order is kept the same as the keys that we use to create the factor. - */ - typedef typename CAMERA::Measurement Z; - typedef typename CAMERA::MeasurementVector ZVector; - - static const int D = traits::dimension; ///< Camera dimension - static const int ZDim = traits::dimension; ///< Measurement dimension - - /// Make a vector of re-projection errors - static Vector ErrorVector(const ZVector& predicted, - const ZVector& measured) { - - // Check size - size_t m = predicted.size(); - if (measured.size() != m) - throw std::runtime_error("CameraSet::errors: size mismatch"); - - // Project and fill error vector - Vector b(ZDim * m); - for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - Vector bi = traits::Local(measured[i], predicted[i]); - if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the right pixel is missing (nan) - bi(1) = 0; - } - b.segment(row) = bi; - } - return b; + using Base = std::vector>; + + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + typedef typename CAMERA::Measurement Z; + typedef typename CAMERA::MeasurementVector ZVector; + + static const int D = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + + /// Make a vector of re-projection errors + static Vector ErrorVector(const ZVector& predicted, const ZVector& measured) { + // Check size + size_t m = predicted.size(); + if (measured.size() != m) + throw std::runtime_error("CameraSet::errors: size mismatch"); + + // Project and fill error vector + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + Vector bi = traits::Local(measured[i], predicted[i]); + if (ZDim == 3 && std::isnan(bi(1))) { // if it is a stereo point and the + // right pixel is missing (nan) + bi(1) = 0; + } + b.segment(row) = bi; + } + return b; } public: - - /// Destructor - virtual ~CameraSet() = default; - - /// Definitions for blocks of F - using MatrixZD = Eigen::Matrix; - using FBlocks = std::vector>; - - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter useful for printing Symbols - */ - virtual void print(const std::string& s = "") const { - std::cout << s << "CameraSet, cameras = \n"; - for (size_t k = 0; k < this->size(); ++k) - this->at(k).print(s); + using Base::Base; // Inherit the vector constructors + + /// Destructor + virtual ~CameraSet() = default; + + /// Definitions for blocks of F + using MatrixZD = Eigen::Matrix; + using FBlocks = std::vector>; + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + virtual void print(const std::string& s = "") const { + std::cout << s << "CameraSet, cameras = \n"; + for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s); } /// equals diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index bc843ad75d..95397d5b39 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -30,12 +30,8 @@ #include #include -#include -#include - using namespace std; using namespace gtsam; -using namespace boost::assign; // Some common constants @@ -51,34 +47,34 @@ static const PinholeCamera kCamera1(kPose1, *kSharedCal); static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0)); static const PinholeCamera kCamera2(kPose2, *kSharedCal); -// landmark ~5 meters infront of camera +static const std::vector kPoses = {kPose1, kPose2}; + + +// landmark ~5 meters in front of camera static const Point3 kLandmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate static const Point2 kZ1 = kCamera1.project(kLandmark); static const Point2 kZ2 = kCamera2.project(kLandmark); +static const Point2Vector kMeasurements{kZ1, kZ2}; //****************************************************************************** // Simple test with a well-behaved two camera situation TEST(triangulation, twoPoses) { - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose2; - measurements += kZ1, kZ2; + Point2Vector measurements = kMeasurements; double rank_tol = 1e-9; // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // - triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, @@ -87,13 +83,13 @@ TEST(triangulation, twoPoses) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } @@ -102,7 +98,7 @@ TEST(triangulation, twoCamerasUsingLOST) { cameras.push_back(kCamera1); cameras.push_back(kCamera2); - Point2Vector measurements = {kZ1, kZ2}; + Point2Vector measurements = kMeasurements; SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4); double rank_tol = 1e-9; @@ -175,25 +171,21 @@ TEST(triangulation, twoPosesCal3DS2) { Point2 z1Distorted = camera1Distorted.project(kLandmark); Point2 z2Distorted = camera2Distorted.project(kLandmark); - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose2; - measurements += z1Distorted, z2Distorted; + Point2Vector measurements{z1Distorted, z2Distorted}; double rank_tol = 1e-9; // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -203,14 +195,14 @@ TEST(triangulation, twoPosesCal3DS2) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); } @@ -232,25 +224,21 @@ TEST(triangulation, twoPosesFisheye) { Point2 z1Distorted = camera1Distorted.project(kLandmark); Point2 z2Distorted = camera2Distorted.project(kLandmark); - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose2; - measurements += z1Distorted, z2Distorted; + Point2Vector measurements{z1Distorted, z2Distorted}; double rank_tol = 1e-9; // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -260,14 +248,14 @@ TEST(triangulation, twoPosesFisheye) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); } @@ -284,17 +272,13 @@ TEST(triangulation, twoPosesBundler) { Point2 z1 = camera1.project(kLandmark); Point2 z2 = camera2.project(kLandmark); - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose2; - measurements += z1, z2; + Point2Vector measurements{z1, z2}; bool optimize = true; double rank_tol = 1e-9; boost::optional actual = // - triangulatePoint3(poses, bundlerCal, measurements, rank_tol, + triangulatePoint3(kPoses, bundlerCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual, 1e-7)); @@ -303,19 +287,15 @@ TEST(triangulation, twoPosesBundler) { measurements.at(1) += Point2(-0.2, 0.3); boost::optional actual2 = // - triangulatePoint3(poses, bundlerCal, measurements, rank_tol, + triangulatePoint3(kPoses, bundlerCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3)); } //****************************************************************************** TEST(triangulation, fourPoses) { - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose2; - measurements += kZ1, kZ2; - + Pose3Vector poses = kPoses; + Point2Vector measurements = kMeasurements; boost::optional actual = triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); @@ -334,8 +314,8 @@ TEST(triangulation, fourPoses) { PinholeCamera camera3(pose3, *kSharedCal); Point2 z3 = camera3.project(kLandmark); - poses += pose3; - measurements += z3 + Point2(0.1, -0.1); + poses.push_back(pose3); + measurements.push_back(z3 + Point2(0.1, -0.1)); boost::optional triangulated_3cameras = // triangulatePoint3(poses, kSharedCal, measurements); @@ -353,8 +333,8 @@ TEST(triangulation, fourPoses) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); - poses += pose4; - measurements += Point2(400, 400); + poses.push_back(pose4); + measurements.emplace_back(400, 400); CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationCheiralityException); @@ -368,10 +348,8 @@ TEST(triangulation, threePoses_robustNoiseModel) { PinholeCamera camera3(pose3, *kSharedCal); Point2 z3 = camera3.project(kLandmark); - vector poses; - Point2Vector measurements; - poses += kPose1, kPose2, pose3; - measurements += kZ1, kZ2, z3; + const vector poses{kPose1, kPose2, pose3}; + Point2Vector measurements{kZ1, kZ2, z3}; // noise free, so should give exactly the landmark boost::optional actual = @@ -410,10 +388,9 @@ TEST(triangulation, fourPoses_robustNoiseModel) { PinholeCamera camera3(pose3, *kSharedCal); Point2 z3 = camera3.project(kLandmark); - vector poses; - Point2Vector measurements; - poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1 - measurements += kZ1, kZ1, kZ2, z3; + const vector poses{kPose1, kPose1, kPose2, pose3}; + // 2 measurements from pose 1: + Point2Vector measurements{kZ1, kZ1, kZ2, z3}; // noise free, so should give exactly the landmark boost::optional actual = @@ -463,11 +440,8 @@ TEST(triangulation, fourPoses_distinct_Ks) { Point2 z1 = camera1.project(kLandmark); Point2 z2 = camera2.project(kLandmark); - CameraSet> cameras; - Point2Vector measurements; - - cameras += camera1, camera2; - measurements += z1, z2; + CameraSet> cameras{camera1, camera2}; + Point2Vector measurements{z1, z2}; boost::optional actual = // triangulatePoint3(cameras, measurements); @@ -488,8 +462,8 @@ TEST(triangulation, fourPoses_distinct_Ks) { PinholeCamera camera3(pose3, K3); Point2 z3 = camera3.project(kLandmark); - cameras += camera3; - measurements += z3 + Point2(0.1, -0.1); + cameras.push_back(camera3); + measurements.push_back(z3 + Point2(0.1, -0.1)); boost::optional triangulated_3cameras = // triangulatePoint3(cameras, measurements); @@ -508,8 +482,8 @@ TEST(triangulation, fourPoses_distinct_Ks) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); - cameras += camera4; - measurements += Point2(400, 400); + cameras.push_back(camera4); + measurements.emplace_back(400, 400); CHECK_EXCEPTION(triangulatePoint3(cameras, measurements), TriangulationCheiralityException); #endif @@ -529,11 +503,8 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) { Point2 z1 = camera1.project(kLandmark); Point2 z2 = camera2.project(kLandmark); - CameraSet> cameras; - Point2Vector measurements; - - cameras += camera1, camera2; - measurements += z1, z2; + const CameraSet> cameras{camera1, camera2}; + const Point2Vector measurements{z1, z2}; boost::optional actual = // triangulatePoint3(cameras, measurements); @@ -554,11 +525,8 @@ TEST(triangulation, outliersAndFarLandmarks) { Point2 z1 = camera1.project(kLandmark); Point2 z2 = camera2.project(kLandmark); - CameraSet> cameras; - Point2Vector measurements; - - cameras += camera1, camera2; - measurements += z1, z2; + CameraSet> cameras{camera1, camera2}; + Point2Vector measurements{z1, z2}; double landmarkDistanceThreshold = 10; // landmark is closer than that TriangulationParameters params( @@ -582,8 +550,8 @@ TEST(triangulation, outliersAndFarLandmarks) { PinholeCamera camera3(pose3, K3); Point2 z3 = camera3.project(kLandmark); - cameras += camera3; - measurements += z3 + Point2(10, -10); + cameras.push_back(camera3); + measurements.push_back(z3 + Point2(10, -10)); landmarkDistanceThreshold = 10; // landmark is closer than that double outlierThreshold = 100; // loose, the outlier is going to pass @@ -608,11 +576,8 @@ TEST(triangulation, twoIdenticalPoses) { // 1. Project two landmarks into two cameras and triangulate Point2 z1 = camera1.project(kLandmark); - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose1; - measurements += z1, z1; + const vector poses{kPose1, kPose1}; + const Point2Vector measurements{z1, z1}; CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); @@ -623,22 +588,19 @@ TEST(triangulation, onePose) { // we expect this test to fail with a TriangulationUnderconstrainedException // because there's only one camera observation - vector poses; - Point2Vector measurements; - - poses += Pose3(); - measurements += Point2(0, 0); + const vector poses{Pose3()}; + const Point2Vector measurements {{0,0}}; CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); } //****************************************************************************** -TEST(triangulation, StereotriangulateNonlinear) { +TEST(triangulation, StereoTriangulateNonlinear) { auto stereoK = boost::make_shared(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612); - // two camera poses m1, m2 + // two camera kPoses m1, m2 Matrix4 m1, m2; m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, 0.592783835, -0.77156583, 0.230856632, 66.2186159, 0.116517574, -0.201470143, @@ -648,14 +610,12 @@ TEST(triangulation, StereotriangulateNonlinear) { 0.947083213, 0.131587097, 65.843136, -0.0206094928, 0.131334858, -0.991123524, -4.3525033, 0, 0, 0, 1; - typedef CameraSet Cameras; - Cameras cameras; - cameras.push_back(StereoCamera(Pose3(m1), stereoK)); - cameras.push_back(StereoCamera(Pose3(m2), stereoK)); + typedef CameraSet StereoCameras; + const StereoCameras cameras{{Pose3(m1), stereoK}, {Pose3(m2), stereoK}}; StereoPoint2Vector measurements; - measurements += StereoPoint2(226.936, 175.212, 424.469); - measurements += StereoPoint2(339.571, 285.547, 669.973); + measurements.push_back(StereoPoint2(226.936, 175.212, 424.469)); + measurements.push_back(StereoPoint2(339.571, 285.547, 669.973)); Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191 @@ -741,8 +701,6 @@ TEST(triangulation, StereotriangulateNonlinear) { //****************************************************************************** // Simple test with a well-behaved two camera situation TEST(triangulation, twoPoses_sphericalCamera) { - vector poses; - std::vector measurements; // Project landmark into two cameras and triangulate SphericalCamera cam1(kPose1); @@ -750,8 +708,7 @@ TEST(triangulation, twoPoses_sphericalCamera) { Unit3 u1 = cam1.project(kLandmark); Unit3 u2 = cam2.project(kLandmark); - poses += kPose1, kPose2; - measurements += u1, u2; + std::vector measurements{u1, u2}; CameraSet cameras; cameras.push_back(cam1); @@ -803,9 +760,6 @@ TEST(triangulation, twoPoses_sphericalCamera) { //****************************************************************************** TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { - vector poses; - std::vector measurements; - // Project landmark into two cameras and triangulate Pose3 poseA = Pose3( Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), @@ -825,8 +779,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2, 1e-7)); // behind and to the right of PoseB - poses += kPose1, kPose2; - measurements += u1, u2; + const std::vector measurements{u1, u2}; CameraSet cameras; cameras.push_back(cam1); From 017cd8f8a2c6997b49d48412753d733e3e9fbfe9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 15:43:17 -0800 Subject: [PATCH 04/37] Got rid of boost::assign in many tests --- gtsam/base/tests/testDSFMap.cpp | 45 +++++--------- gtsam/base/tests/testDSFVector.cpp | 60 +++++++++---------- gtsam/base/tests/testFastContainers.cpp | 6 +- gtsam/base/tests/testSymmetricBlockMatrix.cpp | 24 ++++---- gtsam/base/tests/testTreeTraversal.cpp | 17 ++---- gtsam/base/tests/testVerticalBlockMatrix.cpp | 10 ++-- .../tests/testAlgebraicDecisionTree.cpp | 22 +++---- gtsam/discrete/tests/testDecisionTree.cpp | 19 +++--- .../tests/testDiscreteConditional.cpp | 8 +-- .../discrete/tests/testDiscreteMarginals.cpp | 3 +- gtsam/geometry/tests/testOrientedPlane3.cpp | 3 - gtsam/geometry/tests/testPose2.cpp | 9 +-- gtsam/geometry/tests/testPose3.cpp | 20 +++---- gtsam/geometry/tests/testUnit3.cpp | 6 +- tests/testGaussianJunctionTreeB.cpp | 9 +-- 15 files changed, 102 insertions(+), 159 deletions(-) diff --git a/gtsam/base/tests/testDSFMap.cpp b/gtsam/base/tests/testDSFMap.cpp index 816498ce85..2e4b4f146b 100644 --- a/gtsam/base/tests/testDSFMap.cpp +++ b/gtsam/base/tests/testDSFMap.cpp @@ -16,17 +16,14 @@ * @brief unit tests for DSFMap */ -#include - -#include -#include -using namespace boost::assign; - #include +#include #include +#include +#include +#include -using namespace std; using namespace gtsam; /* ************************************************************************* */ @@ -65,9 +62,8 @@ TEST(DSFMap, merge3) { TEST(DSFMap, mergePairwiseMatches) { // Create some "matches" - typedef pair Match; - list matches; - matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); + typedef std::pair Match; + const std::list matches{{1, 2}, {2, 3}, {4, 5}, {4, 6}}; // Merge matches DSFMap dsf; @@ -86,18 +82,17 @@ TEST(DSFMap, mergePairwiseMatches) { TEST(DSFMap, mergePairwiseMatches2) { // Create some measurements with image index and feature index - typedef pair Measurement; + typedef std::pair Measurement; Measurement m11(1,1),m12(1,2),m14(1,4); // in image 1 Measurement m22(2,2),m23(2,3),m25(2,5),m26(2,6); // in image 2 // Add them all - list measurements; - measurements += m11,m12,m14, m22,m23,m25,m26; + const std::list measurements{m11, m12, m14, m22, m23, m25, m26}; // Create some "matches" - typedef pair Match; - list matches; - matches += Match(m11,m22), Match(m12,m23), Match(m14,m25), Match(m14,m26); + typedef std::pair Match; + const std::list matches{ + {m11, m22}, {m12, m23}, {m14, m25}, {m14, m26}}; // Merge matches DSFMap dsf; @@ -114,26 +109,16 @@ TEST(DSFMap, mergePairwiseMatches2) { /* ************************************************************************* */ TEST(DSFMap, sets){ // Create some "matches" - typedef pair Match; - list matches; - matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); + typedef const std::pair Match; + const std::list matches{{1, 2}, {2, 3}, {4, 5}, {4, 6}}; // Merge matches DSFMap dsf; for(const Match& m: matches) dsf.merge(m.first,m.second); - map > sets = dsf.sets(); - set s1, s2; - s1 += 1,2,3; - s2 += 4,5,6; - - /*for(key_pair st: sets){ - cout << "Set " << st.first << " :{"; - for(const size_t s: st.second) - cout << s << ", "; - cout << "}" << endl; - }*/ + std::map > sets = dsf.sets(); + const std::set s1{1, 2, 3}, s2{4, 5, 6}; EXPECT(s1 == sets[1]); EXPECT(s2 == sets[4]); diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp index 88cea8c010..b50ddde339 100644 --- a/gtsam/base/tests/testDSFVector.cpp +++ b/gtsam/base/tests/testDSFVector.cpp @@ -21,14 +21,15 @@ #include #include -#include -#include -#include -using namespace boost::assign; #include +#include +#include +#include -using namespace std; +using std::pair; +using std::map; +using std::vector; using namespace gtsam; /* ************************************************************************* */ @@ -64,8 +65,8 @@ TEST(DSFBase, mergePairwiseMatches) { // Create some "matches" typedef pair Match; - vector matches; - matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); + const vector matches{Match(1, 2), Match(2, 3), Match(4, 5), + Match(4, 6)}; // Merge matches DSFBase dsf(7); // We allow for keys 0..6 @@ -85,7 +86,7 @@ TEST(DSFBase, mergePairwiseMatches) { /* ************************************************************************* */ TEST(DSFVector, merge2) { boost::shared_ptr v = boost::make_shared(5); - std::vector keys; keys += 1, 3; + const std::vector keys {1, 3}; DSFVector dsf(v, keys); dsf.merge(1,3); EXPECT(dsf.find(1) == dsf.find(3)); @@ -95,10 +96,10 @@ TEST(DSFVector, merge2) { TEST(DSFVector, sets) { DSFVector dsf(2); dsf.merge(0,1); - map > sets = dsf.sets(); + map > sets = dsf.sets(); LONGS_EQUAL(1, sets.size()); - set expected; expected += 0, 1; + const std::set expected{0, 1}; EXPECT(expected == sets[dsf.find(0)]); } @@ -109,7 +110,7 @@ TEST(DSFVector, arrays) { map > arrays = dsf.arrays(); LONGS_EQUAL(1, arrays.size()); - vector expected; expected += 0, 1; + const vector expected{0, 1}; EXPECT(expected == arrays[dsf.find(0)]); } @@ -118,10 +119,10 @@ TEST(DSFVector, sets2) { DSFVector dsf(3); dsf.merge(0,1); dsf.merge(1,2); - map > sets = dsf.sets(); + map > sets = dsf.sets(); LONGS_EQUAL(1, sets.size()); - set expected; expected += 0, 1, 2; + const std::set expected{0, 1, 2}; EXPECT(expected == sets[dsf.find(0)]); } @@ -133,7 +134,7 @@ TEST(DSFVector, arrays2) { map > arrays = dsf.arrays(); LONGS_EQUAL(1, arrays.size()); - vector expected; expected += 0, 1, 2; + const vector expected{0, 1, 2}; EXPECT(expected == arrays[dsf.find(0)]); } @@ -141,10 +142,10 @@ TEST(DSFVector, arrays2) { TEST(DSFVector, sets3) { DSFVector dsf(3); dsf.merge(0,1); - map > sets = dsf.sets(); + map > sets = dsf.sets(); LONGS_EQUAL(2, sets.size()); - set expected; expected += 0, 1; + const std::set expected{0, 1}; EXPECT(expected == sets[dsf.find(0)]); } @@ -155,7 +156,7 @@ TEST(DSFVector, arrays3) { map > arrays = dsf.arrays(); LONGS_EQUAL(2, arrays.size()); - vector expected; expected += 0, 1; + const vector expected{0, 1}; EXPECT(expected == arrays[dsf.find(0)]); } @@ -163,10 +164,10 @@ TEST(DSFVector, arrays3) { TEST(DSFVector, set) { DSFVector dsf(3); dsf.merge(0,1); - set set = dsf.set(0); + std::set set = dsf.set(0); LONGS_EQUAL(2, set.size()); - std::set expected; expected += 0, 1; + const std::set expected{0, 1}; EXPECT(expected == set); } @@ -175,10 +176,10 @@ TEST(DSFVector, set2) { DSFVector dsf(3); dsf.merge(0,1); dsf.merge(1,2); - set set = dsf.set(0); + std::set set = dsf.set(0); LONGS_EQUAL(3, set.size()); - std::set expected; expected += 0, 1, 2; + const std::set expected{0, 1, 2}; EXPECT(expected == set); } @@ -195,13 +196,12 @@ TEST(DSFVector, isSingleton) { TEST(DSFVector, mergePairwiseMatches) { // Create some measurements - vector keys; - keys += 1,2,3,4,5,6; + const vector keys{1, 2, 3, 4, 5, 6}; // Create some "matches" typedef pair Match; - vector matches; - matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); + const vector matches{Match(1, 2), Match(2, 3), Match(4, 5), + Match(4, 6)}; // Merge matches DSFVector dsf(keys); @@ -209,13 +209,13 @@ TEST(DSFVector, mergePairwiseMatches) { dsf.merge(m.first,m.second); // Check that we have two connected components, 1,2,3 and 4,5,6 - map > sets = dsf.sets(); + map > sets = dsf.sets(); LONGS_EQUAL(2, sets.size()); - set expected1; expected1 += 1,2,3; - set actual1 = sets[dsf.find(2)]; + const std::set expected1{1, 2, 3}; + std::set actual1 = sets[dsf.find(2)]; EXPECT(expected1 == actual1); - set expected2; expected2 += 4,5,6; - set actual2 = sets[dsf.find(5)]; + const std::set expected2{4, 5, 6}; + std::set actual2 = sets[dsf.find(5)]; EXPECT(expected2 == actual2); } diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp index 12dee036eb..acb6347db2 100644 --- a/gtsam/base/tests/testFastContainers.cpp +++ b/gtsam/base/tests/testFastContainers.cpp @@ -11,12 +11,8 @@ #include #include -#include -#include - #include -using namespace boost::assign; using namespace gtsam; /* ************************************************************************* */ @@ -25,7 +21,7 @@ TEST( testFastContainers, KeySet ) { KeyVector init_vector {2, 3, 4, 5}; KeySet actSet(init_vector); - KeySet expSet; expSet += 2, 3, 4, 5; + KeySet expSet{2, 3, 4, 5}; EXPECT(actSet == expSet); } diff --git a/gtsam/base/tests/testSymmetricBlockMatrix.cpp b/gtsam/base/tests/testSymmetricBlockMatrix.cpp index c24e12c251..24dbe0a967 100644 --- a/gtsam/base/tests/testSymmetricBlockMatrix.cpp +++ b/gtsam/base/tests/testSymmetricBlockMatrix.cpp @@ -17,14 +17,12 @@ #include #include -#include using namespace std; using namespace gtsam; -using boost::assign::list_of; static SymmetricBlockMatrix testBlockMatrix( - list_of(3)(2)(1), + std::vector{3, 2, 1}, (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, 2, 8, 9, 10, 11, 12, @@ -101,7 +99,8 @@ TEST(SymmetricBlockMatrix, Ranges) /* ************************************************************************* */ TEST(SymmetricBlockMatrix, expressions) { - SymmetricBlockMatrix expected1(list_of(2)(3)(1), (Matrix(6, 6) << + const std::vector dimensions{2, 3, 1}; + SymmetricBlockMatrix expected1(dimensions, (Matrix(6, 6) << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 6, 8, 0, @@ -109,7 +108,7 @@ TEST(SymmetricBlockMatrix, expressions) 0, 0, 0, 0, 16, 0, 0, 0, 0, 0, 0, 0).finished()); - SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Matrix(6, 6) << + SymmetricBlockMatrix expected2(dimensions, (Matrix(6, 6) << 0, 0, 10, 15, 20, 0, 0, 0, 12, 18, 24, 0, 0, 0, 0, 0, 0, 0, @@ -120,32 +119,32 @@ TEST(SymmetricBlockMatrix, expressions) Matrix a = (Matrix(1, 3) << 2, 3, 4).finished(); Matrix b = (Matrix(1, 2) << 5, 6).finished(); - SymmetricBlockMatrix bm1(list_of(2)(3)(1)); + SymmetricBlockMatrix bm1(dimensions); bm1.setZero(); bm1.diagonalBlock(1).rankUpdate(a.transpose()); EXPECT(assert_equal(Matrix(expected1.selfadjointView()), bm1.selfadjointView())); - SymmetricBlockMatrix bm2(list_of(2)(3)(1)); + SymmetricBlockMatrix bm2(dimensions); bm2.setZero(); bm2.updateOffDiagonalBlock(0, 1, b.transpose() * a); EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm2.selfadjointView())); - SymmetricBlockMatrix bm3(list_of(2)(3)(1)); + SymmetricBlockMatrix bm3(dimensions); bm3.setZero(); bm3.updateOffDiagonalBlock(1, 0, a.transpose() * b); EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm3.selfadjointView())); - SymmetricBlockMatrix bm4(list_of(2)(3)(1)); + SymmetricBlockMatrix bm4(dimensions); bm4.setZero(); bm4.updateDiagonalBlock(1, expected1.diagonalBlock(1)); EXPECT(assert_equal(Matrix(expected1.selfadjointView()), bm4.selfadjointView())); - SymmetricBlockMatrix bm5(list_of(2)(3)(1)); + SymmetricBlockMatrix bm5(dimensions); bm5.setZero(); bm5.updateOffDiagonalBlock(0, 1, expected2.aboveDiagonalBlock(0, 1)); EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm5.selfadjointView())); - SymmetricBlockMatrix bm6(list_of(2)(3)(1)); + SymmetricBlockMatrix bm6(dimensions); bm6.setZero(); bm6.updateOffDiagonalBlock(1, 0, expected2.aboveDiagonalBlock(0, 1).transpose()); EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm6.selfadjointView())); @@ -162,7 +161,8 @@ TEST(SymmetricBlockMatrix, inverseInPlace) { inputMatrix += c * c.transpose(); const Matrix expectedInverse = inputMatrix.inverse(); - SymmetricBlockMatrix symmMatrix(list_of(2)(1), inputMatrix); + const std::vector dimensions{2, 1}; + SymmetricBlockMatrix symmMatrix(dimensions, inputMatrix); // invert in place symmMatrix.invertInPlace(); EXPECT(assert_equal(expectedInverse, symmMatrix.selfadjointView())); diff --git a/gtsam/base/tests/testTreeTraversal.cpp b/gtsam/base/tests/testTreeTraversal.cpp index 88e476cb9e..147f4f0307 100644 --- a/gtsam/base/tests/testTreeTraversal.cpp +++ b/gtsam/base/tests/testTreeTraversal.cpp @@ -23,16 +23,13 @@ #include #include #include -#include -using boost::assign::operator+=; -using namespace std; using namespace gtsam; struct TestNode { typedef boost::shared_ptr shared_ptr; int data; - vector children; + std::vector children; TestNode() : data(-1) {} TestNode(int data) : data(data) {} }; @@ -110,10 +107,8 @@ TEST(treeTraversal, DepthFirst) TestForest testForest = makeTestForest(); // Expected visit order - std::list preOrderExpected; - preOrderExpected += 0, 2, 3, 4, 1; - std::list postOrderExpected; - postOrderExpected += 2, 4, 3, 0, 1; + const std::list preOrderExpected{0, 2, 3, 4, 1}; + const std::list postOrderExpected{2, 4, 3, 0, 1}; // Actual visit order PreOrderVisitor preVisitor; @@ -135,8 +130,7 @@ TEST(treeTraversal, CloneForest) testForest2.roots_ = treeTraversal::CloneForest(testForest1); // Check that the original and clone both are expected - std::list preOrder1Expected; - preOrder1Expected += 0, 2, 3, 4, 1; + const std::list preOrder1Expected{0, 2, 3, 4, 1}; std::list preOrder1Actual = getPreorder(testForest1); std::list preOrder2Actual = getPreorder(testForest2); EXPECT(assert_container_equality(preOrder1Expected, preOrder1Actual)); @@ -144,8 +138,7 @@ TEST(treeTraversal, CloneForest) // Modify clone - should not modify original testForest2.roots_[0]->children[1]->data = 10; - std::list preOrderModifiedExpected; - preOrderModifiedExpected += 0, 2, 10, 4, 1; + const std::list preOrderModifiedExpected{0, 2, 10, 4, 1}; // Check that original is the same and only the clone is modified std::list preOrder1ModActual = getPreorder(testForest1); diff --git a/gtsam/base/tests/testVerticalBlockMatrix.cpp b/gtsam/base/tests/testVerticalBlockMatrix.cpp index 8bb4474a45..e6630427ae 100644 --- a/gtsam/base/tests/testVerticalBlockMatrix.cpp +++ b/gtsam/base/tests/testVerticalBlockMatrix.cpp @@ -18,14 +18,14 @@ #include #include -#include -using namespace std; +#include +#include + using namespace gtsam; -using boost::assign::list_of; -list L = list_of(3)(2)(1); -vector dimensions(L.begin(),L.end()); +const std::list L{3, 2, 1}; +const std::vector dimensions(L.begin(), L.end()); //***************************************************************************** TEST(VerticalBlockMatrix, Constructor1) { diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index ac5eccd14f..e3f9d9dd2e 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -399,13 +399,9 @@ TEST(ADT, factor_graph) { /* ************************************************************************* */ // test equality TEST(ADT, equality_noparser) { - DiscreteKey A(0, 2), B(1, 2); - Signature::Table tableA, tableB; - Signature::Row rA, rB; - rA += 80, 20; - rB += 60, 40; - tableA += rA; - tableB += rB; + const DiscreteKey A(0, 2), B(1, 2); + const Signature::Row rA{80, 20}, rB{60, 40}; + const Signature::Table tableA{rA}, tableB{rB}; // Check straight equality ADT pA1 = create(A % tableA); @@ -520,9 +516,9 @@ TEST(ADT, elimination) { // normalize ADT actual = f1 / actualSum; - vector cpt; - cpt += 1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11, // - 1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10; + const vector cpt{ + 1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11, // + 1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10}; ADT expected(A & B & C, cpt); CHECK(assert_equal(expected, actual)); } @@ -535,9 +531,9 @@ TEST(ADT, elimination) { // normalize ADT actual = f1 / actualSum; - vector cpt; - cpt += 1.0 / 21, 2.0 / 21, 3.0 / 21, 4.0 / 21, 5.0 / 21, 6.0 / 21, // - 1.0 / 25, 8.0 / 25, 3.0 / 25, 3.0 / 25, 5.0 / 25, 5.0 / 25; + const vector cpt{ + 1.0 / 21, 2.0 / 21, 3.0 / 21, 4.0 / 21, 5.0 / 21, 6.0 / 21, // + 1.0 / 25, 8.0 / 25, 3.0 / 25, 3.0 / 25, 5.0 / 25, 5.0 / 25}; ADT expected(A & B & C, cpt); CHECK(assert_equal(expected, actual)); } diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 46e6c38136..2f385263c1 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -26,7 +26,9 @@ #include #include -using namespace std; +using std::vector; +using std::string; +using std::map; using namespace gtsam; template @@ -280,8 +282,7 @@ TEST(DecisionTree, Compose) { DT f1(B, DT(A, 0, 1), DT(A, 2, 3)); // Create from string - vector keys; - keys += DT::LabelC(A, 2), DT::LabelC(B, 2); + vector keys{DT::LabelC(A, 2), DT::LabelC(B, 2)}; DT f2(keys, "0 2 1 3"); EXPECT(assert_equal(f2, f1, 1e-9)); @@ -291,7 +292,7 @@ TEST(DecisionTree, Compose) { DOT(f4); // a bigger tree - keys += DT::LabelC(C, 2); + keys.push_back(DT::LabelC(C, 2)); DT f5(keys, "0 4 2 6 1 5 3 7"); EXPECT(assert_equal(f5, f4, 1e-9)); DOT(f5); @@ -322,7 +323,7 @@ TEST(DecisionTree, Containers) { /* ************************************************************************** */ // Test nrAssignments. TEST(DecisionTree, NrAssignments) { - pair A("A", 2), B("B", 2), C("C", 2); + const std::pair A("A", 2), B("B", 2), C("C", 2); DT tree({A, B, C}, "1 1 1 1 1 1 1 1"); EXPECT(tree.root_->isLeaf()); auto leaf = boost::dynamic_pointer_cast(tree.root_); @@ -472,8 +473,8 @@ TEST(DecisionTree, unzip) { // Test thresholding. TEST(DecisionTree, threshold) { // Create three level tree - vector keys; - keys += DT::LabelC("C", 2), DT::LabelC("B", 2), DT::LabelC("A", 2); + const vector keys{DT::LabelC("C", 2), DT::LabelC("B", 2), + DT::LabelC("A", 2)}; DT tree(keys, "0 1 2 3 4 5 6 7"); // Check number of leaves equal to zero @@ -495,8 +496,8 @@ TEST(DecisionTree, threshold) { // Test apply with assignment. TEST(DecisionTree, ApplyWithAssignment) { // Create three level tree - vector keys; - keys += DT::LabelC("C", 2), DT::LabelC("B", 2), DT::LabelC("A", 2); + const vector keys{DT::LabelC("C", 2), DT::LabelC("B", 2), + DT::LabelC("A", 2)}; DT tree(keys, "1 2 3 4 5 6 7 8"); DecisionTree probTree( diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index 3df4bf9e6f..f4a2e30ea4 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -53,12 +53,8 @@ TEST(DiscreteConditional, constructors) { TEST(DiscreteConditional, constructors_alt_interface) { DiscreteKey X(0, 2), Y(2, 3), Z(1, 2); // watch ordering ! - Signature::Table table; - Signature::Row r1, r2, r3; - r1 += 1.0, 1.0; - r2 += 2.0, 3.0; - r3 += 1.0, 4.0; - table += r1, r2, r3; + const Signature::Row r1{1, 1}, r2{2, 3}, r3{1, 4}; + const Signature::Table table{r1, r2, r3}; DiscreteConditional actual1(X, {Y}, table); DecisionTreeFactor f1(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8"); diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index 3213e514f1..ffca8a481a 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -183,8 +183,7 @@ TEST_UNSAFE(DiscreteMarginals, truss2) { F[j] /= sum; // Marginals - vector table; - table += F[j], T[j]; + const vector table{F[j], T[j]}; DecisionTreeFactor expectedM(key[j], table); DiscreteFactor::shared_ptr actualM = marginals(j); EXPECT(assert_equal( diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 533041a2cf..c7498cd58b 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -20,12 +20,9 @@ #include #include #include -#include -using namespace boost::assign; using namespace std::placeholders; using namespace gtsam; -using namespace std; using boost::none; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 88c00a2e97..44dc55a81d 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -23,12 +23,10 @@ #include #include -#include // for operator += #include #include #include -using namespace boost::assign; using namespace gtsam; using namespace std; @@ -749,11 +747,10 @@ namespace align_3 { TEST(Pose2, align_3) { using namespace align_3; - Point2Pairs ab_pairs; Point2Pair ab1(make_pair(a1, b1)); Point2Pair ab2(make_pair(a2, b2)); Point2Pair ab3(make_pair(a3, b3)); - ab_pairs += ab1, ab2, ab3; + const Point2Pairs ab_pairs{ab1, ab2, ab3}; boost::optional aTb = Pose2::Align(ab_pairs); EXPECT(assert_equal(expected, *aTb)); @@ -778,9 +775,7 @@ namespace { TEST(Pose2, align_4) { using namespace align_3; - Point2Vector as, bs; - as += a1, a2, a3; - bs += b3, b1, b2; // note in 3,1,2 order ! + Point2Vector as{a1, a2, a3}, bs{b3, b1, b2}; // note in 3,1,2 order ! Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2; Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 78a4af799e..dd7da172ed 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -20,15 +20,13 @@ #include #include -#include // for operator += -using namespace boost::assign; -using namespace std::placeholders; #include #include using namespace std; using namespace gtsam; +using namespace std::placeholders; GTSAM_CONCEPT_TESTABLE_INST(Pose3) GTSAM_CONCEPT_LIE_INST(Pose3) @@ -809,11 +807,10 @@ TEST( Pose3, adjointMap) { TEST(Pose3, Align1) { Pose3 expected(Rot3(), Point3(10,10,0)); - vector correspondences; - Point3Pair ab1(make_pair(Point3(10,10,0), Point3(0,0,0))); - Point3Pair ab2(make_pair(Point3(30,20,0), Point3(20,10,0))); - Point3Pair ab3(make_pair(Point3(20,30,0), Point3(10,20,0))); - correspondences += ab1, ab2, ab3; + Point3Pair ab1(Point3(10,10,0), Point3(0,0,0)); + Point3Pair ab2(Point3(30,20,0), Point3(20,10,0)); + Point3Pair ab3(Point3(20,30,0), Point3(10,20,0)); + const vector correspondences{ab1, ab2, ab3}; boost::optional actual = Pose3::Align(correspondences); EXPECT(assert_equal(expected, *actual)); @@ -825,15 +822,12 @@ TEST(Pose3, Align2) { Rot3 R = Rot3::RzRyRx(0.3, 0.2, 0.1); Pose3 expected(R, t); - vector correspondences; Point3 p1(0,0,1), p2(10,0,2), p3(20,-10,30); Point3 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2), q3 = expected.transformFrom(p3); - Point3Pair ab1(make_pair(q1, p1)); - Point3Pair ab2(make_pair(q2, p2)); - Point3Pair ab3(make_pair(q3, p3)); - correspondences += ab1, ab2, ab3; + const Point3Pair ab1{q1, p1}, ab2{q2, p2}, ab3{q3, p3}; + const vector correspondences{ab1, ab2, ab3}; boost::optional actual = Pose3::Align(correspondences); EXPECT(assert_equal(expected, *actual, 1e-5)); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index b548b93153..c951d857c1 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -36,7 +36,6 @@ #include #include -using namespace boost::assign; using namespace std::placeholders; using namespace gtsam; using namespace std; @@ -51,9 +50,8 @@ Point3 point3_(const Unit3& p) { } TEST(Unit3, point3) { - vector ps; - ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0) - / sqrt(2.0); + const vector ps{Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), + Point3(1, 1, 0) / sqrt(2.0)}; Matrix actualH, expectedH; for(Point3 p: ps) { Unit3 s(p); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index dfdb32b468..681554645e 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -42,17 +42,11 @@ #include -#include -#include -#include - #include #include #include #include -using namespace boost::assign; - #include using namespace std; @@ -79,8 +73,7 @@ TEST( GaussianJunctionTreeB, constructor2 ) { // linearize GaussianFactorGraph::shared_ptr fg = nlfg.linearize(values); - Ordering ordering; - ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4); + const Ordering ordering {X(1), X(3), X(5), X(7), X(2), X(6), X(4)}; // create an ordering GaussianEliminationTree etree(*fg, ordering); From 313ab013d377849b9d3681e8f69513d42a024eeb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 15:44:12 -0800 Subject: [PATCH 05/37] Got rid of Boost cref_list_of --- gtsam/inference/BayesTree-inst.h | 9 +++------ gtsam/linear/HessianFactor.cpp | 23 ++++++++++++----------- gtsam/linear/JacobianFactor.cpp | 18 +++++++++--------- gtsam/nonlinear/NonlinearFactor.h | 3 --- gtsam/sfm/ShonanGaugeFactor.h | 2 +- gtsam/symbolic/SymbolicFactor.h | 12 ++++++------ 6 files changed, 31 insertions(+), 36 deletions(-) diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 82899b299a..055971a82b 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -26,11 +26,8 @@ #include #include -#include #include -using boost::assign::cref_list_of; - namespace gtsam { /* ************************************************************************* */ @@ -281,8 +278,8 @@ namespace gtsam { FactorGraphType cliqueMarginal = clique->marginal2(function); // Now, marginalize out everything that is not variable j - BayesNetType marginalBN = *cliqueMarginal.marginalMultifrontalBayesNet( - Ordering(cref_list_of<1,Key>(j)), function); + BayesNetType marginalBN = + *cliqueMarginal.marginalMultifrontalBayesNet(Ordering{j}, function); // The Bayes net should contain only one conditional for variable j, so return it return marginalBN.front(); @@ -403,7 +400,7 @@ namespace gtsam { } // now, marginalize out everything that is not variable j1 or j2 - return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), function); + return p_BC1C2.marginalMultifrontalBayesNet(Ordering{j1, j2}, function); } /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 8646a9cae1..c5cde2c151 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -32,7 +32,6 @@ #include #include #include -#include #include #include #include @@ -41,7 +40,6 @@ #include using namespace std; -using namespace boost::assign; namespace br { using namespace boost::range; using namespace boost::adaptors; @@ -49,6 +47,9 @@ using namespace boost::adaptors; namespace gtsam { +// Typedefs used in constructors below. +using Dims = std::vector; + /* ************************************************************************* */ void HessianFactor::Allocate(const Scatter& scatter) { gttic(HessianFactor_Allocate); @@ -74,14 +75,14 @@ HessianFactor::HessianFactor(const Scatter& scatter) { /* ************************************************************************* */ HessianFactor::HessianFactor() : - info_(cref_list_of<1>(1)) { + info_(Dims{1}) { assert(info_.rows() == 1); constantTerm() = 0.0; } /* ************************************************************************* */ -HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) : - GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) { +HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) + : GaussianFactor(KeyVector{j}), info_(Dims{G.cols(), 1}) { if (G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); @@ -93,8 +94,8 @@ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) /* ************************************************************************* */ // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g -HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : - GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(Sigma.cols())(1)) { +HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) + : GaussianFactor(KeyVector{j}), info_(Dims{Sigma.cols(), 1}) { if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); @@ -107,8 +108,8 @@ HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2, double f) : - GaussianFactor(cref_list_of<2>(j1)(j2)), info_( - cref_list_of<3>(G11.cols())(G22.cols())(1)) { + GaussianFactor(KeyVector{j1,j2}), info_( + Dims{G11.cols(),G22.cols(),1}) { info_.setDiagonalBlock(0, G11); info_.setOffDiagonalBlock(0, 1, G12); info_.setDiagonalBlock(1, G22); @@ -121,8 +122,8 @@ HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22, const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3, double f) : - GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), info_( - cref_list_of<4>(G11.cols())(G22.cols())(G33.cols())(1)) { + GaussianFactor(KeyVector{j1,j2,j3}), info_( + Dims{G11.cols(),G22.cols(),G33.cols(),1}) { if (G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || G22.cols() != G12.cols() || G33.cols() != G13.cols() diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 6354766874..84083c4bc1 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -31,7 +31,6 @@ #include #include -#include #include #include #include @@ -44,13 +43,16 @@ #include using namespace std; -using namespace boost::assign; namespace gtsam { +// Typedefs used in constructors below. +using Dims = std::vector; +using Pairs = std::vector>; + /* ************************************************************************* */ JacobianFactor::JacobianFactor() : - Ab_(cref_list_of<1>(1), 0) { + Ab_(Dims{1}, 0) { getb().setZero(); } @@ -68,29 +70,27 @@ JacobianFactor::JacobianFactor(const GaussianFactor& gf) { /* ************************************************************************* */ JacobianFactor::JacobianFactor(const Vector& b_in) : - Ab_(cref_list_of<1>(1), b_in.size()) { + Ab_(Dims{1}, b_in.size()) { getb() = b_in; } /* ************************************************************************* */ JacobianFactor::JacobianFactor(Key i1, const Matrix& A1, const Vector& b, const SharedDiagonal& model) { - fillTerms(cref_list_of<1>(make_pair(i1, A1)), b, model); + fillTerms(Pairs{{i1, A1}}, b, model); } /* ************************************************************************* */ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, const Matrix& A2, const Vector& b, const SharedDiagonal& model) { - fillTerms(cref_list_of<2>(make_pair(i1, A1))(make_pair(i2, A2)), b, model); + fillTerms(Pairs{{i1, A1}, {i2, A2}}, b, model); } /* ************************************************************************* */ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) { - fillTerms( - cref_list_of<3>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)), - b, model); + fillTerms(Pairs{{i1, A1}, {i2, A2}, {i3, A3}}, b, model); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index f6517d2810..622d4de376 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -29,12 +29,9 @@ #include // boost::index_sequence #include -#include namespace gtsam { -using boost::assign::cref_list_of; - /* ************************************************************************* */ /** diff --git a/gtsam/sfm/ShonanGaugeFactor.h b/gtsam/sfm/ShonanGaugeFactor.h index d6240b1c42..4dc5b285e7 100644 --- a/gtsam/sfm/ShonanGaugeFactor.h +++ b/gtsam/sfm/ShonanGaugeFactor.h @@ -59,7 +59,7 @@ class GTSAM_EXPORT ShonanGaugeFactor : public NonlinearFactor { */ ShonanGaugeFactor(Key key, size_t p, size_t d = 3, boost::optional gamma = boost::none) - : NonlinearFactor(boost::assign::cref_list_of<1>(key)) { + : NonlinearFactor(KeyVector{key}) { if (p < d) { throw std::invalid_argument("ShonanGaugeFactor must have p>=d."); } diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 767998d22c..91dbe3464a 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -55,27 +55,27 @@ namespace gtsam { /** Construct unary factor */ explicit SymbolicFactor(Key j) : - Base(boost::assign::cref_list_of<1>(j)) {} + Base(KeyVector{j}) {} /** Construct binary factor */ SymbolicFactor(Key j1, Key j2) : - Base(boost::assign::cref_list_of<2>(j1)(j2)) {} + Base(KeyVector{j1, j2}) {} /** Construct ternary factor */ SymbolicFactor(Key j1, Key j2, Key j3) : - Base(boost::assign::cref_list_of<3>(j1)(j2)(j3)) {} + Base(KeyVector{j1, j2, j3}) {} /** Construct 4-way factor */ SymbolicFactor(Key j1, Key j2, Key j3, Key j4) : - Base(boost::assign::cref_list_of<4>(j1)(j2)(j3)(j4)) {} + Base(KeyVector{j1, j2, j3, j4}) {} /** Construct 5-way factor */ SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5) : - Base(boost::assign::cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {} + Base(KeyVector{j1, j2, j3, j4, j5}) {} /** Construct 6-way factor */ SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : - Base(boost::assign::cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {} + Base(KeyVector{j1, j2, j3, j4, j5, j6}) {} /** Create symbolic version of any factor */ explicit SymbolicFactor(const Factor& factor) : Base(factor.keys()) {} From 80137792c9d055c5d75c30e6429f70b3e5d3ef35 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 15:46:30 -0800 Subject: [PATCH 06/37] Removed unused headers --- gtsam/inference/tests/testKey.cpp | 3 --- gtsam/inference/tests/testLabeledSymbol.cpp | 2 -- gtsam/linear/KalmanFilter.cpp | 2 -- gtsam/linear/tests/testGaussianBayesNet.cpp | 3 --- gtsam/linear/tests/testGaussianFactorGraph.cpp | 4 ---- 5 files changed, 14 deletions(-) diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 98c5d36bf8..59245a53a7 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -21,11 +21,8 @@ #include -#include // for operator += - #include -using namespace boost::assign; using namespace std; using namespace gtsam; diff --git a/gtsam/inference/tests/testLabeledSymbol.cpp b/gtsam/inference/tests/testLabeledSymbol.cpp index 4ac171c73d..f63b438b45 100644 --- a/gtsam/inference/tests/testLabeledSymbol.cpp +++ b/gtsam/inference/tests/testLabeledSymbol.cpp @@ -19,9 +19,7 @@ #include #include -#include // for operator += -using namespace boost::assign; using namespace std; using namespace gtsam; diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 8a1b145c34..05e82be087 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -27,9 +27,7 @@ #include #include -#include -using namespace boost::assign; using namespace std; namespace gtsam { diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 771a24631e..4113c3efdd 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -25,15 +25,12 @@ #include #include -#include -#include // for operator += #include // STL/C++ #include #include -using namespace boost::assign; using namespace std::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 41464a1109..221f499565 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -26,10 +26,6 @@ #include #include -#include -#include // for operator += -using namespace boost::assign; - #include #include From d3380bc065e8fed741e774f774ab9c4f118ba543 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 15:47:01 -0800 Subject: [PATCH 07/37] Convert to initializer lists --- gtsam/inference/tests/testOrdering.cpp | 76 ++++++++----------- gtsam/inference/tests/testVariableSlots.cpp | 15 ++-- gtsam/linear/tests/testErrors.cpp | 16 ++-- .../linear/tests/testGaussianConditional.cpp | 50 ++++-------- .../slam/tests/testSmartProjectionFactor.cpp | 6 +- 5 files changed, 61 insertions(+), 102 deletions(-) diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 1afa1cfde8..0ec120fe14 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -22,11 +22,8 @@ #include #include -#include - using namespace std; using namespace gtsam; -using namespace boost::assign; namespace example { SymbolicFactorGraph symbolicChain() { @@ -47,33 +44,33 @@ TEST(Ordering, constrained_ordering) { // unconstrained version { Ordering actual = Ordering::Colamd(symbolicGraph); - Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + Ordering expected = Ordering({0, 1, 2, 3, 4, 5}); EXPECT(assert_equal(expected, actual)); } // constrained version - push one set to the end { - Ordering actual = Ordering::ColamdConstrainedLast(symbolicGraph, list_of(2)(4)); - Ordering expected = Ordering(list_of(0)(1)(5)(3)(4)(2)); - EXPECT(assert_equal(expected, actual)); + Ordering actual = Ordering::ColamdConstrainedLast(symbolicGraph, {2, 4}); + Ordering expected = Ordering({0, 1, 5, 3, 4, 2}); + EXPECT(assert_equal(expected, actual)); } // constrained version - push one set to the start { - Ordering actual = Ordering::ColamdConstrainedFirst(symbolicGraph, list_of(2)(4)); - Ordering expected = Ordering(list_of(2)(4)(0)(1)(3)(5)); + Ordering actual = Ordering::ColamdConstrainedFirst(symbolicGraph, {2, 4}); + Ordering expected = Ordering({2, 4, 0, 1, 3, 5}); EXPECT(assert_equal(expected, actual)); } // Make sure giving empty constraints does not break the code { Ordering actual = Ordering::ColamdConstrainedLast(symbolicGraph, {}); - Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + Ordering expected = Ordering({0, 1, 2, 3, 4, 5}); EXPECT(assert_equal(expected, actual)); } { Ordering actual = Ordering::ColamdConstrainedFirst(symbolicGraph, {}); - Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + Ordering expected = Ordering({0, 1, 2, 3, 4, 5}); EXPECT(assert_equal(expected, actual)); } @@ -81,11 +78,11 @@ TEST(Ordering, constrained_ordering) { SymbolicFactorGraph emptyGraph; Ordering empty; { - Ordering actual = Ordering::ColamdConstrainedLast(emptyGraph, list_of(2)(4)); + Ordering actual = Ordering::ColamdConstrainedLast(emptyGraph, {2, 4}); EXPECT(assert_equal(empty, actual)); } { - Ordering actual = Ordering::ColamdConstrainedFirst(emptyGraph, list_of(2)(4)); + Ordering actual = Ordering::ColamdConstrainedFirst(emptyGraph, {2, 4}); EXPECT(assert_equal(empty, actual)); } } @@ -105,7 +102,7 @@ TEST(Ordering, grouped_constrained_ordering) { constraints[5] = 2; Ordering actual = Ordering::ColamdConstrained(symbolicGraph, constraints); - Ordering expected = list_of(0)(1)(3)(2)(4)(5); + Ordering expected = {0, 1, 3, 2, 4, 5}; EXPECT(assert_equal(expected, actual)); } @@ -139,9 +136,11 @@ TEST(Ordering, csr_format) { MetisIndex mi(symbolicGraph); - vector xadjExpected, adjExpected; - xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; - adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, 13, 8, 12, 14, 9, 13; + const vector xadjExpected{0, 2, 5, 8, 11, 13, 16, 20, + 24, 28, 31, 33, 36, 39, 42, 44}, + adjExpected{1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, + 10, 1, 5, 7, 11, 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, + 14, 5, 11, 6, 10, 12, 7, 11, 13, 8, 12, 14, 9, 13}; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); @@ -161,9 +160,8 @@ TEST(Ordering, csr_format_2) { MetisIndex mi(symbolicGraph); - vector xadjExpected, adjExpected; - xadjExpected += 0, 1, 4, 6, 8, 10; - adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; + const std::vector xadjExpected{0, 1, 4, 6, 8, 10}, + adjExpected{1, 0, 2, 4, 1, 3, 2, 4, 1, 3}; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); @@ -183,9 +181,8 @@ TEST(Ordering, csr_format_3) { MetisIndex mi(symbolicGraph); - vector xadjExpected, adjExpected; - xadjExpected += 0, 1, 4, 6, 8, 10; - adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; + const std::vector xadjExpected{0, 1, 4, 6, 8, 10}, + adjExpected{1, 0, 2, 4, 1, 3, 2, 4, 1, 3}; //size_t minKey = mi.minKey(); vector adjAcutal = mi.adj(); @@ -202,24 +199,18 @@ TEST(Ordering, csr_format_3) { /* ************************************************************************* */ TEST(Ordering, AppendVector) { using symbol_shorthand::X; + KeyVector keys{X(0), X(1), X(2)}; Ordering actual; - KeyVector keys = {X(0), X(1), X(2)}; actual += keys; - Ordering expected; - expected += X(0); - expected += X(1); - expected += X(2); + Ordering expected{X(0), X(1), X(2)}; EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST(Ordering, Contains) { using symbol_shorthand::X; - Ordering ordering; - ordering += X(0); - ordering += X(1); - ordering += X(2); + Ordering ordering{X(0), X(1), X(2)}; EXPECT(ordering.contains(X(1))); EXPECT(!ordering.contains(X(4))); @@ -239,9 +230,8 @@ TEST(Ordering, csr_format_4) { MetisIndex mi(symbolicGraph); - vector xadjExpected, adjExpected; - xadjExpected += 0, 1, 3, 5, 7, 9, 10; - adjExpected += 1, 0, 2, 1, 3, 2, 4, 3, 5, 4; + const vector xadjExpected{0, 1, 3, 5, 7, 9, 10}, + adjExpected{1, 0, 2, 1, 3, 2, 4, 3, 5, 4}; vector adjAcutal = mi.adj(); vector xadjActual = mi.xadj(); @@ -274,9 +264,7 @@ TEST(Ordering, metis) { MetisIndex mi(symbolicGraph); - vector xadjExpected, adjExpected; - xadjExpected += 0, 1, 3, 4; - adjExpected += 1, 0, 2, 1; + const vector xadjExpected{0, 1, 3, 4}, adjExpected{1, 0, 2, 1}; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); @@ -303,7 +291,7 @@ TEST(Ordering, MetisLoop) { // | - P( 4 | 0 3) // | | - P( 5 | 0 4) // | - P( 2 | 1 3) - Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); + Ordering expected = Ordering({5, 4, 2, 1, 0, 3}); EXPECT(assert_equal(expected, actual)); } #elif defined(_WIN32) @@ -313,7 +301,7 @@ TEST(Ordering, MetisLoop) { // | - P( 3 | 5 2) // | | - P( 4 | 5 3) // | - P( 1 | 0 2) - Ordering expected = Ordering(list_of(4)(3)(1)(0)(5)(2)); + Ordering expected = Ordering({4, 3, 1, 0, 5, 2}); EXPECT(assert_equal(expected, actual)); } #else @@ -323,7 +311,7 @@ TEST(Ordering, MetisLoop) { // | - P( 2 | 4 1) // | | - P( 3 | 4 2) // | - P( 5 | 0 1) - Ordering expected = Ordering(list_of(3)(2)(5)(0)(4)(1)); + Ordering expected = Ordering({3, 2, 5, 0, 4, 1}); EXPECT(assert_equal(expected, actual)); } #endif @@ -347,7 +335,7 @@ TEST(Ordering, MetisSingleNode) { symbolicGraph.push_factor(7); Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph); - Ordering expected = Ordering(list_of(7)); + Ordering expected = Ordering({7}); EXPECT(assert_equal(expected, actual)); } #endif @@ -365,7 +353,7 @@ TEST(Ordering, Create) { //| | | - P( 1 | 2) //| | | | - P( 0 | 1) Ordering actual = Ordering::Create(Ordering::COLAMD, symbolicGraph); - Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + Ordering expected = Ordering({0, 1, 2, 3, 4, 5}); EXPECT(assert_equal(expected, actual)); } @@ -376,7 +364,7 @@ TEST(Ordering, Create) { //- P( 1 0 2) //| - P( 3 4 | 2) //| | - P( 5 | 4) - Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); + Ordering expected = Ordering({5, 3, 4, 1, 0, 2}); EXPECT(assert_equal(expected, actual)); } #endif diff --git a/gtsam/inference/tests/testVariableSlots.cpp b/gtsam/inference/tests/testVariableSlots.cpp index a7a0945e79..b030331c14 100644 --- a/gtsam/inference/tests/testVariableSlots.cpp +++ b/gtsam/inference/tests/testVariableSlots.cpp @@ -22,11 +22,8 @@ #include #include -#include - using namespace gtsam; using namespace std; -using namespace boost::assign; /* ************************************************************************* */ TEST(VariableSlots, constructor) { @@ -41,12 +38,12 @@ TEST(VariableSlots, constructor) { static const size_t none = numeric_limits::max(); VariableSlots expected((SymbolicFactorGraph())); - expected[0] += none, 0, 0, none; - expected[1] += none, 1, none, none; - expected[2] += 0, none, 1, none; - expected[3] += 1, none, none, none; - expected[5] += none, none, none, 0; - expected[9] += none, none, none, 1; + expected[0] = {none, 0, 0, none}; + expected[1] = {none, 1, none, none}; + expected[2] = {0, none, 1, none}; + expected[3] = {1, none, none, none}; + expected[5] = {none, none, none, 0}; + expected[9] = {none, none, none, 1}; CHECK(assert_equal(expected, actual)); } diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index f11fb90b99..507254a6cc 100644 --- a/gtsam/linear/tests/testErrors.cpp +++ b/gtsam/linear/tests/testErrors.cpp @@ -15,9 +15,6 @@ * @author Frank Dellaert */ -#include // for += -using namespace boost::assign; - #include #include #include @@ -26,16 +23,13 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST( Errors, arithmetic ) -{ - Errors e; - e += Vector2(1.0,2.0), Vector3(3.0,4.0,5.0); - DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9); +TEST(Errors, arithmetic) { + Errors e{Vector2(1.0, 2.0), Vector3(3.0, 4.0, 5.0)}; + DOUBLES_EQUAL(1 + 4 + 9 + 16 + 25, dot(e, e), 1e-9); axpy(2.0, e, e); - Errors expected; - expected += Vector2(3.0,6.0), Vector3(9.0,12.0,15.0); - CHECK(assert_equal(expected,e)); + const Errors expected{Vector2(3.0, 6.0), Vector3(9.0, 12.0, 15.0)}; + CHECK(assert_equal(expected, e)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index d8c3f94552..36f7eb72df 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -26,11 +26,8 @@ #include #include -#include #include -#include #include -#include #include #include @@ -38,7 +35,6 @@ using namespace gtsam; using namespace std; -using namespace boost::assign; using symbol_shorthand::X; using symbol_shorthand::Y; @@ -64,11 +60,7 @@ TEST(GaussianConditional, constructor) Vector d = Vector2(1.0, 2.0); SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector2(3.0, 4.0)); - vector > terms = pair_list_of - (1, R) - (3, S1) - (5, S2) - (7, S3); + vector > terms = {{1, R}, {3, S1}, {5, S2}, {7, S3}}; GaussianConditional actual(terms, 1, d, s); @@ -223,14 +215,10 @@ TEST( GaussianConditional, solve ) Vector sx1(2); sx1 << 1.0, 1.0; Vector sl1(2); sl1 << 1.0, 1.0; - VectorValues expected = map_list_of - (1, expectedX) - (2, sx1) - (10, sl1); + VectorValues expected = {{1, expectedX} {2, sx1} {10, sl1}}; - VectorValues solution = map_list_of - (2, sx1) // parents - (10, sl1); + VectorValues solution = {{2, sx1}, // parents + {10, sl1}}; solution.insert(cg.solve(solution)); EXPECT(assert_equal(expected, solution, tol)); @@ -254,12 +242,10 @@ TEST( GaussianConditional, solve_simple ) Vector sx1 = Vector2(9.0, 10.0); // elimination order: 1, 2 - VectorValues actual = map_list_of - (2, sx1); // parent + VectorValues actual = {{2, sx1}}; // parent - VectorValues expected = map_list_of - (2, sx1) - (1, (Vector(4) << -3.1,-3.4,-11.9,-13.2).finished()); + VectorValues expected = { + {2, sx1}, {1, (Vector(4) << -3.1, -3.4, -11.9, -13.2).finished()}}; // verify indices/size EXPECT_LONGS_EQUAL(2, (long)cg.size()); @@ -290,13 +276,10 @@ TEST( GaussianConditional, solve_multifrontal ) Vector sl1 = Vector2(9.0, 10.0); // elimination order; _x_, _x1_, _l1_ - VectorValues actual = map_list_of - (10, sl1); // parent + VectorValues actual = {{10, sl1}}; // parent - VectorValues expected = map_list_of - (1, Vector2(-3.1,-3.4)) - (2, Vector2(-11.9,-13.2)) - (10, sl1); + VectorValues expected = { + {1, Vector2(-3.1, -3.4)}, {2, Vector2(-11.9, -13.2)}, {10, sl1}}; // verify indices/size EXPECT_LONGS_EQUAL(3, (long)cg.size()); @@ -330,13 +313,10 @@ TEST( GaussianConditional, solveTranspose ) { // 2 = 1 2 // 5 1 1 3 - VectorValues - x = map_list_of - (1, (Vector(1) << 2.).finished()) - (2, (Vector(1) << 5.).finished()), - y = map_list_of - (1, (Vector(1) << 2.).finished()) - (2, (Vector(1) << 3.).finished()); + VectorValues x = {{1, (Vector(1) << 2.).finished()}, + {2, (Vector(1) << 5.).finished()}}, + y = {{1, (Vector(1) << 2.).finished()}, + {2, (Vector(1) << 3.).finished()}}; // test functional version VectorValues actual = cbn.backSubstituteTranspose(x); @@ -395,7 +375,7 @@ TEST(GaussianConditional, FromMeanAndStddev) { const Vector2 b(20, 40), x0(1, 2), x1(3, 4), x2(5, 6); const double sigma = 3; - VectorValues values = map_list_of(X(0), x0)(X(1), x1)(X(2), x2); + VectorValues values = {{X(0), x0}, {X(1), x1}, {X(2), x2}; auto conditional1 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma); diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index ecdb5287fd..9ab55a8d3b 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -802,9 +802,9 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) { Implicit9& implicitSchurFactor = dynamic_cast(*gaussianImplicitSchurFactor); - VectorValues x = map_list_of(c1, - (Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished())(c2, - (Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished()); + VectorValues x{ + {c1, (Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished()}, + {c2, (Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished()}}; VectorValues yExpected, yActual; double alpha = 1.0; From f9ccf111d16e3d15a1548c5c7be06bc2680b04a9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 18:48:52 -0800 Subject: [PATCH 08/37] Rid gtsam/linear of boost::assign (a Herculean task made easier with Copilot) --- gtsam/inference/BayesNet.h | 2 +- gtsam/inference/FactorGraph.h | 16 +- gtsam/linear/Errors.cpp | 3 - gtsam/linear/Errors.h | 18 +- gtsam/linear/GaussianBayesNet.h | 13 +- gtsam/linear/GaussianFactorGraph.h | 6 + gtsam/linear/tests/testErrors.cpp | 5 +- gtsam/linear/tests/testGaussianBayesNet.cpp | 73 +++-- gtsam/linear/tests/testGaussianBayesTree.cpp | 249 +++++++++--------- .../linear/tests/testGaussianConditional.cpp | 20 +- .../linear/tests/testGaussianFactorGraph.cpp | 24 +- gtsam/linear/tests/testHessianFactor.cpp | 53 ++-- gtsam/linear/tests/testJacobianFactor.cpp | 63 ++--- gtsam/linear/tests/testNoiseModel.cpp | 3 - .../linear/tests/testRegularHessianFactor.cpp | 19 +- .../tests/testRegularJacobianFactor.cpp | 15 +- .../linear/tests/testSerializationLinear.cpp | 15 +- gtsam/linear/tests/testSparseEigen.cpp | 5 +- gtsam/linear/tests/testVectorValues.cpp | 3 - 19 files changed, 288 insertions(+), 317 deletions(-) diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 1b118adc75..d562f3ae6e 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -39,7 +39,7 @@ class BayesNet : public FactorGraph { sharedConditional; ///< A shared pointer to a conditional protected: - /// @name Standard Constructors + /// @name Protected Constructors /// @{ /** Default constructor as an empty BayesNet */ diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 89fd09037c..915ae273af 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -154,10 +154,24 @@ class FactorGraph { /// @} public: + /// @name Constructors + /// @{ + /// Default destructor - // Public and virtual so boost serialization can call it. + /// Public and virtual so boost serialization can call it. virtual ~FactorGraph() = default; + /** + * Constructor that takes an initializer list of shared pointers. + * FactorGraph fg = {make_shared(), ...}; + */ + template > + FactorGraph( + std::initializer_list> sharedFactors) { + for (auto&& f : sharedFactors) factors_.push_back(f); + } + + /// @} /// @name Adding Single Factors /// @{ diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index 41c6c3d09e..e7d893d25e 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -25,9 +25,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -Errors::Errors(){} - /* ************************************************************************* */ Errors::Errors(const VectorValues& V) { for(const Vector& e: V | boost::adaptors::map_values) { diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index f6e147084b..41ba441443 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -31,29 +31,31 @@ namespace gtsam { class VectorValues; /** vector of errors */ - class Errors : public FastList { + class GTSAM_EXPORT Errors : public FastList { + + using Base = FastList; public: - GTSAM_EXPORT Errors() ; + using Base::Base; // inherit constructors /** break V into pieces according to its start indices */ - GTSAM_EXPORT Errors(const VectorValues&V); + Errors(const VectorValues&V); /** print */ - GTSAM_EXPORT void print(const std::string& s = "Errors") const; + void print(const std::string& s = "Errors") const; /** equals, for unit testing */ - GTSAM_EXPORT bool equals(const Errors& expected, double tol=1e-9) const; + bool equals(const Errors& expected, double tol=1e-9) const; /** Addition */ - GTSAM_EXPORT Errors operator+(const Errors& b) const; + Errors operator+(const Errors& b) const; /** subtraction */ - GTSAM_EXPORT Errors operator-(const Errors& b) const; + Errors operator-(const Errors& b) const; /** negation */ - GTSAM_EXPORT Errors operator-() const ; + Errors operator-() const ; }; // Errors diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index c8a28e0757..a63cfa6c70 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -65,8 +65,19 @@ namespace gtsam { explicit GaussianBayesNet(const FactorGraph& graph) : Base(graph) {} + /** + * Constructor that takes an initializer list of shared pointers. + * BayesNet bn = {make_shared(), ...}; + */ + template + GaussianBayesNet( + std::initializer_list > + sharedConditionals) { + for (auto&& gc : sharedConditionals) push_back(gc); + } + /// Destructor - virtual ~GaussianBayesNet() {} + virtual ~GaussianBayesNet() = default; /// @} diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 0d5057aa88..9a48856894 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -74,9 +74,14 @@ namespace gtsam { typedef EliminateableFactorGraph BaseEliminateable; ///< Typedef to base elimination class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + /// @name Constructors + /// @{ + /** Default constructor */ GaussianFactorGraph() {} + using Base::Base; // Inherit constructors + /** Construct from iterator over factors */ template GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} @@ -92,6 +97,7 @@ namespace gtsam { /** Virtual destructor */ virtual ~GaussianFactorGraph() {} + /// @} /// @name Testable /// @{ diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index 507254a6cc..415cd9496b 100644 --- a/gtsam/linear/tests/testErrors.cpp +++ b/gtsam/linear/tests/testErrors.cpp @@ -24,11 +24,12 @@ using namespace gtsam; /* ************************************************************************* */ TEST(Errors, arithmetic) { - Errors e{Vector2(1.0, 2.0), Vector3(3.0, 4.0, 5.0)}; + Errors e = std::list{Vector2(1.0, 2.0), Vector3(3.0, 4.0, 5.0)}; DOUBLES_EQUAL(1 + 4 + 9 + 16 + 25, dot(e, e), 1e-9); axpy(2.0, e, e); - const Errors expected{Vector2(3.0, 6.0), Vector3(9.0, 12.0, 15.0)}; + const Errors expected = + std::list{Vector2(3.0, 6.0), Vector3(9.0, 12.0, 15.0)}; CHECK(assert_equal(expected, e)); } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 4113c3efdd..7b55d00c38 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -38,15 +38,15 @@ using symbol_shorthand::X; static const Key _x_ = 11, _y_ = 22, _z_ = 33; -static GaussianBayesNet smallBayesNet = - list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1))( - GaussianConditional(_y_, Vector1::Constant(5), I_1x1)); +static GaussianBayesNet smallBayesNet = { + boost::make_shared(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1), + boost::make_shared(_y_, Vector1::Constant(5), I_1x1)}; -static GaussianBayesNet noisyBayesNet = - list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1, - noiseModel::Isotropic::Sigma(1, 2.0)))( - GaussianConditional(_y_, Vector1::Constant(5), I_1x1, - noiseModel::Isotropic::Sigma(1, 3.0))); +static GaussianBayesNet noisyBayesNet = { + boost::make_shared(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1, + noiseModel::Isotropic::Sigma(1, 2.0)), + boost::make_shared(_y_, Vector1::Constant(5), I_1x1, + noiseModel::Isotropic::Sigma(1, 3.0))}; /* ************************************************************************* */ TEST( GaussianBayesNet, Matrix ) @@ -112,11 +112,11 @@ TEST( GaussianBayesNet, NoisyMatrix ) /* ************************************************************************* */ TEST(GaussianBayesNet, Optimize) { - VectorValues expected = - map_list_of(_x_, Vector1::Constant(4))(_y_, Vector1::Constant(5)); - VectorValues actual = smallBayesNet.optimize(); + const VectorValues expected{{_x_, Vector1::Constant(4)}, + {_y_, Vector1::Constant(5)}}; + const VectorValues actual = smallBayesNet.optimize(); EXPECT(assert_equal(expected, actual)); -} + } /* ************************************************************************* */ TEST(GaussianBayesNet, NoisyOptimize) { @@ -124,7 +124,7 @@ TEST(GaussianBayesNet, NoisyOptimize) { Vector d; boost::tie(R, d) = noisyBayesNet.matrix(); // find matrix and RHS const Vector x = R.inverse() * d; - VectorValues expected = map_list_of(_x_, x.head(1))(_y_, x.tail(1)); + const VectorValues expected{{_x_, x.head(1)}, {_y_, x.tail(1)}}; VectorValues actual = noisyBayesNet.optimize(); EXPECT(assert_equal(expected, actual)); @@ -133,17 +133,16 @@ TEST(GaussianBayesNet, NoisyOptimize) { /* ************************************************************************* */ TEST( GaussianBayesNet, optimizeIncomplete ) { - static GaussianBayesNet incompleteBayesNet = list_of - (GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1)); + static GaussianBayesNet incompleteBayesNet; + incompleteBayesNet.emplace_shared( + _x_, Vector1::Constant(9), I_1x1, _y_, I_1x1); - VectorValues solutionForMissing = map_list_of - (_y_, Vector1::Constant(5)); + VectorValues solutionForMissing { {_y_, Vector1::Constant(5)} }; VectorValues actual = incompleteBayesNet.optimize(solutionForMissing); - VectorValues expected = map_list_of - (_x_, Vector1::Constant(4)) - (_y_, Vector1::Constant(5)); + VectorValues expected{{_x_, Vector1::Constant(4)}, + {_y_, Vector1::Constant(5)}}; EXPECT(assert_equal(expected,actual)); } @@ -156,14 +155,11 @@ TEST( GaussianBayesNet, optimize3 ) // 5 1 5 // NOTE: we are supplying a new RHS here - VectorValues expected = map_list_of - (_x_, Vector1::Constant(-1)) - (_y_, Vector1::Constant(5)); + VectorValues expected { {_x_, Vector1::Constant(-1)}, + {_y_, Vector1::Constant(5)} }; // Test different RHS version - VectorValues gx = map_list_of - (_x_, Vector1::Constant(4)) - (_y_, Vector1::Constant(5)); + VectorValues gx{{_x_, Vector1::Constant(4)}, {_y_, Vector1::Constant(5)}}; VectorValues actual = smallBayesNet.backSubstitute(gx); EXPECT(assert_equal(expected, actual)); } @@ -173,9 +169,9 @@ namespace sampling { static Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); static const Vector2 mean(20, 40), b(10, 10); static const double sigma = 0.01; -static const GaussianBayesNet gbn = - list_of(GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma))( - GaussianDensity::FromMeanAndStddev(X(1), mean, sigma)); +static const GaussianBayesNet gbn = { + GaussianConditional::sharedMeanAndStddev(X(0), A1, X(1), b, sigma), + GaussianDensity::sharedMeanAndStddev(X(1), mean, sigma)}; } // namespace sampling /* ************************************************************************* */ @@ -250,13 +246,9 @@ TEST( GaussianBayesNet, backSubstituteTranspose ) // x=R'*y, expected=inv(R')*x // 2 = 1 2 // 5 1 1 3 - VectorValues - x = map_list_of - (_x_, Vector1::Constant(2)) - (_y_, Vector1::Constant(5)), - expected = map_list_of - (_x_, Vector1::Constant(2)) - (_y_, Vector1::Constant(3)); + const VectorValues x{{_x_, Vector1::Constant(2)}, + {_y_, Vector1::Constant(5)}}, + expected{{_x_, Vector1::Constant(2)}, {_y_, Vector1::Constant(3)}}; VectorValues actual = smallBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); @@ -273,13 +265,8 @@ TEST( GaussianBayesNet, backSubstituteTransposeNoisy ) // x=R'*y, expected=inv(R')*x // 2 = 1 2 // 5 1 1 3 - VectorValues - x = map_list_of - (_x_, Vector1::Constant(2)) - (_y_, Vector1::Constant(5)), - expected = map_list_of - (_x_, Vector1::Constant(4)) - (_y_, Vector1::Constant(9)); + VectorValues x{{_x_, Vector1::Constant(2)}, {_y_, Vector1::Constant(5)}}, + expected{{_x_, Vector1::Constant(4)}, {_y_, Vector1::Constant(9)}}; VectorValues actual = noisyBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 18b4674b5e..ba499ea24d 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -23,43 +23,45 @@ #include #include -#include -#include // for operator += -#include // for operator += #include +#include -using namespace boost::assign; using namespace std::placeholders; using namespace std; using namespace gtsam; +using Pairs = std::vector>; + namespace { const Key x1=1, x2=2, x3=3, x4=4; const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); - const GaussianFactorGraph chain = list_of - (JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x1, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) - (JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x3, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) - (JacobianFactor(x3, (Matrix(1, 1) << 1.).finished(), x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) - (JacobianFactor(x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)); - const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); + const GaussianFactorGraph chain = { + boost::make_shared( + x2, I_1x1, x1, I_1x1, (Vector(1) << 1.).finished(), chainNoise), + boost::make_shared( + x2, I_1x1, x3, I_1x1, (Vector(1) << 1.).finished(), chainNoise), + boost::make_shared( + x3, I_1x1, x4, I_1x1, (Vector(1) << 1.).finished(), chainNoise), + boost::make_shared( + x4, I_1x1, (Vector(1) << 1.).finished(), chainNoise)}; + const Ordering chainOrdering {x2, x1, x3, x4}; /* ************************************************************************* */ // Helper functions for below - GaussianBayesTreeClique::shared_ptr MakeClique(const GaussianConditional& conditional) + GaussianBayesTreeClique::shared_ptr LeafClique(const GaussianConditional& conditional) { return boost::make_shared( boost::make_shared(conditional)); } - template + typedef std::vector Children; + GaussianBayesTreeClique::shared_ptr MakeClique( - const GaussianConditional& conditional, const CHILDREN& children) + const GaussianConditional& conditional, const Children& children) { - GaussianBayesTreeClique::shared_ptr clique = - boost::make_shared( - boost::make_shared(conditional)); + auto clique = LeafClique(conditional); clique->children.assign(children.begin(), children.end()); - for(typename CHILDREN::const_iterator child = children.begin(); child != children.end(); ++child) + for(Children::const_iterator child = children.begin(); child != children.end(); ++child) (*child)->parent_ = clique; return clique; } @@ -90,23 +92,25 @@ TEST( GaussianBayesTree, eliminate ) EXPECT_LONGS_EQUAL(3, scatter.at(2).key); EXPECT_LONGS_EQUAL(4, scatter.at(3).key); - Matrix two = (Matrix(1, 1) << 2.).finished(); - Matrix one = (Matrix(1, 1) << 1.).finished(); + const Matrix two = (Matrix(1, 1) << 2.).finished(); + const Matrix one = I_1x1; + + const GaussianConditional gc1( + std::map{ + {x3, (Matrix21() << 2., 0.).finished()}, + {x4, (Matrix21() << 2., 2.).finished()}, + }, + 2, Vector2(2., 2.)); + const GaussianConditional gc2( + Pairs{ + {x2, (Matrix21() << -2. * sqrt(2.), 0.).finished()}, + {x1, (Matrix21() << -sqrt(2.), -sqrt(2.)).finished()}, + {x3, (Matrix21() << -sqrt(2.), sqrt(2.)).finished()}, + }, + 2, (Vector(2) << -2. * sqrt(2.), 0.).finished()); GaussianBayesTree bayesTree_expected; - bayesTree_expected.insertRoot( - MakeClique( - GaussianConditional( - pair_list_of(x3, (Matrix21() << 2., 0.).finished())( - x4, (Matrix21() << 2., 2.).finished()), 2, Vector2(2., 2.)), - list_of( - MakeClique( - GaussianConditional( - pair_list_of(x2, - (Matrix21() << -2. * sqrt(2.), 0.).finished())(x1, - (Matrix21() << -sqrt(2.), -sqrt(2.)).finished())(x3, - (Matrix21() << -sqrt(2.), sqrt(2.)).finished()), 2, - (Vector(2) << -2. * sqrt(2.), 0.).finished()))))); + bayesTree_expected.insertRoot(MakeClique(gc1, Children{LeafClique(gc2)})); EXPECT(assert_equal(bayesTree_expected, bt)); } @@ -114,11 +118,10 @@ TEST( GaussianBayesTree, eliminate ) /* ************************************************************************* */ TEST( GaussianBayesTree, optimizeMultiFrontal ) { - VectorValues expected = pair_list_of - (x1, (Vector(1) << 0.).finished()) - (x2, (Vector(1) << 1.).finished()) - (x3, (Vector(1) << 0.).finished()) - (x4, (Vector(1) << 1.).finished()); + VectorValues expected = {{x1, (Vector(1) << 0.).finished()}, + {x2, (Vector(1) << 1.).finished()}, + {x3, (Vector(1) << 0.).finished()}, + {x4, (Vector(1) << 1.).finished()}}; VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize(); EXPECT(assert_equal(expected,actual)); @@ -126,40 +129,61 @@ TEST( GaussianBayesTree, optimizeMultiFrontal ) /* ************************************************************************* */ TEST(GaussianBayesTree, complicatedMarginal) { - // Create the conditionals to go in the BayesTree + const GaussianConditional gc1( + Pairs{{11, (Matrix(3, 1) << 0.0971, 0, 0).finished()}, + {12, (Matrix(3, 2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655) + .finished()}}, + 2, Vector3(0.2638, 0.1455, 0.1361)); + const GaussianConditional gc2( + Pairs{ + {9, (Matrix(3, 1) << 0.7952, 0, 0).finished()}, + {10, (Matrix(3, 2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797) + .finished()}, + {11, (Matrix(3, 1) << 0.6551, 0.1626, 0.1190).finished()}, + {12, (Matrix(3, 2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513) + .finished()}}, + 2, Vector3(0.4314, 0.9106, 0.1818)); + const GaussianConditional gc3( + Pairs{{7, (Matrix(3, 1) << 0.2551, 0, 0).finished()}, + {8, (Matrix(3, 2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575) + .finished()}, + {11, (Matrix(3, 1) << 0.8407, 0.2543, 0.8143).finished()}}, + 2, Vector3(0.3998, 0.2599, 0.8001)); + const GaussianConditional gc4( + Pairs{{5, (Matrix(3, 1) << 0.2435, 0, 0).finished()}, + {6, (Matrix(3, 2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0) + .finished()}, + // NOTE the non-upper-triangular form + // here since this test was written when we had column permutations + // from LDL. The code still works currently (does not enfore + // upper-triangularity in this case) but this test will need to be + // redone if this stops working in the future + {7, (Matrix(3, 1) << 0.5853, 0.5497, 0.9172).finished()}, + {8, (Matrix(3, 2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759) + .finished()}}, + 2, Vector3(0.8173, 0.8687, 0.0844)); + const GaussianConditional gc5( + Pairs{{3, (Matrix(3, 1) << 0.0540, 0, 0).finished()}, + {4, (Matrix(3, 2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371) + .finished()}, + {6, (Matrix(3, 2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020) + .finished()}}, + 2, Vector3(0.9619, 0.0046, 0.7749)); + const GaussianConditional gc6( + Pairs{{1, (Matrix(3, 1) << 0.2630, 0, 0).finished()}, + {2, (Matrix(3, 2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524) + .finished()}, + {5, (Matrix(3, 1) << 0.8258, 0.5383, 0.9961).finished()}}, + 2, Vector3(0.0782, 0.4427, 0.1067)); + + // Create the bayes tree: GaussianBayesTree bt; - bt.insertRoot( - MakeClique(GaussianConditional(pair_list_of (11, (Matrix(3,1) << 0.0971, 0, 0).finished()) - (12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655).finished()), - 2, Vector3(0.2638, 0.1455, 0.1361)), list_of - (MakeClique(GaussianConditional(pair_list_of (9, (Matrix(3,1) << 0.7952, 0, 0).finished()) - (10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797).finished()) - (11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190).finished()) - (12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513).finished()), - 2, Vector3(0.4314, 0.9106, 0.1818)))) - (MakeClique(GaussianConditional(pair_list_of (7, (Matrix(3,1) << 0.2551, 0, 0).finished()) - (8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575).finished()) - (11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143).finished()), - 2, Vector3(0.3998, 0.2599, 0.8001)), list_of - (MakeClique(GaussianConditional(pair_list_of (5, (Matrix(3,1) << 0.2435, 0, 0).finished()) - (6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0).finished()) - // NOTE the non-upper-triangular form - // here since this test was written when we had column permutations - // from LDL. The code still works currently (does not enfore - // upper-triangularity in this case) but this test will need to be - // redone if this stops working in the future - (7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172).finished()) - (8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759).finished()), - 2, Vector3(0.8173, 0.8687, 0.0844)), list_of - (MakeClique(GaussianConditional(pair_list_of (3, (Matrix(3,1) << 0.0540, 0, 0).finished()) - (4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371).finished()) - (6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020).finished()), - 2, Vector3(0.9619, 0.0046, 0.7749)))) - (MakeClique(GaussianConditional(pair_list_of (1, (Matrix(3,1) << 0.2630, 0, 0).finished()) - (2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524).finished()) - (5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961).finished()), - 2, Vector3(0.0782, 0.4427, 0.1067)))))))))); + bt.insertRoot(MakeClique( + gc1, Children{LeafClique(gc2), + MakeClique(gc3, Children{MakeClique( + gc4, Children{LeafClique(gc5), + LeafClique(gc6)})})})); // Marginal on 5 Matrix expectedCov = (Matrix(1,1) << 236.5166).finished(); @@ -201,61 +225,33 @@ double computeError(const GaussianBayesTree& gbt, const Vector10& values) { /* ************************************************************************* */ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { + const GaussianConditional gc1( + Pairs{{2, (Matrix(6, 2) << 31.0, 32.0, 0.0, 34.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0) + .finished()}, + {3, (Matrix(6, 2) << 35.0, 36.0, 37.0, 38.0, 41.0, 42.0, 0.0, 44.0, + 0.0, 0.0, 0.0, 0.0) + .finished()}, + {4, (Matrix(6, 2) << 0.0, 0.0, 0.0, 0.0, 45.0, 46.0, 47.0, 48.0, + 51.0, 52.0, 0.0, 54.0) + .finished()}}, + 3, (Vector(6) << 29.0, 30.0, 39.0, 40.0, 49.0, 50.0).finished()), + gc2(Pairs{{0, (Matrix(4, 2) << 3.0, 4.0, 0.0, 6.0, 0.0, 0.0, 0.0, 0.0) + .finished()}, + {1, (Matrix(4, 2) << 0.0, 0.0, 0.0, 0.0, 17.0, 18.0, 0.0, 20.0) + .finished()}, + {2, (Matrix(4, 2) << 0.0, 0.0, 0.0, 0.0, 21.0, 22.0, 23.0, 24.0) + .finished()}, + {3, (Matrix(4, 2) << 7.0, 8.0, 9.0, 10.0, 0.0, 0.0, 0.0, 0.0) + .finished()}, + {4, (Matrix(4, 2) << 11.0, 12.0, 13.0, 14.0, 25.0, 26.0, 27.0, + 28.0) + .finished()}}, + 2, (Vector(4) << 1.0, 2.0, 15.0, 16.0).finished()); // Create an arbitrary Bayes Tree GaussianBayesTree bt; - bt.insertRoot(MakeClique(GaussianConditional( - pair_list_of - (2, (Matrix(6, 2) << - 31.0,32.0, - 0.0,34.0, - 0.0,0.0, - 0.0,0.0, - 0.0,0.0, - 0.0,0.0).finished()) - (3, (Matrix(6, 2) << - 35.0,36.0, - 37.0,38.0, - 41.0,42.0, - 0.0,44.0, - 0.0,0.0, - 0.0,0.0).finished()) - (4, (Matrix(6, 2) << - 0.0,0.0, - 0.0,0.0, - 45.0,46.0, - 47.0,48.0, - 51.0,52.0, - 0.0,54.0).finished()), - 3, (Vector(6) << 29.0,30.0,39.0,40.0,49.0,50.0).finished()), list_of - (MakeClique(GaussianConditional( - pair_list_of - (0, (Matrix(4, 2) << - 3.0,4.0, - 0.0,6.0, - 0.0,0.0, - 0.0,0.0).finished()) - (1, (Matrix(4, 2) << - 0.0,0.0, - 0.0,0.0, - 17.0,18.0, - 0.0,20.0).finished()) - (2, (Matrix(4, 2) << - 0.0,0.0, - 0.0,0.0, - 21.0,22.0, - 23.0,24.0).finished()) - (3, (Matrix(4, 2) << - 7.0,8.0, - 9.0,10.0, - 0.0,0.0, - 0.0,0.0).finished()) - (4, (Matrix(4, 2) << - 11.0,12.0, - 13.0,14.0, - 25.0,26.0, - 27.0,28.0).finished()), - 2, (Vector(4) << 1.0,2.0,15.0,16.0).finished()))))); + bt.insertRoot(MakeClique(gc1, Children{LeafClique(gc2)})); // Compute the Hessian numerically Matrix hessian = numericalHessian( @@ -276,12 +272,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { Vector expected = gradient * step; // Known steepest descent point from Bayes' net version - VectorValues expectedFromBN = pair_list_of - (0, Vector2(0.000129034, 0.000688183)) - (1, Vector2(0.0109679, 0.0253767)) - (2, Vector2(0.0680441, 0.114496)) - (3, Vector2(0.16125, 0.241294)) - (4, Vector2(0.300134, 0.423233)); + VectorValues expectedFromBN{{0, Vector2(0.000129034, 0.000688183)}, + {1, Vector2(0.0109679, 0.0253767)}, + {2, Vector2(0.0680441, 0.114496)}, + {3, Vector2(0.16125, 0.241294)}, + {4, Vector2(0.300134, 0.423233)}}; // Compute the steepest descent point with the dogleg function VectorValues actual = bt.optimizeGradientSearch(); diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 36f7eb72df..7b56775601 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -44,6 +44,8 @@ static Matrix R = (Matrix(2, 2) << -12.1244, -5.1962, 0., 4.6904).finished(); +using Dims = std::vector; + /* ************************************************************************* */ TEST(GaussianConditional, constructor) { @@ -215,7 +217,7 @@ TEST( GaussianConditional, solve ) Vector sx1(2); sx1 << 1.0, 1.0; Vector sl1(2); sl1 << 1.0, 1.0; - VectorValues expected = {{1, expectedX} {2, sx1} {10, sl1}}; + VectorValues expected = {{1, expectedX}, {2, sx1}, {10, sl1}}; VectorValues solution = {{2, sx1}, // parents {10, sl1}}; @@ -228,7 +230,7 @@ TEST( GaussianConditional, solve ) TEST( GaussianConditional, solve_simple ) { // 2 variables, frontal has dim=4 - VerticalBlockMatrix blockMatrix(list_of(4)(2)(1), 4); + VerticalBlockMatrix blockMatrix(Dims{4, 2, 1}, 4); blockMatrix.matrix() << 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, @@ -236,7 +238,7 @@ TEST( GaussianConditional, solve_simple ) 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4; // solve system as a non-multifrontal version first - GaussianConditional cg(list_of(1)(2), 1, blockMatrix); + GaussianConditional cg(KeyVector{1,2}, 1, blockMatrix); // partial solution Vector sx1 = Vector2(9.0, 10.0); @@ -260,7 +262,7 @@ TEST( GaussianConditional, solve_simple ) TEST( GaussianConditional, solve_multifrontal ) { // create full system, 3 variables, 2 frontals, all 2 dim - VerticalBlockMatrix blockMatrix(list_of(2)(2)(2)(1), 4); + VerticalBlockMatrix blockMatrix(Dims{2, 2, 2, 1}, 4); blockMatrix.matrix() << 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, @@ -268,7 +270,7 @@ TEST( GaussianConditional, solve_multifrontal ) 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4; // 3 variables, all dim=2 - GaussianConditional cg(list_of(1)(2)(10), 2, blockMatrix); + GaussianConditional cg(KeyVector{1, 2, 10}, 2, blockMatrix); EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.d())); @@ -305,9 +307,9 @@ TEST( GaussianConditional, solveTranspose ) { d2(0) = 5; // define nodes and specify in reverse topological sort (i.e. parents last) - GaussianBayesNet cbn = list_of - (GaussianConditional(1, d1, R11, 2, S12)) - (GaussianConditional(1, d2, R22)); + GaussianBayesNet cbn; + cbn.emplace_shared(1, d1, R11, 2, S12); + cbn.emplace_shared(1, d2, R22); // x=R'*y, y=inv(R')*x // 2 = 1 2 @@ -375,7 +377,7 @@ TEST(GaussianConditional, FromMeanAndStddev) { const Vector2 b(20, 40), x0(1, 2), x1(3, 4), x2(5, 6); const double sigma = 3; - VectorValues values = {{X(0), x0}, {X(1), x1}, {X(2), x2}; + VectorValues values{{X(0), x0}, {X(1), x1}, {X(2), x2}}; auto conditional1 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma); diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 221f499565..ca9b31a1b8 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -208,8 +208,9 @@ TEST(GaussianFactorGraph, gradient) { // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + // 25*(l1-x2-[-0.2;0.3])^2 // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] - VectorValues expected = map_list_of(1, Vector2(5.0, -12.5))(2, Vector2(30.0, 5.0))( - 0, Vector2(-25.0, 17.5)); + VectorValues expected{{1, Vector2(5.0, -12.5)}, + {2, Vector2(30.0, 5.0)}, + {0, Vector2(-25.0, 17.5)}}; // Check the gradient at delta=0 VectorValues zero = VectorValues::Zero(expected); @@ -221,14 +222,14 @@ TEST(GaussianFactorGraph, gradient) { VectorValues solution = fg.optimize(); VectorValues actual2 = fg.gradient(solution); EXPECT(assert_equal(VectorValues::Zero(solution), actual2)); -} + } /* ************************************************************************* */ TEST(GaussianFactorGraph, transposeMultiplication) { GaussianFactorGraph A = createSimpleGaussianFactorGraph(); - Errors e; - e += Vector2(0.0, 0.0), Vector2(15.0, 0.0), Vector2(0.0, -5.0), Vector2(-7.5, -5.0); + Errors e = std::list{Vector2(0.0, 0.0), Vector2(15.0, 0.0), + Vector2(0.0, -5.0), Vector2(-7.5, -5.0)}; VectorValues expected; expected.insert(1, Vector2(-37.5, -50.0)); @@ -284,7 +285,7 @@ TEST(GaussianFactorGraph, matrices2) { TEST(GaussianFactorGraph, multiplyHessianAdd) { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - VectorValues x = map_list_of(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6)); + const VectorValues x{{0, Vector2(1, 2)}, {1, Vector2(3, 4)}, {2, Vector2(5, 6)}}; VectorValues expected; expected.insert(0, Vector2(-450, -450)); @@ -303,8 +304,8 @@ TEST(GaussianFactorGraph, multiplyHessianAdd) { /* ************************************************************************* */ static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - gfg += HessianFactor(1, 2, 100*I_2x2, Z_2x2, Vector2(0.0, 1.0), - 400*I_2x2, Vector2(1.0, 1.0), 3.0); + gfg.emplace_shared(1, 2, 100 * I_2x2, Z_2x2, Vector2(0.0, 1.0), + 400 * I_2x2, Vector2(1.0, 1.0), 3.0); return gfg; } @@ -322,8 +323,7 @@ TEST(GaussianFactorGraph, multiplyHessianAdd2) { Y << -450, -450, 300, 400, 2950, 3450; EXPECT(assert_equal(Y, AtA * X)); - VectorValues x = map_list_of(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6)); - + const VectorValues x {{0, Vector2(1, 2)}, {1, Vector2(3, 4)}, {2, Vector2(5, 6)}}; VectorValues expected; expected.insert(0, Vector2(-450, -450)); expected.insert(1, Vector2(300, 400)); @@ -367,10 +367,10 @@ TEST(GaussianFactorGraph, gradientAtZero) { /* ************************************************************************* */ TEST(GaussianFactorGraph, clone) { // 2 variables, frontal has dim=4 - VerticalBlockMatrix blockMatrix(list_of(4)(2)(1), 4); + VerticalBlockMatrix blockMatrix(KeyVector{4, 2, 1}, 4); blockMatrix.matrix() << 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4; - GaussianConditional cg(list_of(1)(2), 1, blockMatrix); + GaussianConditional cg(KeyVector{1, 2}, 1, blockMatrix); GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor(); init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 3e13ecf10a..92ffe71acc 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -25,11 +25,6 @@ #include -#include -#include -#include -using namespace boost::assign; - #include #include @@ -38,6 +33,8 @@ using namespace gtsam; const double tol = 1e-5; +using Dims = std::vector; // For constructing block matrices + /* ************************************************************************* */ TEST(HessianFactor, Slot) { @@ -61,8 +58,8 @@ TEST(HessianFactor, emptyConstructor) /* ************************************************************************* */ TEST(HessianFactor, ConversionConstructor) { - HessianFactor expected(list_of(0)(1), - SymmetricBlockMatrix(list_of(2)(4)(1), (Matrix(7,7) << + HessianFactor expected(KeyVector{0, 1}, + SymmetricBlockMatrix(Dims{2, 4, 1}, (Matrix(7,7) << 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000, -25.0000, 0.0, 25.0000, 0.0, 0.0, 0.0, -5.0000, @@ -85,9 +82,7 @@ TEST(HessianFactor, ConversionConstructor) HessianFactor actual(jacobian); - VectorValues values = pair_list_of - (0, Vector2(1.0, 2.0)) - (1, (Vector(4) << 3.0, 4.0, 5.0, 6.0).finished()); + VectorValues values{{0, Vector2(1.0, 2.0)}, {1, Vector4(3.0, 4.0, 5.0, 6.0)}}; EXPECT_LONGS_EQUAL(2, (long)actual.size()); EXPECT(assert_equal(expected, actual, 1e-9)); @@ -108,7 +103,7 @@ TEST(HessianFactor, Constructor1) EXPECT(assert_equal(g, Vector(factor.linearTerm()))); EXPECT_LONGS_EQUAL(1, (long)factor.size()); - VectorValues dx = pair_list_of(0, Vector2(1.5, 2.5)); + VectorValues dx{{0, Vector2(1.5, 2.5)}}; // error 0.5*(f - 2*x'*g + x'*G*x) double expected = 80.375; @@ -150,9 +145,7 @@ TEST(HessianFactor, Constructor2) Vector dx0 = (Vector(1) << 0.5).finished(); Vector dx1 = Vector2(1.5, 2.5); - VectorValues dx = pair_list_of - (0, dx0) - (1, dx1); + VectorValues dx{{0, dx0}, {1, dx1}}; HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); @@ -171,10 +164,7 @@ TEST(HessianFactor, Constructor2) EXPECT(assert_equal(G22, factor.info().diagonalBlock(1))); // Check case when vector values is larger than factor - VectorValues dxLarge = pair_list_of - (0, dx0) - (1, dx1) - (2, Vector2(0.1, 0.2)); + VectorValues dxLarge {{0, dx0}, {1, dx1}, {2, Vector2(0.1, 0.2)}}; EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10); } @@ -200,11 +190,7 @@ TEST(HessianFactor, Constructor3) Vector dx1 = Vector2(1.5, 2.5); Vector dx2 = Vector3(1.5, 2.5, 3.5); - VectorValues dx = pair_list_of - (0, dx0) - (1, dx1) - (2, dx2); - + VectorValues dx {{0, dx0}, {1, dx1}, {2, dx2}}; HessianFactor factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); double expected = 371.3750; @@ -247,10 +233,7 @@ TEST(HessianFactor, ConstructorNWay) Vector dx1 = Vector2(1.5, 2.5); Vector dx2 = Vector3(1.5, 2.5, 3.5); - VectorValues dx = pair_list_of - (0, dx0) - (1, dx1) - (2, dx2); + VectorValues dx {{0, dx0}, {1, dx1}, {2, dx2}}; KeyVector js {0, 1, 2}; std::vector Gs; @@ -309,7 +292,7 @@ TEST(HessianFactor, CombineAndEliminate1) { hessian.augmentedInformation(), 1e-9)); // perform elimination on jacobian - Ordering ordering = list_of(1); + Ordering ordering {1}; GaussianConditional::shared_ptr expectedConditional; JacobianFactor::shared_ptr expectedFactor; boost::tie(expectedConditional, expectedFactor) = jacobian.eliminate(ordering); @@ -372,7 +355,7 @@ TEST(HessianFactor, CombineAndEliminate2) { hessian.augmentedInformation(), 1e-9)); // perform elimination on jacobian - Ordering ordering = list_of(0); + Ordering ordering {0}; GaussianConditional::shared_ptr expectedConditional; JacobianFactor::shared_ptr expectedFactor; boost::tie(expectedConditional, expectedFactor) = // @@ -437,10 +420,10 @@ TEST(HessianFactor, eliminate2 ) // eliminate the combined factor HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined)); - GaussianFactorGraph combinedLFG_Chol = list_of(combinedLF_Chol); + GaussianFactorGraph combinedLFG_Chol {combinedLF_Chol}; std::pair actual_Chol = - EliminateCholesky(combinedLFG_Chol, Ordering(list_of(0))); + EliminateCholesky(combinedLFG_Chol, Ordering{0}); // create expected Conditional Gaussian double oldSigma = 0.0894427; // from when R was made unit @@ -483,8 +466,8 @@ TEST(HessianFactor, combine) { 0.0, -8.94427191).finished(); Vector b = Vector2(2.23606798,-1.56524758); SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector::Ones(2)); - GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model)); - GaussianFactorGraph factors = list_of(f); + GaussianFactorGraph factors{ + boost::make_shared(0, A0, 1, A1, 2, A2, b, model)}; // Form Ab' * Ab HessianFactor actual(factors); @@ -514,7 +497,7 @@ TEST(HessianFactor, gradientAtZero) HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); // test gradient at zero - VectorValues expectedG = pair_list_of(0, -g1) (1, -g2); + VectorValues expectedG{{0, -g1}, {1, -g2}}; Matrix A; Vector b; boost::tie(A,b) = factor.jacobian(); KeyVector keys {0, 1}; EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys))); @@ -537,7 +520,7 @@ TEST(HessianFactor, gradient) // test gradient Vector x0 = (Vector(1) << 3.0).finished(); Vector x1 = (Vector(2) << -3.5, 7.1).finished(); - VectorValues x = pair_list_of(0, x0) (1, x1); + VectorValues x {{0, x0}, {1, x1}}; Vector expectedGrad0 = (Vector(1) << 10.0).finished(); Vector expectedGrad1 = (Vector(2) << 4.5, 16.1).finished(); diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 21146066d7..e0f4d8a0b9 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -25,26 +25,24 @@ #include #include -#include -#include #include #include using namespace std; using namespace gtsam; -using namespace boost::assign; + +using Dims = std::vector; // For constructing block matrices namespace { namespace simple { // Terms we'll use - const vector > terms = list_of > - (make_pair(5, Matrix3::Identity())) - (make_pair(10, 2*Matrix3::Identity())) - (make_pair(15, 3*Matrix3::Identity())); - - // RHS and sigmas - const Vector b = Vector3(1., 2., 3.); - const SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.5)); + const vector > terms{ + {5, I_3x3}, {10, 2 * I_3x3}, {15, 3 * I_3x3}}; + + // RHS and sigmas + const Vector b = Vector3(1., 2., 3.); + const SharedDiagonal noise = + noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.5)); } } @@ -128,7 +126,7 @@ TEST(JacobianFactor, constructors_and_accessors) // VerticalBlockMatrix constructor JacobianFactor expected( boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, noise); - VerticalBlockMatrix blockMatrix(list_of(3)(3)(3)(1), 3); + VerticalBlockMatrix blockMatrix(Dims{3, 3, 3, 1}, 3); blockMatrix(0) = terms[0].second; blockMatrix(1) = terms[1].second; blockMatrix(2) = terms[2].second; @@ -191,22 +189,25 @@ Key keyX(10), keyY(8), keyZ(12); double sigma1 = 0.1; Matrix A11 = I_2x2; Vector2 b1(2, -1); -JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1)); +auto factor1 = boost::make_shared( + keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1)); double sigma2 = 0.5; Matrix A21 = -2 * I_2x2; Matrix A22 = 3 * I_2x2; Vector2 b2(4, -5); -JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2)); +auto factor2 = boost::make_shared( + keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2)); double sigma3 = 1.0; Matrix A32 = -4 * I_2x2; Matrix A33 = 5 * I_2x2; Vector2 b3(3, -6); -JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3)); +auto factor3 = boost::make_shared( + keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3)); -GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3)); -Ordering ordering(list_of(keyX)(keyY)(keyZ)); +const GaussianFactorGraph factors { factor1, factor2, factor3 }; +const Ordering ordering { keyX, keyY, keyZ }; } /* ************************************************************************* */ @@ -436,11 +437,11 @@ TEST(JacobianFactor, eliminate) Vector9 sigmas; sigmas << s1, s0, s2; JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); - GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(list_of(0)); + GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(Ordering{0}); JacobianFactor::shared_ptr expectedJacobian = boost::dynamic_pointer_cast< JacobianFactor>(expected.second); - GaussianFactorGraph::EliminationResult actual = EliminateQR(gfg, list_of(0)); + GaussianFactorGraph::EliminationResult actual = EliminateQR(gfg, Ordering{0}); JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast< JacobianFactor>(actual.second); @@ -487,7 +488,7 @@ TEST(JacobianFactor, eliminate2 ) // eliminate the combined factor pair - actual = combined.eliminate(Ordering(list_of(2))); + actual = combined.eliminate(Ordering{2}); // create expected Conditional Gaussian double oldSigma = 0.0894427; // from when R was made unit @@ -539,11 +540,11 @@ TEST(JacobianFactor, EliminateQR) // Create factor graph const SharedDiagonal sig_4D = noiseModel::Isotropic::Sigma(4, 0.5); const SharedDiagonal sig_2D = noiseModel::Isotropic::Sigma(2, 0.5); - GaussianFactorGraph factors = list_of - (JacobianFactor(list_of(3)(5)(7)(9)(11), VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), Ab.block(0, 0, 4, 11)), sig_4D)) - (JacobianFactor(list_of(5)(7)(9)(11), VerticalBlockMatrix(list_of(2)(2)(2)(2)(1), Ab.block(4, 2, 4, 9)), sig_4D)) - (JacobianFactor(list_of(7)(9)(11), VerticalBlockMatrix(list_of(2)(2)(2)(1), Ab.block(8, 4, 4, 7)), sig_4D)) - (JacobianFactor(list_of(11), VerticalBlockMatrix(list_of(2)(1), Ab.block(12, 8, 2, 3)), sig_2D)); + GaussianFactorGraph factors = { + boost::make_shared(KeyVector{3, 5, 7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 2, 2, 1}, Ab.block(0, 0, 4, 11)), sig_4D), + boost::make_shared(KeyVector{5, 7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 2, 1}, Ab.block(4, 2, 4, 9)), sig_4D), + boost::make_shared(KeyVector{7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 1}, Ab.block(8, 4, 4, 7)), sig_4D), + boost::make_shared(KeyVector{11}, VerticalBlockMatrix(Dims{2, 1}, Ab.block(12, 8, 2, 3)), sig_2D)}; // extract the dense matrix for the graph Matrix actualDense = factors.augmentedJacobian(); @@ -562,12 +563,12 @@ TEST(JacobianFactor, EliminateQR) 0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476, 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090).finished(); - GaussianConditional expectedFragment(list_of(3)(5)(7)(9)(11), 3, - VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R.topRows(6)), + GaussianConditional expectedFragment(KeyVector{3,5,7,9,11}, 3, + VerticalBlockMatrix(Dims{2, 2, 2, 2, 2, 1}, R.topRows(6)), noiseModel::Unit::Create(6)); // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!! - GaussianFactorGraph::EliminationResult actual = EliminateQR(factors, list_of(3)(5)(7)); + GaussianFactorGraph::EliminationResult actual = EliminateQR(factors, Ordering{3, 5, 7}); const JacobianFactor &actualJF = dynamic_cast(*actual.second); EXPECT(assert_equal(expectedFragment, *actual.first, 0.001)); @@ -589,7 +590,7 @@ TEST ( JacobianFactor, constraint_eliminate1 ) // eliminate it pair - actual = lc.eliminate(list_of(1)); + actual = lc.eliminate(Ordering{1}); // verify linear factor is a null ptr EXPECT(actual.second->empty()); @@ -617,7 +618,7 @@ TEST ( JacobianFactor, constraint_eliminate2 ) // eliminate x and verify results pair - actual = lc.eliminate(list_of(1)); + actual = lc.eliminate(Ordering{1}); // LF should be empty // It's tricky to create Eigen matrices that are only zero along one dimension @@ -648,7 +649,7 @@ TEST(JacobianFactor, OverdeterminedEliminate) { SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); JacobianFactor factor(0, Ab.leftCols(3), Ab.col(3), diagonal); - GaussianFactorGraph::EliminationResult actual = factor.eliminate(list_of(0)); + GaussianFactorGraph::EliminationResult actual = factor.eliminate(Ordering{0}); Matrix expectedRd(3, 4); expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131, // diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index f83ba7831b..442923662f 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -22,15 +22,12 @@ #include -#include - #include #include using namespace std; using namespace gtsam; using namespace noiseModel; -using namespace boost::assign; static const double kSigma = 2, kInverseSigma = 1.0 / kSigma, kVariance = kSigma * kSigma, prc = 1.0 / kVariance; diff --git a/gtsam/linear/tests/testRegularHessianFactor.cpp b/gtsam/linear/tests/testRegularHessianFactor.cpp index 3441c6cc2a..2e4d2249e8 100644 --- a/gtsam/linear/tests/testRegularHessianFactor.cpp +++ b/gtsam/linear/tests/testRegularHessianFactor.cpp @@ -21,13 +21,8 @@ #include -#include -#include -#include - using namespace std; using namespace gtsam; -using namespace boost::assign; /* ************************************************************************* */ TEST(RegularHessianFactor, Constructors) @@ -36,8 +31,7 @@ TEST(RegularHessianFactor, Constructors) // 0.5*|x0 + x1 + x3 - [1;2]|^2 = 0.5*|A*x-b|^2, with A=[I I I] Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2; Vector2 b(1,2); - vector > terms; - terms += make_pair(0, A1), make_pair(1, A2), make_pair(3, A3); + const vector > terms {{0, A1}, {1, A2}, {3, A3}}; RegularJacobianFactor<2> jf(terms, b); // Test conversion from JacobianFactor @@ -65,8 +59,8 @@ TEST(RegularHessianFactor, Constructors) // Test n-way constructor KeyVector keys {0, 1, 3}; - vector Gs; Gs += G11, G12, G13, G22, G23, G33; - vector gs; gs += g1, g2, g3; + vector Gs {G11, G12, G13, G22, G23, G33}; + vector gs {g1, g2, g3}; RegularHessianFactor<2> factor3(keys, Gs, gs, f); EXPECT(assert_equal(factor, factor3)); @@ -82,7 +76,7 @@ TEST(RegularHessianFactor, Constructors) // Test constructor from Information matrix Matrix info = factor.augmentedInformation(); - vector dims; dims += 2, 2, 2; + vector dims {2, 2, 2}; SymmetricBlockMatrix sym(dims, info, true); RegularHessianFactor<2> factor6(keys, sym); EXPECT(assert_equal(factor, factor6)); @@ -97,10 +91,7 @@ TEST(RegularHessianFactor, Constructors) Vector Y(6); Y << 9, 12, 9, 12, 9, 12; EXPECT(assert_equal(Y,AtA*X)); - VectorValues x = map_list_of - (0, Vector2(1,2)) - (1, Vector2(3,4)) - (3, Vector2(5,6)); + VectorValues x{{0, Vector2(1, 2)}, {1, Vector2(3, 4)}, {3, Vector2(5, 6)}}; VectorValues expected; expected.insert(0, Y.segment<2>(0)); diff --git a/gtsam/linear/tests/testRegularJacobianFactor.cpp b/gtsam/linear/tests/testRegularJacobianFactor.cpp index b8c4aa689a..205d9d092d 100644 --- a/gtsam/linear/tests/testRegularJacobianFactor.cpp +++ b/gtsam/linear/tests/testRegularJacobianFactor.cpp @@ -24,14 +24,11 @@ #include -#include -#include #include #include using namespace std; using namespace gtsam; -using namespace boost::assign; static const size_t fixedDim = 3; static const size_t nrKeys = 3; @@ -40,10 +37,8 @@ static const size_t nrKeys = 3; namespace { namespace simple { // Terms we'll use - const vector > terms = list_of > - (make_pair(0, Matrix3::Identity())) - (make_pair(1, 2*Matrix3::Identity())) - (make_pair(2, 3*Matrix3::Identity())); + const vector > terms{ + {0, 1 * I_3x3}, {1, 2 * I_3x3}, {2, 3 * I_3x3}}; // RHS and sigmas const Vector b = (Vector(3) << 1., 2., 3.).finished(); @@ -52,10 +47,8 @@ namespace { namespace simple2 { // Terms - const vector > terms2 = list_of > - (make_pair(0, 2*Matrix3::Identity())) - (make_pair(1, 4*Matrix3::Identity())) - (make_pair(2, 6*Matrix3::Identity())); + const vector > terms2{ + {0, 2 * I_3x3}, {1, 4 * I_3x3}, {2, 6 * I_3x3}}; // RHS const Vector b2 = (Vector(3) << 2., 4., 6.).finished(); diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index ee21de364c..da093db977 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -22,9 +22,6 @@ #include #include -#include -using namespace boost::assign; - #include #include @@ -228,13 +225,13 @@ TEST(Serialization, gaussian_bayes_net) { /* ************************************************************************* */ TEST (Serialization, gaussian_bayes_tree) { const Key x1=1, x2=2, x3=3, x4=4; - const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); + const Ordering chainOrdering {x2, x1, x3, x4}; const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); - const GaussianFactorGraph chain = list_of - (JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x1, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) - (JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x3, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) - (JacobianFactor(x3, (Matrix(1, 1) << 1.).finished(), x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) - (JacobianFactor(x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)); + const GaussianFactorGraph chain = { + boost::make_shared(x2, (Matrix(1, 1) << 1.).finished(), x1, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise), + boost::make_shared(x2, (Matrix(1, 1) << 1.).finished(), x3, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise), + boost::make_shared(x3, (Matrix(1, 1) << 1.).finished(), x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise), + boost::make_shared(x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)}; GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering); GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering); diff --git a/gtsam/linear/tests/testSparseEigen.cpp b/gtsam/linear/tests/testSparseEigen.cpp index 225e1dab22..39c4f37e75 100644 --- a/gtsam/linear/tests/testSparseEigen.cpp +++ b/gtsam/linear/tests/testSparseEigen.cpp @@ -21,9 +21,6 @@ #include #include -#include -using boost::assign::list_of; - #include #include @@ -49,7 +46,7 @@ TEST(SparseEigen, sparseJacobianEigen) { EXPECT(assert_equal(gfg.augmentedJacobian(), Matrix(sparseResult))); // Call sparseJacobian with optional ordering... - auto ordering = Ordering(list_of(x45)(x123)); + const Ordering ordering{x45, x123}; // Eigen Sparse with optional ordering EXPECT(assert_equal(gfg.augmentedJacobian(ordering), diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index 5974f8320d..49fc56d193 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -21,14 +21,11 @@ #include -#include -#include #include #include using namespace std; -using namespace boost::assign; using boost::adaptors::map_keys; using namespace gtsam; From 6cbd7c286cea8299ad11facb04ece4a522750ed9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 19:25:34 -0800 Subject: [PATCH 09/37] Add initializer list constructors --- gtsam/symbolic/SymbolicBayesNet.h | 10 ++++++++++ gtsam/symbolic/SymbolicFactorGraph.h | 9 +++++++++ 2 files changed, 19 insertions(+) diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index d9b8ecb576..33c77c6dee 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -60,6 +60,16 @@ namespace gtsam { explicit SymbolicBayesNet(const FactorGraph& graph) : Base(graph) {} + /** + * Constructor that takes an initializer list of shared pointers. + * FactorGraph fg = {make_shared(), ...}; + */ + SymbolicBayesNet( + std::initializer_list> + sharedFactors) { + for (auto&& f : sharedFactors) factors_.push_back(f); + } + /// Destructor virtual ~SymbolicBayesNet() {} diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 36379fd831..95892eec87 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -81,6 +81,15 @@ namespace gtsam { template SymbolicFactorGraph(const FactorGraph& graph) : Base(graph) {} + /** + * Constructor that takes an initializer list of shared pointers. + * FactorGraph fg = {make_shared(), ...}; + */ + SymbolicFactorGraph(std::initializer_list> + sharedFactors) { + for (auto&& f : sharedFactors) factors_.push_back(f); + } + /// Destructor virtual ~SymbolicFactorGraph() {} From 8527b394ab7a11b2f230fe93549ff703ed51d435 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 19:26:56 -0800 Subject: [PATCH 10/37] Removed copy/pasted but unused boost::assign headers. --- gtsam/geometry/tests/testUnit3.cpp | 2 -- gtsam/linear/tests/testGaussianConditional.cpp | 1 - gtsam/navigation/ScenarioRunner.cpp | 2 -- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 3 --- gtsam/nonlinear/tests/testExpression.cpp | 4 ---- gtsam/nonlinear/tests/testLinearContainerFactor.cpp | 3 --- gtsam/nonlinear/tests/testValues.cpp | 4 ---- gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp | 2 -- gtsam/sfm/ShonanGaugeFactor.h | 2 -- gtsam/slam/dataset.cpp | 1 - gtsam/slam/tests/testGeneralSFMFactor.cpp | 2 -- gtsam/slam/tests/testInitializePose3.cpp | 1 - gtsam/slam/tests/testLago.cpp | 1 - gtsam/slam/tests/testOrientedPlane3Factor.cpp | 4 ---- gtsam/slam/tests/testRegularImplicitSchurFactor.cpp | 3 --- gtsam/slam/tests/testRotateFactor.cpp | 2 -- gtsam/slam/tests/testSerializationInSlam.cpp | 1 - gtsam/slam/tests/testSmartProjectionFactor.cpp | 3 --- gtsam/slam/tests/testSmartProjectionPoseFactor.cpp | 4 +--- gtsam/slam/tests/testSmartProjectionRigFactor.cpp | 2 -- gtsam/slam/tests/testTriangulationFactor.cpp | 3 --- gtsam/symbolic/SymbolicFactor.h | 1 - 22 files changed, 1 insertion(+), 50 deletions(-) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index c951d857c1..96a5522fb1 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -31,8 +31,6 @@ #include -#include - #include #include diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 7b56775601..e4bdbdffc4 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -26,7 +26,6 @@ #include #include -#include #include #include diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 9d3e258de9..19d241c955 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -18,11 +18,9 @@ #include #include -#include #include using namespace std; -using namespace boost::assign; namespace gtsam { diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 1423b473e7..d2cabeb85e 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -29,9 +29,6 @@ #include -#include -using boost::assign::list_of; -using boost::assign::map_list_of; namespace gtsam { diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 9987cca3f9..eea30c6669 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -25,10 +25,6 @@ #include -#include -using boost::assign::list_of; -using boost::assign::map_list_of; - using namespace std; using namespace gtsam; diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index a5015546f5..22ae4d73e6 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -15,10 +15,7 @@ #include #include -#include - using namespace std; -using namespace boost::assign; using namespace gtsam; const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector2(1.0, 1.0)); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index bed2a8af93..e31188e388 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -25,15 +25,11 @@ #include #include -#include // for operator += -#include -#include #include #include #include #include -using namespace boost::assign; using namespace std::placeholders; using namespace gtsam; using namespace std; diff --git a/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp b/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp index 877c9adbcf..c5ffd1ff79 100644 --- a/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp +++ b/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp @@ -18,8 +18,6 @@ #include #include -#include // for operator += -using namespace boost::assign; using namespace gtsam; using namespace std; diff --git a/gtsam/sfm/ShonanGaugeFactor.h b/gtsam/sfm/ShonanGaugeFactor.h index 4dc5b285e7..d814aafa10 100644 --- a/gtsam/sfm/ShonanGaugeFactor.h +++ b/gtsam/sfm/ShonanGaugeFactor.h @@ -22,8 +22,6 @@ #include #include -#include - namespace gtsam { /** * The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index f25d59ab7c..db79fcd3c0 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -41,7 +41,6 @@ #include #include -#include #include #include #include diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index e19a41b8df..ce7d7b6537 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -28,10 +28,8 @@ #include #include -#include #include #include -using namespace boost::assign; #include #include diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 995a109fa0..ed9e6bbbca 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -29,7 +29,6 @@ using namespace std; using namespace gtsam; -using namespace boost::assign; static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); static SharedNoiseModel model(noiseModel::Isotropic::Sigma(6, 0.1)); diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 781317d7a9..0b74f06d7b 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -31,7 +31,6 @@ using namespace std; using namespace gtsam; -using namespace boost::assign; static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 502c62c11c..bc3f7ce94b 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -25,10 +25,6 @@ #include -#include -#include - -using namespace boost::assign; using namespace std::placeholders; using namespace gtsam; using namespace std; diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 09ee61c97b..8819707d80 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -27,14 +27,11 @@ #include #include -#include -#include #include #include #include using namespace std; -using namespace boost::assign; using namespace gtsam; // F diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index b8fd01730e..01bfc866b4 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -12,11 +12,9 @@ #include #include -#include #include using namespace std; -using namespace boost::assign; using namespace std::placeholders; using namespace gtsam; diff --git a/gtsam/slam/tests/testSerializationInSlam.cpp b/gtsam/slam/tests/testSerializationInSlam.cpp index 6aec8ecb0d..08fb80c0a9 100644 --- a/gtsam/slam/tests/testSerializationInSlam.cpp +++ b/gtsam/slam/tests/testSerializationInSlam.cpp @@ -25,7 +25,6 @@ #include -#include #include namespace { diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 9ab55a8d3b..564b7f6403 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -23,11 +23,8 @@ #include #include #include -#include #include -using namespace boost::assign; - namespace { static const bool isDebugTest = false; static const Symbol l1('l', 1), l2('l', 2), l3('l', 3); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 5c38233c18..ba5a53bbb9 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -25,10 +25,8 @@ #include #include #include -#include #include -using namespace boost::assign; using namespace std::placeholders; namespace { @@ -437,7 +435,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { E(2, 0) = 10; E(2, 2) = 1; E(3, 1) = 10; - SmartFactor::FBlocks Fs = list_of(F1)(F2); + SmartFactor::FBlocks Fs {F1, F2}; Vector b(4); b.setZero(); diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index b142b20091..6d39eb5b03 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -25,13 +25,11 @@ #include #include -#include #include #include "smartFactorScenarios.h" #define DISABLE_TIMING -using namespace boost::assign; using namespace std::placeholders; static const double rankTol = 1.0; diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index ad88c88fcc..08d8e66c05 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -25,13 +25,10 @@ #include #include -#include -#include #include using namespace std; using namespace gtsam; -using namespace boost::assign; using namespace std::placeholders; // Some common constants diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 91dbe3464a..d0e8adb55f 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -22,7 +22,6 @@ #include #include -#include #include #include From 7e4b033ece741f370373a8e77120586825320a36 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 20:11:34 -0800 Subject: [PATCH 11/37] Using initializers for almost everything in gtsam now. --- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 3 +- gtsam/nonlinear/tests/testExpression.cpp | 32 +++--- gtsam/nonlinear/tests/testValues.cpp | 13 +-- gtsam/slam/tests/testGeneralSFMFactor.cpp | 4 +- .../tests/testRegularImplicitSchurFactor.cpp | 20 ++-- gtsam/symbolic/tests/symbolicExampleGraphs.h | 98 +++++++++---------- .../tests/testSymbolicConditional.cpp | 10 +- gtsam/symbolic/tests/testSymbolicFactor.cpp | 14 ++- .../tests/testSymbolicFactorGraph.cpp | 40 ++++---- gtsam/symbolic/tests/testSymbolicISAM.cpp | 5 +- .../tests/testSymbolicJunctionTree.cpp | 9 +- gtsam/symbolic/tests/testVariableIndex.cpp | 10 +- gtsam_unstable/discrete/Constraint.h | 5 +- gtsam_unstable/slam/BetweenFactorEM.h | 2 +- gtsam_unstable/slam/DummyFactor.cpp | 2 +- .../slam/TransformBtwRobotsUnaryFactor.h | 2 +- .../slam/TransformBtwRobotsUnaryFactorEM.h | 2 +- 17 files changed, 121 insertions(+), 150 deletions(-) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index d2cabeb85e..2deece2282 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -250,8 +250,7 @@ TEST(AdaptAutoDiff, SnavelyExpression) { internal::upAligned(RecordSize) + P.traceSize() + X.traceSize(), expression.traceSize()); - set expected = list_of(1)(2); - + const set expected{1, 2}; EXPECT(expected == expression.keys()); } diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index eea30c6669..a93f8a0e13 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -86,7 +86,7 @@ Vector f3(const Point3& p, OptionalJacobian H) { return p; } Point3_ pointExpression(1); -set expected = list_of(1); +const set expected{1}; } // namespace unary // Create a unary expression that takes another expression as a single argument. @@ -186,14 +186,14 @@ TEST(Expression, BinaryToDouble) { /* ************************************************************************* */ // Check keys of an expression created from class method. TEST(Expression, BinaryKeys) { - set expected = list_of(1)(2); + const set expected{1, 2}; EXPECT(expected == binary::p_cam.keys()); } /* ************************************************************************* */ // Check dimensions by calling `dims` method. TEST(Expression, BinaryDimensions) { - map actual, expected = map_list_of(1, 6)(2, 3); + map actual, expected{{1, 6}, {2, 3}}; binary::p_cam.dims(actual); EXPECT(actual == expected); } @@ -223,14 +223,14 @@ Expression uv_hat(uncalibrate, K, projection); /* ************************************************************************* */ // keys TEST(Expression, TreeKeys) { - set expected = list_of(1)(2)(3); + const set expected{1, 2, 3}; EXPECT(expected == tree::uv_hat.keys()); } /* ************************************************************************* */ // dimensions TEST(Expression, TreeDimensions) { - map actual, expected = map_list_of(1, 6)(2, 3)(3, 5); + map actual, expected{{1, 6}, {2, 3}, {3, 5}}; tree::uv_hat.dims(actual); EXPECT(actual == expected); } @@ -261,7 +261,7 @@ TEST(Expression, compose1) { Rot3_ R3 = R1 * R2; // Check keys - set expected = list_of(1)(2); + const set expected{1, 2}; EXPECT(expected == R3.keys()); } @@ -273,7 +273,7 @@ TEST(Expression, compose2) { Rot3_ R3 = R1 * R2; // Check keys - set expected = list_of(1); + const set expected{1}; EXPECT(expected == R3.keys()); } @@ -285,7 +285,7 @@ TEST(Expression, compose3) { Rot3_ R3 = R1 * R2; // Check keys - set expected = list_of(3); + const set expected{3}; EXPECT(expected == R3.keys()); } @@ -298,7 +298,7 @@ TEST(Expression, compose4) { Double_ R3 = R1 * R2; // Check keys - set expected = list_of(1); + const set expected{1}; EXPECT(expected == R3.keys()); } @@ -322,7 +322,7 @@ TEST(Expression, ternary) { Rot3_ ABC(composeThree, A, B, C); // Check keys - set expected = list_of(1)(2)(3); + const set expected {1, 2, 3}; EXPECT(expected == ABC.keys()); } @@ -332,10 +332,10 @@ TEST(Expression, ScalarMultiply) { const Key key(67); const Point3_ expr = 23 * Point3_(key); - set expected_keys = list_of(key); + const set expected_keys{key}; EXPECT(expected_keys == expr.keys()); - map actual_dims, expected_dims = map_list_of(key, 3); + map actual_dims, expected_dims {{key, 3}}; expr.dims(actual_dims); EXPECT(actual_dims == expected_dims); @@ -363,10 +363,10 @@ TEST(Expression, BinarySum) { const Key key(67); const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1)); - set expected_keys = list_of(key); + const set expected_keys{key}; EXPECT(expected_keys == sum_.keys()); - map actual_dims, expected_dims = map_list_of(key, 3); + map actual_dims, expected_dims {{key, 3}}; sum_.dims(actual_dims); EXPECT(actual_dims == expected_dims); @@ -458,7 +458,7 @@ TEST(Expression, UnaryOfSum) { const Point3_ sum_ = Point3_(key1) + Point3_(key2); const Double_ norm_(>sam::norm3, sum_); - map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); + map actual_dims, expected_dims = {{key1, 3}, {key2, 3}}; norm_.dims(actual_dims); EXPECT(actual_dims == expected_dims); @@ -481,7 +481,7 @@ TEST(Expression, WeightedSum) { const Key key1(42), key2(67); const Point3_ weighted_sum_ = 17 * Point3_(key1) + 23 * Point3_(key2); - map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); + map actual_dims, expected_dims {{key1, 3}, {key2, 3}}; weighted_sum_.dims(actual_dims); EXPECT(actual_dims == expected_dims); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index e31188e388..2465903db1 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -220,9 +220,8 @@ TEST(Values, retract_full) config0.insert(key1, Vector3(1.0, 2.0, 3.0)); config0.insert(key2, Vector3(5.0, 6.0, 7.0)); - VectorValues delta = pair_list_of - (key1, Vector3(1.0, 1.1, 1.2)) - (key2, Vector3(1.3, 1.4, 1.5)); + VectorValues delta {{key1, Vector3(1.0, 1.1, 1.2)}, + {key2, Vector3(1.3, 1.4, 1.5)}}; Values expected; expected.insert(key1, Vector3(2.0, 3.1, 4.2)); @@ -239,8 +238,7 @@ TEST(Values, retract_partial) config0.insert(key1, Vector3(1.0, 2.0, 3.0)); config0.insert(key2, Vector3(5.0, 6.0, 7.0)); - VectorValues delta = pair_list_of - (key2, Vector3(1.3, 1.4, 1.5)); + VectorValues delta {{key2, Vector3(1.3, 1.4, 1.5)}}; Values expected; expected.insert(key1, Vector3(1.0, 2.0, 3.0)); @@ -275,9 +273,8 @@ TEST(Values, localCoordinates) valuesA.insert(key1, Vector3(1.0, 2.0, 3.0)); valuesA.insert(key2, Vector3(5.0, 6.0, 7.0)); - VectorValues expDelta = pair_list_of - (key1, Vector3(0.1, 0.2, 0.3)) - (key2, Vector3(0.4, 0.5, 0.6)); + VectorValues expDelta{{key1, Vector3(0.1, 0.2, 0.3)}, + {key2, Vector3(0.4, 0.5, 0.6)}}; Values valuesB = valuesA.retract(expDelta); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index ce7d7b6537..23ddb82073 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -454,8 +454,8 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) { using namespace noiseModel; Rot2 R = Rot2::fromAngle(0.3); Matrix2 cov = R.matrix() * R.matrix().transpose(); - models += SharedNoiseModel(), Unit::Create(2), // - Isotropic::Sigma(2, 0.5), Constrained::All(2), Gaussian::Covariance(cov); + models = {SharedNoiseModel(), Unit::Create(2), Isotropic::Sigma(2, 0.5), + Constrained::All(2), Gaussian::Covariance(cov)}; } // Now loop over all these noise models diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 8819707d80..9f39082a5f 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -65,17 +65,15 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { Matrix3 P = (E.transpose() * E).inverse(); double alpha = 0.5; - VectorValues xvalues = map_list_of // - (0, Vector::Constant(6, 2))// - (1, Vector::Constant(6, 4))// - (2, Vector::Constant(6, 0))// distractor - (3, Vector::Constant(6, 8)); - - VectorValues yExpected = map_list_of// - (0, Vector::Constant(6, 27))// - (1, Vector::Constant(6, -40))// - (2, Vector::Constant(6, 0))// distractor - (3, Vector::Constant(6, 279)); + VectorValues xvalues{{0, Vector::Constant(6, 2)}, // + {1, Vector::Constant(6, 4)}, // + {2, Vector::Constant(6, 0)}, // distractor + {3, Vector::Constant(6, 8)}}; + + VectorValues yExpected{{0, Vector::Constant(6, 27)}, // + {1, Vector::Constant(6, -40)}, // + {2, Vector::Constant(6, 0)}, // distractor + {3, Vector::Constant(6, 279)}}; // Create full F size_t M=4, m = 3, d = 6; diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index b70595ac91..fa90f54efe 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -26,38 +26,37 @@ #include #include #include -#include namespace gtsam { namespace { - const SymbolicFactorGraph simpleTestGraph1 = boost::assign::list_of - (boost::make_shared(0,1)) - (boost::make_shared(0,2)) - (boost::make_shared(1,4)) - (boost::make_shared(2,4)) - (boost::make_shared(3,4)); - - const SymbolicBayesNet simpleTestGraph1BayesNet = boost::assign::list_of - (boost::make_shared(0,1,2)) - (boost::make_shared(1,2,4)) - (boost::make_shared(2,4)) - (boost::make_shared(3,4)) - (boost::make_shared(4)); - - const SymbolicFactorGraph simpleTestGraph2 = boost::assign::list_of - (boost::make_shared(0,1)) - (boost::make_shared(0,2)) - (boost::make_shared(1,3)) - (boost::make_shared(1,4)) - (boost::make_shared(2,3)) - (boost::make_shared(4,5)); + const SymbolicFactorGraph simpleTestGraph1 { + boost::make_shared(0,1), + boost::make_shared(0,2), + boost::make_shared(1,4), + boost::make_shared(2,4), + boost::make_shared(3,4)}; + + const SymbolicBayesNet simpleTestGraph1BayesNet { + boost::make_shared(0,1,2), + boost::make_shared(1,2,4), + boost::make_shared(2,4), + boost::make_shared(3,4), + boost::make_shared(4)}; + + const SymbolicFactorGraph simpleTestGraph2 { + boost::make_shared(0,1), + boost::make_shared(0,2), + boost::make_shared(1,3), + boost::make_shared(1,4), + boost::make_shared(2,3), + boost::make_shared(4,5)}; /** 1 - 0 - 2 - 3 */ - const SymbolicFactorGraph simpleChain = boost::assign::list_of - (boost::make_shared(1,0)) - (boost::make_shared(0,2)) - (boost::make_shared(2,3)); + const SymbolicFactorGraph simpleChain { + boost::make_shared(1,0), + boost::make_shared(0,2), + boost::make_shared(2,3)}; /* ************************************************************************* * * 2 3 @@ -67,10 +66,10 @@ namespace gtsam { SymbolicBayesTree result; result.insertRoot(boost::make_shared( boost::make_shared( - SymbolicConditional::FromKeys(boost::assign::list_of(2)(3), 2)))); + SymbolicConditional::FromKeys(KeyVector{2,3}, 2)))); result.addClique(boost::make_shared( boost::make_shared( - SymbolicConditional::FromKeys(boost::assign::list_of(0)(1)(2), 2))), + SymbolicConditional::FromKeys(KeyVector{0,1,2}, 2))), result.roots().front()); return result; } @@ -84,39 +83,39 @@ namespace gtsam { _L_=gtsam::symbol_shorthand::L(0), _B_=gtsam::symbol_shorthand::B(0); // Factor graph for Asia example - const SymbolicFactorGraph asiaGraph = boost::assign::list_of - (boost::make_shared(_T_)) - (boost::make_shared(_S_)) - (boost::make_shared(_T_, _E_, _L_)) - (boost::make_shared(_L_, _S_)) - (boost::make_shared(_S_, _B_)) - (boost::make_shared(_E_, _B_)) - (boost::make_shared(_E_, _X_)); - - const SymbolicBayesNet asiaBayesNet = boost::assign::list_of - (boost::make_shared(_T_, _E_, _L_)) - (boost::make_shared(_X_, _E_)) - (boost::make_shared(_E_, _B_, _L_)) - (boost::make_shared(_S_, _B_, _L_)) - (boost::make_shared(_L_, _B_)) - (boost::make_shared(_B_)); + const SymbolicFactorGraph asiaGraph = { + boost::make_shared(_T_), + boost::make_shared(_S_), + boost::make_shared(_T_, _E_, _L_), + boost::make_shared(_L_, _S_), + boost::make_shared(_S_, _B_), + boost::make_shared(_E_, _B_), + boost::make_shared(_E_, _X_)}; + + const SymbolicBayesNet asiaBayesNet = { + boost::make_shared(_T_, _E_, _L_), + boost::make_shared(_X_, _E_), + boost::make_shared(_E_, _B_, _L_), + boost::make_shared(_S_, _B_, _L_), + boost::make_shared(_L_, _B_), + boost::make_shared(_B_)}; SymbolicBayesTree __asiaBayesTree() { SymbolicBayesTree result; result.insertRoot(boost::make_shared( boost::make_shared( - SymbolicConditional::FromKeys(boost::assign::list_of(_E_)(_L_)(_B_), 3)))); + SymbolicConditional::FromKeys(KeyVector{_E_, _L_, _B_}, 3)))); result.addClique(boost::make_shared( boost::make_shared( - SymbolicConditional::FromKeys(boost::assign::list_of(_S_)(_B_) (_L_), 1))), + SymbolicConditional::FromKeys(KeyVector{_S_, _B_, _L_}, 1))), result.roots().front()); result.addClique(boost::make_shared( boost::make_shared( - SymbolicConditional::FromKeys(boost::assign::list_of(_T_)(_E_)(_L_), 1))), + SymbolicConditional::FromKeys(KeyVector{_T_, _E_, _L_}, 1))), result.roots().front()); result.addClique(boost::make_shared( boost::make_shared( - SymbolicConditional::FromKeys(boost::assign::list_of(_X_)(_E_), 1))), + SymbolicConditional::FromKeys(KeyVector{_X_, _E_}, 1))), result.roots().front()); return result; } @@ -124,7 +123,6 @@ namespace gtsam { const SymbolicBayesTree asiaBayesTree = __asiaBayesTree(); /* ************************************************************************* */ - const Ordering asiaOrdering = boost::assign::list_of(_X_)(_T_)(_S_)(_E_)(_L_)(_B_); - + const Ordering asiaOrdering{_X_, _T_, _S_, _E_, _L_, _B_}; } } diff --git a/gtsam/symbolic/tests/testSymbolicConditional.cpp b/gtsam/symbolic/tests/testSymbolicConditional.cpp index 963d0dfef8..d8c13d0922 100644 --- a/gtsam/symbolic/tests/testSymbolicConditional.cpp +++ b/gtsam/symbolic/tests/testSymbolicConditional.cpp @@ -15,8 +15,6 @@ * @author Frank Dellaert */ -#include -using namespace boost::assign; #include #include @@ -69,8 +67,7 @@ TEST( SymbolicConditional, threeParents ) /* ************************************************************************* */ TEST( SymbolicConditional, fourParents ) { - SymbolicConditional c0 = SymbolicConditional::FromKeys( - list_of(0)(1)(2)(3)(4), 1); + auto c0 = SymbolicConditional::FromKeys(KeyVector{0, 1, 2, 3, 4}, 1); LONGS_EQUAL(1, (long)c0.nrFrontals()); LONGS_EQUAL(4, (long)c0.nrParents()); } @@ -78,9 +75,8 @@ TEST( SymbolicConditional, fourParents ) /* ************************************************************************* */ TEST( SymbolicConditional, FromRange ) { - SymbolicConditional::shared_ptr c0 = - boost::make_shared( - SymbolicConditional::FromKeys(list_of(1)(2)(3)(4)(5), 2)); + auto c0 = boost::make_shared( + SymbolicConditional::FromKeys(KeyVector{1, 2, 3, 4, 5}, 2)); LONGS_EQUAL(2, (long)c0->nrFrontals()); LONGS_EQUAL(3, (long)c0->nrParents()); } diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp index 378e780cde..105270c9c1 100644 --- a/gtsam/symbolic/tests/testSymbolicFactor.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp @@ -21,8 +21,6 @@ #include #include -#include -#include #include #include @@ -70,19 +68,19 @@ TEST(SymbolicFactor, Constructors) /* ************************************************************************* */ TEST(SymbolicFactor, EliminateSymbolic) { - const SymbolicFactorGraph factors = list_of - (SymbolicFactor(2,4,6)) - (SymbolicFactor(1,2,5)) - (SymbolicFactor(0,3)); + const SymbolicFactorGraph factors = { + boost::make_shared(2, 4, 6), + boost::make_shared(1, 2, 5), + boost::make_shared(0, 3)}; const SymbolicFactor expectedFactor(4,5,6); const SymbolicConditional expectedConditional = - SymbolicConditional::FromKeys(list_of(0)(1)(2)(3)(4)(5)(6), 4); + SymbolicConditional::FromKeys(KeyVector{0,1,2,3,4,5,6}, 4); SymbolicFactor::shared_ptr actualFactor; SymbolicConditional::shared_ptr actualConditional; boost::tie(actualConditional, actualFactor) = - EliminateSymbolic(factors, list_of(0)(1)(2)(3)); + EliminateSymbolic(factors, Ordering{0, 1, 2, 3}); CHECK(assert_equal(expectedConditional, *actualConditional)); CHECK(assert_equal(expectedFactor, *actualFactor)); diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 8f4eb3c08d..9662642906 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -25,7 +25,8 @@ #include -#include +#include +using namespace boost::assign; using namespace std; using namespace gtsam; @@ -33,16 +34,14 @@ using namespace boost::assign; /* ************************************************************************* */ TEST(SymbolicFactorGraph, keys1) { - KeySet expected; - expected += 0, 1, 2, 3, 4; + KeySet expected {0, 1, 2, 3, 4}; KeySet actual = simpleTestGraph1.keys(); EXPECT(expected == actual); } /* ************************************************************************* */ TEST(SymbolicFactorGraph, keys2) { - KeySet expected; - expected += 0, 1, 2, 3, 4, 5; + KeySet expected {0, 1, 2, 3, 4, 5}; KeySet actual = simpleTestGraph2.keys(); EXPECT(expected == actual); } @@ -50,8 +49,7 @@ TEST(SymbolicFactorGraph, keys2) { /* ************************************************************************* */ TEST(SymbolicFactorGraph, eliminateFullSequential) { // Test with simpleTestGraph1 - Ordering order; - order += 0, 1, 2, 3, 4; + Ordering order{0, 1, 2, 3, 4}; SymbolicBayesNet actual1 = *simpleTestGraph1.eliminateSequential(order); EXPECT(assert_equal(simpleTestGraph1BayesNet, actual1)); @@ -63,7 +61,7 @@ TEST(SymbolicFactorGraph, eliminateFullSequential) { /* ************************************************************************* */ TEST(SymbolicFactorGraph, eliminatePartialSequential) { // Eliminate 0 and 1 - const Ordering order = list_of(0)(1); + const Ordering order {0, 1}; const SymbolicBayesNet expectedBayesNet = list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3, 4)); @@ -74,7 +72,7 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential) { SymbolicBayesNet::shared_ptr actualBayesNet; SymbolicFactorGraph::shared_ptr actualSfg; boost::tie(actualBayesNet, actualSfg) = - simpleTestGraph2.eliminatePartialSequential(Ordering(list_of(0)(1))); + simpleTestGraph2.eliminatePartialSequential(Ordering{0, 1}); EXPECT(assert_equal(expectedSfg, *actualSfg)); EXPECT(assert_equal(expectedBayesNet, *actualBayesNet)); @@ -82,8 +80,7 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential) { SymbolicBayesNet::shared_ptr actualBayesNet2; SymbolicFactorGraph::shared_ptr actualSfg2; boost::tie(actualBayesNet2, actualSfg2) = - simpleTestGraph2.eliminatePartialSequential( - list_of(0)(1).convert_to_container()); + simpleTestGraph2.eliminatePartialSequential(Ordering{0, 1}); EXPECT(assert_equal(expectedSfg, *actualSfg2)); EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2)); @@ -105,7 +102,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { SymbolicBayesTree expectedBayesTree; SymbolicConditional::shared_ptr root = boost::make_shared( - SymbolicConditional::FromKeys(list_of(4)(5)(1), 2)); + SymbolicConditional::FromKeys(KeyVector{4, 5, 1}, 2)); expectedBayesTree.insertRoot( boost::make_shared(root)); @@ -116,7 +113,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { SymbolicBayesTree::shared_ptr actualBayesTree; SymbolicFactorGraph::shared_ptr actualFactorGraph; boost::tie(actualBayesTree, actualFactorGraph) = - simpleTestGraph2.eliminatePartialMultifrontal(Ordering(list_of(4)(5))); + simpleTestGraph2.eliminatePartialMultifrontal(Ordering{4, 5}); EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph)); EXPECT(assert_equal(expectedBayesTree, *actualBayesTree)); @@ -132,8 +129,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { SymbolicBayesTree::shared_ptr actualBayesTree2; SymbolicFactorGraph::shared_ptr actualFactorGraph2; boost::tie(actualBayesTree2, actualFactorGraph2) = - simpleTestGraph2.eliminatePartialMultifrontal( - list_of(4)(5).convert_to_container()); + simpleTestGraph2.eliminatePartialMultifrontal(KeyVector{4, 5}); EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2)); EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2)); @@ -146,7 +142,7 @@ TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) { SymbolicConditional(2, 3))(SymbolicConditional(3)); SymbolicBayesNet actual1 = *simpleTestGraph2.marginalMultifrontalBayesNet( - Ordering(list_of(0)(1)(2)(3))); + Ordering{0, 1, 2, 3}); EXPECT(assert_equal(expectedBayesNet, actual1)); } @@ -184,7 +180,7 @@ TEST(SymbolicFactorGraph, marginals) { fg.push_factor(3, 4); // eliminate - Ordering ord(list_of(3)(4)(2)(1)(0)); + Ordering ord{3, 4, 2, 1, 0}; auto actual = fg.eliminateSequential(ord); SymbolicBayesNet expected; expected.emplace_shared(3, 4); @@ -196,7 +192,7 @@ TEST(SymbolicFactorGraph, marginals) { { // jointBayesNet - Ordering ord(list_of(0)(4)(3)); + Ordering ord {0, 4, 3}; auto actual = fg.eliminatePartialSequential(ord); SymbolicBayesNet expectedBN; expectedBN.emplace_shared(0, 1, 2); @@ -207,7 +203,7 @@ TEST(SymbolicFactorGraph, marginals) { { // jointBayesNet - Ordering ord(list_of(0)(2)(3)); + Ordering ord {0, 2, 3}; auto actual = fg.eliminatePartialSequential(ord); SymbolicBayesNet expectedBN; expectedBN.emplace_shared(0, 1, 2); @@ -218,7 +214,7 @@ TEST(SymbolicFactorGraph, marginals) { { // conditionalBayesNet - Ordering ord(list_of(0)(2)); + Ordering ord{0, 2}; auto actual = fg.eliminatePartialSequential(ord); SymbolicBayesNet expectedBN; expectedBN.emplace_shared(0, 1, 2); @@ -306,7 +302,7 @@ TEST(SymbolicFactorGraph, add_factors) { expected.push_factor(1); expected.push_factor(11); expected.push_factor(2); - const FactorIndices expectedIndices = list_of(1)(3); + const FactorIndices expectedIndices {1, 3}; const FactorIndices actualIndices = fg1.add_factors(fg2, true); EXPECT(assert_equal(expected, fg1)); @@ -314,7 +310,7 @@ TEST(SymbolicFactorGraph, add_factors) { expected.push_factor(1); expected.push_factor(2); - const FactorIndices expectedIndices2 = list_of(4)(5); + const FactorIndices expectedIndices2 {4, 5}; const FactorIndices actualIndices2 = fg1.add_factors(fg2, false); EXPECT(assert_equal(expected, fg1)); diff --git a/gtsam/symbolic/tests/testSymbolicISAM.cpp b/gtsam/symbolic/tests/testSymbolicISAM.cpp index e3ab36c943..e84af28a3b 100644 --- a/gtsam/symbolic/tests/testSymbolicISAM.cpp +++ b/gtsam/symbolic/tests/testSymbolicISAM.cpp @@ -20,9 +20,6 @@ #include -#include // for operator += -using namespace boost::assign; - using namespace std; using namespace gtsam; @@ -86,7 +83,7 @@ TEST( SymbolicISAM, iSAM ) fullGraph += SymbolicFactor(_B_, _S_); // This ordering is chosen to match the one chosen by COLAMD during the ISAM update - Ordering ordering(list_of(_X_)(_B_)(_S_)(_E_)(_L_)(_T_)); + Ordering ordering {_X_, _B_, _S_, _E_, _L_, _T_}; SymbolicBayesTree expected = *fullGraph.eliminateMultifrontal(ordering); // Add factor on B and S diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index c5b1f4ff1f..796bdc49e3 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -23,9 +23,6 @@ #include #include -#include -using namespace boost::assign; - #include "symbolicExampleGraphs.h" using namespace gtsam; @@ -43,9 +40,9 @@ TEST( JunctionTree, constructor ) SymbolicJunctionTree actual(SymbolicEliminationTree(simpleChain, order)); SymbolicJunctionTree::Node::Keys - frontal1 = list_of(2)(3), - frontal2 = list_of(0)(1), - sep1, sep2 = list_of(2); + frontal1 {2, 3}, + frontal2 {0, 1}, + sep1, sep2 {2}; EXPECT(assert_container_equality(frontal1, actual.roots().front()->orderedFrontalKeys)); //EXPECT(assert_equal(sep1, actual.roots().front()->separator)); LONGS_EQUAL(1, (long)actual.roots().front()->factors.size()); diff --git a/gtsam/symbolic/tests/testVariableIndex.cpp b/gtsam/symbolic/tests/testVariableIndex.cpp index 6afb47e26a..8fdb7bee18 100644 --- a/gtsam/symbolic/tests/testVariableIndex.cpp +++ b/gtsam/symbolic/tests/testVariableIndex.cpp @@ -22,10 +22,6 @@ #include -#include -#include -using namespace boost::assign; - using namespace std; using namespace gtsam; @@ -79,7 +75,7 @@ TEST(VariableIndex, augment2) { VariableIndex expected(fgCombined); - FactorIndices newIndices = list_of(5)(6)(7)(8); + FactorIndices newIndices {5, 6, 7, 8}; VariableIndex actual(fg1); actual.augment(fg2, newIndices); @@ -108,7 +104,7 @@ TEST(VariableIndex, remove) { vector indices; indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3); actual.remove(indices.begin(), indices.end(), fg1); - std::list unusedVariables; unusedVariables += 0, 9; + std::list unusedVariables{0, 9}; actual.removeUnusedVariables(unusedVariables.begin(), unusedVariables.end()); CHECK(assert_equal(expected, actual)); @@ -135,7 +131,7 @@ TEST(VariableIndex, deep_copy) { vector indices; indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3); clone.remove(indices.begin(), indices.end(), fg1); - std::list unusedVariables; unusedVariables += 0, 9; + std::list unusedVariables{0, 9}; clone.removeUnusedVariables(unusedVariables.begin(), unusedVariables.end()); // When modifying the clone, the original should have stayed the same diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 168891e6fd..d0695002d0 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -40,11 +40,10 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { protected: /// Construct unary constraint factor. - Constraint(Key j) : DiscreteFactor(boost::assign::cref_list_of<1>(j)) {} + Constraint(Key j) : DiscreteFactor(KeyVector{j}) {} /// Construct binary constraint factor. - Constraint(Key j1, Key j2) - : DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) {} + Constraint(Key j1, Key j2) : DiscreteFactor(KeyVector{j1, j2}) {} /// Construct n-way constraint factor. Constraint(const KeyVector& js) : DiscreteFactor(js) {} diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 848abc9cc5..ea1ce0d431 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -73,7 +73,7 @@ class BetweenFactorEM: public NonlinearFactor { const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, const double prior_inlier, const double prior_outlier, const bool flag_bump_up_near_zero_probs = false) : - Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_( + Base(KeyVector{key1, key2}), key1_(key1), key2_(key2), measured_( measured), model_inlier_(model_inlier), model_outlier_(model_outlier), prior_inlier_( prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_( flag_bump_up_near_zero_probs) { diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index df6b1e50dc..d2da5977b8 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -15,7 +15,7 @@ namespace gtsam { /* ************************************************************************* */ DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2) -: NonlinearFactor(cref_list_of<2>(key1)(key2)) +: NonlinearFactor(KeyVector{key1, key2}) { dims_.push_back(dim1); dims_.push_back(dim2); diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index afe731bd5f..0318c3eb12 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -70,7 +70,7 @@ namespace gtsam { TransformBtwRobotsUnaryFactor(Key key, const VALUE& measured, Key keyA, Key keyB, const gtsam::Values& valA, const gtsam::Values& valB, const SharedGaussian& model) : - Base(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB), + Base(KeyVector{key}), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB), model_(model){ setValAValB(valA, valB); diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index 2748d337e6..58553b81fb 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -84,7 +84,7 @@ namespace gtsam { const double prior_inlier, const double prior_outlier, const bool flag_bump_up_near_zero_probs = false, const bool start_with_M_step = false) : - Base(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB), + Base(KeyVector{key}), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB), model_inlier_(model_inlier), model_outlier_(model_outlier), prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs), start_with_M_step_(false){ From 4e078e41f18cad5256c40c7bee083e18d184b363 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 23:17:54 -0800 Subject: [PATCH 12/37] Heavy lift on removing boost::assign from symbolic tests. --- gtsam/symbolic/SymbolicBayesNet.h | 16 + gtsam/symbolic/SymbolicFactorGraph.h | 16 + .../symbolic/tests/testSymbolicBayesTree.cpp | 528 +++++++++--------- .../tests/testSymbolicEliminationTree.cpp | 112 ++-- gtsam/symbolic/tests/testSymbolicFactor.cpp | 1 - .../tests/testSymbolicFactorGraph.cpp | 44 +- 6 files changed, 377 insertions(+), 340 deletions(-) diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 33c77c6dee..43f1edd692 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -70,6 +70,22 @@ namespace gtsam { for (auto&& f : sharedFactors) factors_.push_back(f); } + /// Construct from a single conditional + SymbolicBayesNet(SymbolicConditional&& c) { + push_back(boost::make_shared(c)); + } + + /** + * @brief Add a single conditional and return a reference. + * This allows for chaining, e.g., + * SymbolicBayesNet bn = + * SymbolicBayesNet(SymbolicConditional(...))(SymbolicConditional(...)); + */ + SymbolicBayesNet& operator()(SymbolicConditional&& c) { + push_back(boost::make_shared(c)); + return *this; + } + /// Destructor virtual ~SymbolicBayesNet() {} diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 95892eec87..6b4c456fa6 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -90,6 +90,22 @@ namespace gtsam { for (auto&& f : sharedFactors) factors_.push_back(f); } + /// Construct from a single factor + SymbolicFactorGraph(SymbolicFactor&& c) { + push_back(boost::make_shared(c)); + } + + /** + * @brief Add a single factor and return a reference. + * This allows for chaining, e.g., + * SymbolicFactorGraph bn = + * SymbolicFactorGraph(SymbolicFactor(...))(SymbolicFactor(...)); + */ + SymbolicFactorGraph& operator()(SymbolicFactor&& c) { + push_back(boost::make_shared(c)); + return *this; + } + /// Destructor virtual ~SymbolicFactorGraph() {} diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index ee9b41a5aa..c5ea5a792e 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -17,16 +17,12 @@ * @author Viorela Ila */ -#include -#include #include +#include +#include #include -#include -#include -#include #include -using namespace boost::assign; using boost::adaptors::indirected; #include @@ -38,37 +34,50 @@ using namespace gtsam::symbol_shorthand; static bool debug = false; -namespace { +using sharedClique = SymbolicBayesTreeClique::shared_ptr; - /* ************************************************************************* */ - // Helper functions for below - template - SymbolicBayesTreeClique::shared_ptr MakeClique(const KEYS& keys, DenseIndex nrFrontals) - { - return boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(keys, nrFrontals))); +template +class ListOf { + public: + ListOf(const T& c) { result.push_back(c); } + + ListOf& operator()(const T& c) { + result.push_back(c); + return *this; } - template - SymbolicBayesTreeClique::shared_ptr MakeClique( - const KEYS& keys, DenseIndex nrFrontals, const CHILDREN& children) - { - SymbolicBayesTreeClique::shared_ptr clique = - boost::make_shared( + operator std::vector() { return result; } + + private: + std::vector result; +}; + +using MakeKeys = ListOf; +using MakeCliques = ListOf; + +namespace { +/* ************************************************************************* */ +// Helper functions for below +sharedClique MakeClique(const KeyVector& keys, DenseIndex nrFrontals) { + return boost::make_shared( boost::make_shared( - SymbolicConditional::FromKeys(keys, nrFrontals))); - clique->children.assign(children.begin(), children.end()); - for(typename CHILDREN::const_iterator child = children.begin(); child != children.end(); ++child) - (*child)->parent_ = clique; - return clique; - } + SymbolicConditional::FromKeys(keys, nrFrontals))); +} +sharedClique MakeClique(const KeyVector& keys, DenseIndex nrFrontals, + const std::vector& children) { + sharedClique clique = boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(keys, nrFrontals))); + clique->children.assign(children.begin(), children.end()); + for (auto&& child : children) child->parent_ = clique; + return clique; } +} // namespace + /* ************************************************************************* */ -TEST(SymbolicBayesTree, clear) -{ +TEST(SymbolicBayesTree, clear) { SymbolicBayesTree bayesTree = asiaBayesTree; bayesTree.clear(); @@ -79,34 +88,35 @@ TEST(SymbolicBayesTree, clear) } /* ************************************************************************* */ -TEST(SymbolicBayesTree, clique_structure) -{ +TEST(SymbolicBayesTree, clique_structure) { // l1 l2 // / | / | // x1 --- x2 --- x3 --- x4 --- x5 // \ | // l3 SymbolicFactorGraph graph; - graph += SymbolicFactor(X(1), L(1)); - graph += SymbolicFactor(X(1), X(2)); - graph += SymbolicFactor(X(2), L(1)); - graph += SymbolicFactor(X(2), X(3)); - graph += SymbolicFactor(X(3), X(4)); - graph += SymbolicFactor(X(4), L(2)); - graph += SymbolicFactor(X(4), X(5)); - graph += SymbolicFactor(L(2), X(5)); - graph += SymbolicFactor(X(4), L(3)); - graph += SymbolicFactor(X(5), L(3)); + graph.emplace_shared(X(1), L(1)); + graph.emplace_shared(X(1), X(2)); + graph.emplace_shared(X(2), L(1)); + graph.emplace_shared(X(2), X(3)); + graph.emplace_shared(X(3), X(4)); + graph.emplace_shared(X(4), L(2)); + graph.emplace_shared(X(4), X(5)); + graph.emplace_shared(L(2), X(5)); + graph.emplace_shared(X(4), L(3)); + graph.emplace_shared(X(5), L(3)); SymbolicBayesTree expected; - expected.insertRoot( - MakeClique(list_of(X(2)) (X(3)), 2, list_of - (MakeClique(list_of(X(4)) (X(3)), 1, list_of - (MakeClique(list_of(X(5)) (L(2)) (X(4)), 2, list_of - (MakeClique(list_of(L(3)) (X(4)) (X(5)), 1)))))) - (MakeClique(list_of(X(1)) (L(1)) (X(2)), 2)))); + expected.insertRoot(MakeClique( + MakeKeys(X(2))(X(3)), 2, + MakeCliques(MakeClique( + MakeKeys(X(4))(X(3)), 1, + MakeCliques(MakeClique( + MakeKeys(X(5))(L(2))(X(4)), 2, + MakeCliques(MakeClique(MakeKeys(L(3))(X(4))(X(5)), 1))))))( + MakeClique(MakeKeys(X(1))(L(1))(X(2)), 2)))); - Ordering order = list_of(X(1)) (L(3)) (L(1)) (X(5)) (X(2)) (L(2)) (X(4)) (X(3)); + Ordering order{X(1), L(3), L(1), X(5), X(2), L(2), X(4), X(3)}; SymbolicBayesTree actual = *graph.eliminateMultifrontal(order); @@ -120,56 +130,56 @@ Bayes Tree for testing conversion to a forest of orphans needed for incremental. D|C F|E */ /* ************************************************************************* */ -TEST( BayesTree, removePath ) -{ - const Key _A_=A(0), _B_=B(0), _C_=C(0), _D_=D(0), _E_=E(0), _F_=F(0); +TEST(BayesTree, removePath) { + const Key _A_ = A(0), _B_ = B(0), _C_ = C(0), _D_ = D(0), _E_ = E(0), + _F_ = F(0); SymbolicBayesTree bayesTreeOrig; - bayesTreeOrig.insertRoot( - MakeClique(list_of(_A_)(_B_), 2, list_of - (MakeClique(list_of(_C_)(_A_), 1, list_of - (MakeClique(list_of(_D_)(_C_), 1)))) - (MakeClique(list_of(_E_)(_B_), 1, list_of - (MakeClique(list_of(_F_)(_E_), 1)))))); + bayesTreeOrig.insertRoot(MakeClique( + MakeKeys(_A_)(_B_), 2, + MakeCliques(MakeClique(MakeKeys(_C_)(_A_), 1, + MakeCliques(MakeClique(MakeKeys(_D_)(_C_), 1))))( + MakeClique(MakeKeys(_E_)(_B_), 1, + MakeCliques(MakeClique(MakeKeys(_F_)(_E_), 1)))))); SymbolicBayesTree bayesTree = bayesTreeOrig; // remove C, expected outcome: factor graph with ABC, // Bayes Tree now contains two orphan trees: D|C and E|B,F|E SymbolicFactorGraph expected; - expected += SymbolicFactor(_A_,_B_); - expected += SymbolicFactor(_C_,_A_); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_D_], bayesTree[_E_]; + expected.emplace_shared(_A_, _B_); + expected.emplace_shared(_C_, _A_); + SymbolicBayesTree::Cliques expectedOrphans = + std::list{bayesTree[_D_], bayesTree[_E_]}; SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; bayesTree.removePath(bayesTree[_C_], &bn, &orphans); SymbolicFactorGraph factors(bn); CHECK(assert_equal(expected, factors)); - CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + CHECK(assert_container_equal(expectedOrphans | indirected, + orphans | indirected)); bayesTree = bayesTreeOrig; // remove E: factor graph with EB; E|B removed from second orphan tree SymbolicFactorGraph expected2; - expected2 += SymbolicFactor(_A_,_B_); - expected2 += SymbolicFactor(_E_,_B_); - SymbolicBayesTree::Cliques expectedOrphans2; - expectedOrphans2 += bayesTree[_F_]; - expectedOrphans2 += bayesTree[_C_]; + expected2.emplace_shared(_A_, _B_); + expected2.emplace_shared(_E_, _B_); + SymbolicBayesTree::Cliques expectedOrphans2 = + std::list{bayesTree[_F_], bayesTree[_C_]}; SymbolicBayesNet bn2; SymbolicBayesTree::Cliques orphans2; bayesTree.removePath(bayesTree[_E_], &bn2, &orphans2); SymbolicFactorGraph factors2(bn2); CHECK(assert_equal(expected2, factors2)); - CHECK(assert_container_equal(expectedOrphans2|indirected, orphans2|indirected)); + CHECK(assert_container_equal(expectedOrphans2 | indirected, + orphans2 | indirected)); } /* ************************************************************************* */ -TEST( BayesTree, removePath2 ) -{ +TEST(BayesTree, removePath2) { SymbolicBayesTree bayesTree = asiaBayesTree; // Call remove-path with clique B @@ -180,16 +190,16 @@ TEST( BayesTree, removePath2 ) // Check expected outcome SymbolicFactorGraph expected; - expected += SymbolicFactor(_E_,_L_,_B_); + expected.emplace_shared(_E_, _L_, _B_); CHECK(assert_equal(expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + SymbolicBayesTree::Cliques expectedOrphans = + std::list{bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]}; + CHECK(assert_container_equal(expectedOrphans | indirected, + orphans | indirected)); } /* ************************************************************************* */ -TEST(BayesTree, removePath3) -{ +TEST(BayesTree, removePath3) { SymbolicBayesTree bayesTree = asiaBayesTree; // Call remove-path with clique T @@ -200,199 +210,185 @@ TEST(BayesTree, removePath3) // Check expected outcome SymbolicFactorGraph expected; - expected += SymbolicFactor(_E_, _L_, _B_); - expected += SymbolicFactor(_T_, _E_, _L_); + expected.emplace_shared(_E_, _L_, _B_); + expected.emplace_shared(_T_, _E_, _L_); CHECK(assert_equal(expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_S_], bayesTree[_X_]; - CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + SymbolicBayesTree::Cliques expectedOrphans = + std::list{bayesTree[_S_], bayesTree[_X_]}; + CHECK(assert_container_equal(expectedOrphans | indirected, + orphans | indirected)); } -void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayesTree::Cliques& cliques) { +void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, + SymbolicBayesTree::Cliques& cliques) { // Check if subtree exists if (subtree) { cliques.push_back(subtree); // Recursive call over all child cliques - for(SymbolicBayesTree::sharedClique& childClique: subtree->children) { - getAllCliques(childClique,cliques); + for (SymbolicBayesTree::sharedClique& childClique : subtree->children) { + getAllCliques(childClique, cliques); } } } /* ************************************************************************* */ -TEST( BayesTree, shortcutCheck ) -{ - const Key _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0; - SymbolicFactorGraph chain = list_of - (SymbolicFactor(_A_)) - (SymbolicFactor(_B_, _A_)) - (SymbolicFactor(_C_, _A_)) - (SymbolicFactor(_D_, _C_)) - (SymbolicFactor(_E_, _B_)) - (SymbolicFactor(_F_, _E_)) - (SymbolicFactor(_G_, _F_)); - Ordering ordering(list_of(_G_)(_F_)(_E_)(_D_)(_C_)(_B_)(_A_)); +TEST(BayesTree, shortcutCheck) { + const Key _A_ = 6, _B_ = 5, _C_ = 4, _D_ = 3, _E_ = 2, _F_ = 1, _G_ = 0; + auto chain = + SymbolicFactorGraph(SymbolicFactor(_A_))(SymbolicFactor(_B_, _A_))( + SymbolicFactor(_C_, _A_))(SymbolicFactor(_D_, _C_))(SymbolicFactor( + _E_, _B_))(SymbolicFactor(_F_, _E_))(SymbolicFactor(_G_, _F_)); + Ordering ordering{_G_, _F_, _E_, _D_, _C_, _B_, _A_}; SymbolicBayesTree bayesTree = *chain.eliminateMultifrontal(ordering); - //bayesTree.saveGraph("BT1.dot"); + // bayesTree.saveGraph("BT1.dot"); SymbolicBayesTree::sharedClique rootClique = bayesTree.roots().front(); - //rootClique->printTree(); + // rootClique->printTree(); SymbolicBayesTree::Cliques allCliques; - getAllCliques(rootClique,allCliques); + getAllCliques(rootClique, allCliques); - for(SymbolicBayesTree::sharedClique& clique: allCliques) { - //clique->print("Clique#"); + for (SymbolicBayesTree::sharedClique& clique : allCliques) { + // clique->print("Clique#"); SymbolicBayesNet bn = clique->shortcut(rootClique); - //bn.print("Shortcut:\n"); - //cout << endl; + // bn.print("Shortcut:\n"); + // cout << endl; } // Check if all the cached shortcuts are cleared rootClique->deleteCachedShortcuts(); - for(SymbolicBayesTree::sharedClique& clique: allCliques) { + for (SymbolicBayesTree::sharedClique& clique : allCliques) { bool notCleared = clique->cachedSeparatorMarginal().is_initialized(); - CHECK( notCleared == false); + CHECK(notCleared == false); } EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals()); -// for(SymbolicBayesTree::sharedClique& clique: allCliques) { -// clique->print("Clique#"); -// if(clique->cachedShortcut()){ -// bn = clique->cachedShortcut().get(); -// bn.print("Shortcut:\n"); -// } -// else -// cout << "Not Initialized" << endl; -// cout << endl; -// } + // for(SymbolicBayesTree::sharedClique& clique: allCliques) { + // clique->print("Clique#"); + // if(clique->cachedShortcut()){ + // bn = clique->cachedShortcut().get(); + // bn.print("Shortcut:\n"); + // } + // else + // cout << "Not Initialized" << endl; + // cout << endl; + // } } /* ************************************************************************* */ -TEST( BayesTree, removeTop ) -{ +TEST(BayesTree, removeTop) { SymbolicBayesTree bayesTree = asiaBayesTree; // create a new factor to be inserted - //boost::shared_ptr newFactor(new IndexFactor(_S_,_B_)); + // boost::shared_ptr newFactor(new IndexFactor(_S_,_B_)); // Remove the contaminated part of the Bayes tree SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(_B_)(_S_), &bn, &orphans); + bayesTree.removeTop(MakeKeys(_B_)(_S_), &bn, &orphans); // Check expected outcome SymbolicBayesNet expected; - expected += SymbolicConditional::FromKeys(list_of(_E_)(_L_)(_B_), 3); - expected += SymbolicConditional::FromKeys(list_of(_S_)(_B_)(_L_), 1); + expected += + SymbolicConditional::FromKeys(MakeKeys(_E_)(_L_)(_B_), 3); + expected += + SymbolicConditional::FromKeys(MakeKeys(_S_)(_B_)(_L_), 1); CHECK(assert_equal(expected, bn)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + SymbolicBayesTree::Cliques expectedOrphans = + std::list{bayesTree[_T_], bayesTree[_X_]}; + CHECK(assert_container_equal(expectedOrphans | indirected, + orphans | indirected)); // Try removeTop again with a factor that should not change a thing - //boost::shared_ptr newFactor2(new IndexFactor(_B_)); + // boost::shared_ptr newFactor2(new IndexFactor(_B_)); SymbolicBayesNet bn2; SymbolicBayesTree::Cliques orphans2; - bayesTree.removeTop(list_of(_B_), &bn2, &orphans2); + bayesTree.removeTop(MakeKeys(_B_), &bn2, &orphans2); SymbolicFactorGraph factors2(bn2); SymbolicFactorGraph expected2; CHECK(assert_equal(expected2, factors2)); SymbolicBayesTree::Cliques expectedOrphans2; - CHECK(assert_container_equal(expectedOrphans2|indirected, orphans2|indirected)); + CHECK(assert_container_equal(expectedOrphans2 | indirected, + orphans2 | indirected)); } /* ************************************************************************* */ -TEST( BayesTree, removeTop2 ) -{ +TEST(BayesTree, removeTop2) { SymbolicBayesTree bayesTree = asiaBayesTree; // create two factors to be inserted - //SymbolicFactorGraph newFactors; - //newFactors.push_factor(_B_); - //newFactors.push_factor(_S_); + // SymbolicFactorGraph newFactors; + // newFactors.push_factor(_B_); + // newFactors.push_factor(_S_); // Remove the contaminated part of the Bayes tree SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(_T_), &bn, &orphans); + bayesTree.removeTop(MakeKeys(_T_), &bn, &orphans); // Check expected outcome - SymbolicBayesNet expected = list_of - (SymbolicConditional::FromKeys(list_of(_E_)(_L_)(_B_), 3)) - (SymbolicConditional::FromKeys(list_of(_T_)(_E_)(_L_), 1)); + auto expected = SymbolicBayesNet( + SymbolicConditional::FromKeys(MakeKeys(_E_)(_L_)(_B_), 3))( + SymbolicConditional::FromKeys(MakeKeys(_T_)(_E_)(_L_), 1)); CHECK(assert_equal(expected, bn)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_S_], bayesTree[_X_]; - CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + SymbolicBayesTree::Cliques expectedOrphans = + std::list{bayesTree[_S_], bayesTree[_X_]}; + CHECK(assert_container_equal(expectedOrphans | indirected, + orphans | indirected)); } /* ************************************************************************* */ -TEST( BayesTree, removeTop3 ) -{ - SymbolicFactorGraph graph = list_of - (SymbolicFactor(L(5))) - (SymbolicFactor(X(4), L(5))) - (SymbolicFactor(X(2), X(4))) - (SymbolicFactor(X(3), X(2))); - Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); +TEST(BayesTree, removeTop3) { + auto graph = SymbolicFactorGraph(SymbolicFactor(L(5)))(SymbolicFactor( + X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2))); + Ordering ordering{X(3), X(2), X(4), L(5)}; SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // remove all SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(L(5))(X(4))(X(2))(X(3)), &bn, &orphans); + bayesTree.removeTop(MakeKeys(L(5))(X(4))(X(2))(X(3)), &bn, &orphans); - SymbolicBayesNet expectedBn = list_of - (SymbolicConditional::FromKeys(list_of(X(4))(L(5)), 2)) - (SymbolicConditional(X(2), X(4))) - (SymbolicConditional(X(3), X(2))); + auto expectedBn = SymbolicBayesNet( + SymbolicConditional::FromKeys(MakeKeys(X(4))(L(5)), 2))( + SymbolicConditional(X(2), X(4)))(SymbolicConditional(X(3), X(2))); EXPECT(assert_equal(expectedBn, bn)); EXPECT(orphans.empty()); } /* ************************************************************************* */ -TEST( BayesTree, removeTop4 ) -{ - SymbolicFactorGraph graph = list_of - (SymbolicFactor(L(5))) - (SymbolicFactor(X(4), L(5))) - (SymbolicFactor(X(2), X(4))) - (SymbolicFactor(X(3), X(2))); - Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); +TEST(BayesTree, removeTop4) { + auto graph = SymbolicFactorGraph(SymbolicFactor(L(5)))(SymbolicFactor( + X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2))); + Ordering ordering{X(3), X(2), X(4), L(5)}; SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // remove all SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(X(2))(L(5))(X(4))(X(3)), &bn, &orphans); + bayesTree.removeTop(MakeKeys(X(2))(L(5))(X(4))(X(3)), &bn, &orphans); - SymbolicBayesNet expectedBn = list_of - (SymbolicConditional::FromKeys(list_of(X(4))(L(5)), 2)) - (SymbolicConditional(X(2), X(4))) - (SymbolicConditional(X(3), X(2))); + auto expectedBn = SymbolicBayesNet( + SymbolicConditional::FromKeys(MakeKeys(X(4))(L(5)), 2))( + SymbolicConditional(X(2), X(4)))(SymbolicConditional(X(3), X(2))); EXPECT(assert_equal(expectedBn, bn)); EXPECT(orphans.empty()); } /* ************************************************************************* */ -TEST( BayesTree, removeTop5 ) -{ +TEST(BayesTree, removeTop5) { // Remove top called with variables that are not in the Bayes tree - SymbolicFactorGraph graph = list_of - (SymbolicFactor(L(5))) - (SymbolicFactor(X(4), L(5))) - (SymbolicFactor(X(2), X(4))) - (SymbolicFactor(X(3), X(2))); - Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); + auto graph = SymbolicFactorGraph(SymbolicFactor(L(5)))(SymbolicFactor( + X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2))); + Ordering ordering{X(3), X(2), X(4), L(5)}; SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // Remove nonexistant SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(X(10)), &bn, &orphans); + bayesTree.removeTop(MakeKeys(X(10)), &bn, &orphans); SymbolicBayesNet expectedBn; EXPECT(assert_equal(expectedBn, bn)); @@ -400,29 +396,28 @@ TEST( BayesTree, removeTop5 ) } /* ************************************************************************* */ -TEST( SymbolicBayesTree, thinTree ) { - +TEST(SymbolicBayesTree, thinTree) { // create a thin-tree Bayesnet, a la Jean-Guillaume SymbolicBayesNet bayesNet; - bayesNet.push_back(boost::make_shared(14)); + bayesNet.emplace_shared(14); - bayesNet.push_back(boost::make_shared(13, 14)); - bayesNet.push_back(boost::make_shared(12, 14)); + bayesNet.emplace_shared(13, 14); + bayesNet.emplace_shared(12, 14); - bayesNet.push_back(boost::make_shared(11, 13, 14)); - bayesNet.push_back(boost::make_shared(10, 13, 14)); - bayesNet.push_back(boost::make_shared(9, 12, 14)); - bayesNet.push_back(boost::make_shared(8, 12, 14)); + bayesNet.emplace_shared(11, 13, 14); + bayesNet.emplace_shared(10, 13, 14); + bayesNet.emplace_shared(9, 12, 14); + bayesNet.emplace_shared(8, 12, 14); - bayesNet.push_back(boost::make_shared(7, 11, 13)); - bayesNet.push_back(boost::make_shared(6, 11, 13)); - bayesNet.push_back(boost::make_shared(5, 10, 13)); - bayesNet.push_back(boost::make_shared(4, 10, 13)); + bayesNet.emplace_shared(7, 11, 13); + bayesNet.emplace_shared(6, 11, 13); + bayesNet.emplace_shared(5, 10, 13); + bayesNet.emplace_shared(4, 10, 13); - bayesNet.push_back(boost::make_shared(3, 9, 12)); - bayesNet.push_back(boost::make_shared(2, 9, 12)); - bayesNet.push_back(boost::make_shared(1, 8, 12)); - bayesNet.push_back(boost::make_shared(0, 8, 12)); + bayesNet.emplace_shared(3, 9, 12); + bayesNet.emplace_shared(2, 9, 12); + bayesNet.emplace_shared(1, 8, 12); + bayesNet.emplace_shared(0, 8, 12); if (debug) { GTSAM_PRINT(bayesNet); @@ -430,7 +425,8 @@ TEST( SymbolicBayesTree, thinTree ) { } // create a BayesTree out of a Bayes net - SymbolicBayesTree bayesTree = *SymbolicFactorGraph(bayesNet).eliminateMultifrontal(); + SymbolicBayesTree bayesTree = + *SymbolicFactorGraph(bayesNet).eliminateMultifrontal(); if (debug) { GTSAM_PRINT(bayesTree); bayesTree.saveGraph("/tmp/SymbolicBayesTree.dot"); @@ -442,7 +438,7 @@ TEST( SymbolicBayesTree, thinTree ) { // check shortcut P(S9||R) to root SymbolicBayesTree::Clique::shared_ptr c = bayesTree[9]; SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(14, 11, 13)); + auto expected = SymbolicBayesNet(SymbolicConditional(14, 11, 13)); EXPECT(assert_equal(expected, shortcut)); } @@ -450,9 +446,8 @@ TEST( SymbolicBayesTree, thinTree ) { // check shortcut P(S8||R) to root SymbolicBayesTree::Clique::shared_ptr c = bayesTree[8]; SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of - (SymbolicConditional(12, 14)) - (SymbolicConditional(14, 11, 13)); + auto expected = SymbolicBayesNet(SymbolicConditional(12, 14))( + SymbolicConditional(14, 11, 13)); EXPECT(assert_equal(expected, shortcut)); } @@ -460,7 +455,7 @@ TEST( SymbolicBayesTree, thinTree ) { // check shortcut P(S4||R) to root SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4]; SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(10, 11, 13)); + auto expected = SymbolicBayesNet(SymbolicConditional(10, 11, 13)); EXPECT(assert_equal(expected, shortcut)); } @@ -468,8 +463,8 @@ TEST( SymbolicBayesTree, thinTree ) { // check shortcut P(S2||R) to root SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2]; SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(9, 11, 12, 13)) - (SymbolicConditional(12, 11, 13)); + auto expected = SymbolicBayesNet(SymbolicConditional(9, 11, 12, 13))( + SymbolicConditional(12, 11, 13)); EXPECT(assert_equal(expected, shortcut)); } @@ -477,28 +472,28 @@ TEST( SymbolicBayesTree, thinTree ) { // check shortcut P(S0||R) to root SymbolicBayesTree::Clique::shared_ptr c = bayesTree[0]; SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(8, 11, 12, 13)) - (SymbolicConditional(12, 11, 13)); + auto expected = SymbolicBayesNet(SymbolicConditional(8, 11, 12, 13))( + SymbolicConditional(12, 11, 13)); EXPECT(assert_equal(expected, shortcut)); } SymbolicBayesNet::shared_ptr actualJoint; // Check joint P(8,2) - if (false) { // TODO, not disjoint + if (false) { // TODO, not disjoint actualJoint = bayesTree.jointBayesNet(8, 2); SymbolicBayesNet expected; - expected.push_back(boost::make_shared(8)); - expected.push_back(boost::make_shared(2, 8)); + expected.emplace_shared(8); + expected.emplace_shared(2, 8); EXPECT(assert_equal(expected, *actualJoint)); } // Check joint P(1,2) - if (false) { // TODO, not disjoint + if (false) { // TODO, not disjoint actualJoint = bayesTree.jointBayesNet(1, 2); SymbolicBayesNet expected; - expected.push_back(boost::make_shared(2)); - expected.push_back(boost::make_shared(1, 2)); + expected.emplace_shared(2); + expected.emplace_shared(1, 2); EXPECT(assert_equal(expected, *actualJoint)); } @@ -506,35 +501,33 @@ TEST( SymbolicBayesTree, thinTree ) { if (true) { actualJoint = bayesTree.jointBayesNet(2, 6); SymbolicBayesNet expected; - expected.push_back(boost::make_shared(2, 6)); - expected.push_back(boost::make_shared(6)); + expected.emplace_shared(2, 6); + expected.emplace_shared(6); EXPECT(assert_equal(expected, *actualJoint)); } // Check joint P(4,6) - if (false) { // TODO, not disjoint + if (false) { // TODO, not disjoint actualJoint = bayesTree.jointBayesNet(4, 6); SymbolicBayesNet expected; - expected.push_back(boost::make_shared(6)); - expected.push_back(boost::make_shared(4, 6)); + expected.emplace_shared(6); + expected.emplace_shared(4, 6); EXPECT(assert_equal(expected, *actualJoint)); } } /* ************************************************************************* */ -TEST(SymbolicBayesTree, forest_joint) -{ +TEST(SymbolicBayesTree, forest_joint) { // Create forest - SymbolicBayesTreeClique::shared_ptr root1 = MakeClique(list_of(1), 1); - SymbolicBayesTreeClique::shared_ptr root2 = MakeClique(list_of(2), 1); + sharedClique root1 = MakeClique(MakeKeys(1), 1); + sharedClique root2 = MakeClique(MakeKeys(2), 1); SymbolicBayesTree bayesTree; bayesTree.insertRoot(root1); bayesTree.insertRoot(root2); // Check joint - SymbolicBayesNet expected = list_of - (SymbolicConditional(1)) - (SymbolicConditional(2)); + auto expected = + SymbolicBayesNet(SymbolicConditional(1))(SymbolicConditional(2)); SymbolicBayesNet actual = *bayesTree.jointBayesNet(1, 2); EXPECT(assert_equal(expected, actual)); @@ -550,7 +543,7 @@ TEST(SymbolicBayesTree, forest_joint) C6 0 : 1 **************************************************************************** */ -TEST( SymbolicBayesTree, linear_smoother_shortcuts ) { +TEST(SymbolicBayesTree, linear_smoother_shortcuts) { // Create smoother with 7 nodes SymbolicFactorGraph smoother; smoother.push_factor(0); @@ -581,7 +574,8 @@ TEST( SymbolicBayesTree, linear_smoother_shortcuts ) { { // check shortcut P(S2||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4]; // 4 is frontal in C2 + SymbolicBayesTree::Clique::shared_ptr c = + bayesTree[4]; // 4 is frontal in C2 SymbolicBayesNet shortcut = c->shortcut(R); SymbolicBayesNet expected; EXPECT(assert_equal(expected, shortcut)); @@ -589,45 +583,46 @@ TEST( SymbolicBayesTree, linear_smoother_shortcuts ) { { // check shortcut P(S3||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[3]; // 3 is frontal in C3 + SymbolicBayesTree::Clique::shared_ptr c = + bayesTree[3]; // 3 is frontal in C3 SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(4, 5)); + auto expected = SymbolicBayesNet(SymbolicConditional(4, 5)); EXPECT(assert_equal(expected, shortcut)); } { // check shortcut P(S4||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2]; // 2 is frontal in C4 + SymbolicBayesTree::Clique::shared_ptr c = + bayesTree[2]; // 2 is frontal in C4 SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(3, 5)); + auto expected = SymbolicBayesNet(SymbolicConditional(3, 5)); EXPECT(assert_equal(expected, shortcut)); } } /* ************************************************************************* */ // from testSymbolicJunctionTree, which failed at one point -TEST(SymbolicBayesTree, complicatedMarginal) -{ +TEST(SymbolicBayesTree, complicatedMarginal) { // Create the conditionals to go in the BayesTree - SymbolicBayesTreeClique::shared_ptr cur; - SymbolicBayesTreeClique::shared_ptr root = MakeClique(list_of(11)(12), 2); + sharedClique cur; + sharedClique root = MakeClique(MakeKeys(11)(12), 2); cur = root; - root->children += MakeClique(list_of(9)(10)(11)(12), 2); + root->children.push_back(MakeClique(MakeKeys(9)(10)(11)(12), 2)); root->children.back()->parent_ = root; - root->children += MakeClique(list_of(7)(8)(11), 2); + root->children.push_back(MakeClique(MakeKeys(7)(8)(11), 2)); root->children.back()->parent_ = root; cur = root->children.back(); - cur->children += MakeClique(list_of(5)(6)(7)(8), 2); + cur->children.push_back(MakeClique(MakeKeys(5)(6)(7)(8), 2)); cur->children.back()->parent_ = cur; cur = cur->children.back(); - cur->children += MakeClique(list_of(3)(4)(6), 2); + cur->children.push_back(MakeClique(MakeKeys(3)(4)(6), 2)); cur->children.back()->parent_ = cur; - cur->children += MakeClique(list_of(1)(2)(5), 2); + cur->children.push_back(MakeClique(MakeKeys(1)(2)(5), 2)); cur->children.back()->parent_ = cur; // Create Bayes Tree @@ -656,9 +651,8 @@ TEST(SymbolicBayesTree, complicatedMarginal) { SymbolicBayesTree::Clique::shared_ptr c = bt[5]; SymbolicBayesNet shortcut = c->shortcut(root); - SymbolicBayesNet expected = list_of - (SymbolicConditional(7, 8, 11)) - (SymbolicConditional(8, 11)); + auto expected = SymbolicBayesNet(SymbolicConditional(7, 8, 11))( + SymbolicConditional(8, 11)); EXPECT(assert_equal(expected, shortcut)); } @@ -666,7 +660,7 @@ TEST(SymbolicBayesTree, complicatedMarginal) { SymbolicBayesTree::Clique::shared_ptr c = bt[3]; SymbolicBayesNet shortcut = c->shortcut(root); - SymbolicBayesNet expected = list_of(SymbolicConditional(6, 11)); + auto expected = SymbolicBayesNet(SymbolicConditional(6, 11)); EXPECT(assert_equal(expected, shortcut)); } @@ -674,7 +668,7 @@ TEST(SymbolicBayesTree, complicatedMarginal) { SymbolicBayesTree::Clique::shared_ptr c = bt[1]; SymbolicBayesNet shortcut = c->shortcut(root); - SymbolicBayesNet expected = list_of(SymbolicConditional(5, 11)); + auto expected = SymbolicBayesNet(SymbolicConditional(5, 11)); EXPECT(assert_equal(expected, shortcut)); } @@ -689,12 +683,10 @@ TEST(SymbolicBayesTree, complicatedMarginal) SymbolicFactor::shared_ptr actual = bt.marginalFactor(6); EXPECT(assert_equal(SymbolicFactor(6), *actual, 1e-1)); } - } /* ************************************************************************* */ TEST(SymbolicBayesTree, COLAMDvsMETIS) { - // create circular graph SymbolicFactorGraph sfg; sfg.push_factor(0, 1); @@ -707,7 +699,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // COLAMD { Ordering ordering = Ordering::Create(Ordering::COLAMD, sfg); - EXPECT(assert_equal(Ordering(list_of(0)(5)(1)(4)(2)(3)), ordering)); + EXPECT(assert_equal(Ordering{0, 5, 1, 4, 2, 3}, ordering)); // - P( 4 2 3) // | - P( 1 | 2 4) @@ -715,12 +707,12 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // | | | - P( 0 | 1 5) SymbolicBayesTree expected; expected.insertRoot( - MakeClique(list_of(4)(2)(3), 3, - list_of( - MakeClique(list_of(1)(2)(4), 1, - list_of( - MakeClique(list_of(5)(1)(4), 1, - list_of(MakeClique(list_of(0)(1)(5), 1)))))))); + MakeClique(MakeKeys(4)(2)(3), 3, + MakeCliques(MakeClique( + MakeKeys(1)(2)(4), 1, + MakeCliques(MakeClique( + MakeKeys(5)(1)(4), 1, + MakeCliques(MakeClique(MakeKeys(0)(1)(5), 1)))))))); SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); @@ -732,11 +724,11 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { Ordering ordering = Ordering::Create(Ordering::METIS, sfg); // Linux and Mac split differently when using mettis #if defined(__APPLE__) - EXPECT(assert_equal(Ordering(list_of(5)(4)(2)(1)(0)(3)), ordering)); + EXPECT(assert_equal(Ordering{5, 4, 2, 1, 0, 3}, ordering)); #elif defined(_WIN32) - EXPECT(assert_equal(Ordering(list_of(4)(3)(1)(0)(5)(2)), ordering)); + EXPECT(assert_equal(Ordering{4, 3, 1, 0, 5, 2}, ordering)); #else - EXPECT(assert_equal(Ordering(list_of(3)(2)(5)(0)(4)(1)), ordering)); + EXPECT(assert_equal(Ordering{3, 2, 0, 5, 4, 1}, ordering)); #endif // - P( 1 0 3) @@ -745,26 +737,23 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // | - P( 2 | 1 3) SymbolicBayesTree expected; #if defined(__APPLE__) - expected.insertRoot( - MakeClique(list_of(1)(0)(3), 3, - list_of( - MakeClique(list_of(4)(0)(3), 1, - list_of(MakeClique(list_of(5)(0)(4), 1))))( - MakeClique(list_of(2)(1)(3), 1)))); + expected.insertRoot(MakeClique( + MakeKeys(1)(0)(3), 3, + MakeCliques(MakeClique(MakeKeys(4)(0)(3), 1, + MakeCliques(MakeClique(MakeKeys(5)(0)(4), 1))))( + MakeClique(MakeKeys(2)(1)(3), 1)))); #elif defined(_WIN32) - expected.insertRoot( - MakeClique(list_of(3)(5)(2), 3, - list_of( - MakeClique(list_of(4)(3)(5), 1, - list_of(MakeClique(list_of(0)(2)(5), 1))))( - MakeClique(list_of(1)(0)(2), 1)))); + expected.insertRoot(MakeClique( + MakeKeys(3)(5)(2), 3, + MakeCliques(MakeClique(MakeKeys(4)(3)(5), 1, + MakeCliques(MakeClique(MakeKeys(0)(2)(5), 1))))( + MakeClique(MakeKeys(1)(0)(2), 1)))); #else - expected.insertRoot( - MakeClique(list_of(2)(4)(1), 3, - list_of( - MakeClique(list_of(0)(1)(4), 1, - list_of(MakeClique(list_of(5)(0)(4), 1))))( - MakeClique(list_of(3)(2)(4), 1)))); + expected.insertRoot(MakeClique( + MakeKeys(2)(4)(1), 3, + MakeCliques(MakeClique(MakeKeys(0)(1)(4), 1, + MakeCliques(MakeClique(MakeKeys(5)(0)(4), 1))))( + MakeClique(MakeKeys(3)(2)(4), 1)))); #endif SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); @@ -778,4 +767,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 78bb2182c7..9234d8a514 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -17,47 +17,45 @@ */ #include - -#include -#include -#include - #include -#include #include +#include + +#include +#include #include "symbolicExampleGraphs.h" using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace std; -using boost::assign::list_of; +using sharedNode = SymbolicEliminationTree::sharedNode; class EliminationTreeTester { -public: + public: // build hardcoded tree - static SymbolicEliminationTree buildHardcodedTree(const SymbolicFactorGraph& fg) { - - SymbolicEliminationTree::sharedNode leaf0(new SymbolicEliminationTree::Node); + static SymbolicEliminationTree buildHardcodedTree( + const SymbolicFactorGraph& fg) { + sharedNode leaf0(new SymbolicEliminationTree::Node); leaf0->key = 0; leaf0->factors.push_back(fg[0]); leaf0->factors.push_back(fg[1]); - SymbolicEliminationTree::sharedNode node1(new SymbolicEliminationTree::Node); + sharedNode node1(new SymbolicEliminationTree::Node); node1->key = 1; node1->factors.push_back(fg[2]); node1->children.push_back(leaf0); - SymbolicEliminationTree::sharedNode node2(new SymbolicEliminationTree::Node); + sharedNode node2(new SymbolicEliminationTree::Node); node2->key = 2; node2->factors.push_back(fg[3]); node2->children.push_back(node1); - SymbolicEliminationTree::sharedNode leaf3(new SymbolicEliminationTree::Node); + sharedNode leaf3(new SymbolicEliminationTree::Node); leaf3->key = 3; leaf3->factors.push_back(fg[4]); - SymbolicEliminationTree::sharedNode root(new SymbolicEliminationTree::Node); + sharedNode root(new SymbolicEliminationTree::Node); root->key = 4; root->children.push_back(leaf3); root->children.push_back(node2); @@ -67,29 +65,27 @@ class EliminationTreeTester { return tree; } - template - static SymbolicEliminationTree MakeTree(const ROOTS& roots) - { + static SymbolicEliminationTree MakeTree( + const std::vector& roots) { SymbolicEliminationTree et; et.roots_.assign(roots.begin(), roots.end()); return et; } }; -template -static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& factors) -{ - SymbolicEliminationTree::sharedNode node = boost::make_shared(); +template +static sharedNode MakeNode(Key key, const FACTORS& factors) { + sharedNode node = boost::make_shared(); node->key = key; SymbolicFactorGraph factorsAsGraph = factors; node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); return node; } -template -static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& factors, const CHILDREN& children) -{ - SymbolicEliminationTree::sharedNode node = boost::make_shared(); +template +static sharedNode MakeNode(Key key, const FACTORS& factors, + const std::vector& children) { + sharedNode node = boost::make_shared(); node->key = key; SymbolicFactorGraph factorsAsGraph = factors; node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); @@ -97,24 +93,39 @@ static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& fact return node; } +template +class ListOf { + public: + ListOf(Class&& c) { result.push_back(c); } + + ListOf& operator()(Class&& c) { + result.push_back(c); + return *this; + } + + operator std::vector() { return result; } + + private: + std::vector result; +}; + +using Nodes = ListOf; /* ************************************************************************* */ -TEST(EliminationTree, Create) -{ +TEST(EliminationTree, Create) { SymbolicEliminationTree expected = - EliminationTreeTester::buildHardcodedTree(simpleTestGraph1); + EliminationTreeTester::buildHardcodedTree(simpleTestGraph1); // Build from factor graph Ordering order; - order += 0,1,2,3,4; + order += 0, 1, 2, 3, 4; SymbolicEliminationTree actual(simpleTestGraph1, order); CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST(EliminationTree, Create2) -{ +TEST(EliminationTree, Create2) { // l1 l2 // / | / | // x1 --- x2 --- x3 --- x4 --- x5 @@ -132,20 +143,31 @@ TEST(EliminationTree, Create2) graph += SymbolicFactor(X(4), L(3)); graph += SymbolicFactor(X(5), L(3)); - SymbolicEliminationTree expected = EliminationTreeTester::MakeTree(list_of - (MakeNode(X(3), SymbolicFactorGraph(), list_of - (MakeNode(X(2), list_of(SymbolicFactor(X(2), X(3))), list_of - (MakeNode(L(1), list_of(SymbolicFactor(X(2), L(1))), list_of - (MakeNode(X(1), list_of(SymbolicFactor(X(1), L(1))) (SymbolicFactor(X(1), X(2))))))))) - (MakeNode(X(4), list_of(SymbolicFactor(X(3), X(4))), list_of - (MakeNode(L(2), list_of(SymbolicFactor(X(4), L(2))), list_of - (MakeNode(X(5), list_of(SymbolicFactor(X(4), X(5))) (SymbolicFactor(L(2), X(5))), list_of - (MakeNode(L(3), list_of(SymbolicFactor(X(4), L(3))) (SymbolicFactor(X(5), L(3)))))))))))))); - - Ordering order = list_of(X(1)) (L(3)) (L(1)) (X(5)) (X(2)) (L(2)) (X(4)) (X(3)); - + SymbolicEliminationTree expected = + EliminationTreeTester::MakeTree(Nodes(MakeNode( + X(3), SymbolicFactorGraph(), + Nodes(MakeNode( + X(2), SymbolicFactorGraph(SymbolicFactor(X(2), X(3))), + Nodes(MakeNode( + L(1), SymbolicFactorGraph(SymbolicFactor(X(2), L(1))), + Nodes(MakeNode( + X(1), SymbolicFactorGraph(SymbolicFactor(X(1), L(1)))( + SymbolicFactor(X(1), X(2)))))))))( + MakeNode( + X(4), SymbolicFactorGraph(SymbolicFactor(X(3), X(4))), + Nodes(MakeNode( + L(2), SymbolicFactorGraph(SymbolicFactor(X(4), L(2))), + Nodes(MakeNode( + X(5), + SymbolicFactorGraph(SymbolicFactor(X(4), X(5)))( + SymbolicFactor(L(2), X(5))), + Nodes(MakeNode( + L(3), + SymbolicFactorGraph(SymbolicFactor(X(4), L(3)))( + SymbolicFactor(X(5), L(3)))))))))))))); + + const Ordering order{X(1), L(3), L(1), X(5), X(2), L(2), X(4), X(3)}; SymbolicEliminationTree actual(graph, order); - EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp index 105270c9c1..a8001232a9 100644 --- a/gtsam/symbolic/tests/testSymbolicFactor.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp @@ -26,7 +26,6 @@ using namespace std; using namespace gtsam; -using namespace boost::assign; /* ************************************************************************* */ #ifdef TRACK_ELIMINATE diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 9662642906..8d5885d873 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -15,33 +15,29 @@ * @author Christian Potthast **/ -#include - +#include #include #include #include #include +#include #include #include -#include -using namespace boost::assign; - using namespace std; using namespace gtsam; -using namespace boost::assign; /* ************************************************************************* */ TEST(SymbolicFactorGraph, keys1) { - KeySet expected {0, 1, 2, 3, 4}; + KeySet expected{0, 1, 2, 3, 4}; KeySet actual = simpleTestGraph1.keys(); EXPECT(expected == actual); } /* ************************************************************************* */ TEST(SymbolicFactorGraph, keys2) { - KeySet expected {0, 1, 2, 3, 4, 5}; + KeySet expected{0, 1, 2, 3, 4, 5}; KeySet actual = simpleTestGraph2.keys(); EXPECT(expected == actual); } @@ -61,12 +57,12 @@ TEST(SymbolicFactorGraph, eliminateFullSequential) { /* ************************************************************************* */ TEST(SymbolicFactorGraph, eliminatePartialSequential) { // Eliminate 0 and 1 - const Ordering order {0, 1}; + const Ordering order{0, 1}; - const SymbolicBayesNet expectedBayesNet = - list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3, 4)); + const auto expectedBayesNet = SymbolicBayesNet(SymbolicConditional(0, 1, 2))( + SymbolicConditional(1, 2, 3, 4)); - const SymbolicFactorGraph expectedSfg = list_of(SymbolicFactor(2, 3))( + const auto expectedSfg = SymbolicFactorGraph(SymbolicFactor(2, 3))( SymbolicFactor(4, 5))(SymbolicFactor(2, 3, 4)); SymbolicBayesNet::shared_ptr actualBayesNet; @@ -106,9 +102,9 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { expectedBayesTree.insertRoot( boost::make_shared(root)); - SymbolicFactorGraph expectedFactorGraph = - list_of(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))(SymbolicFactor(1, 3))( - SymbolicFactor(2, 3))(SymbolicFactor(1)); + const auto expectedFactorGraph = + SymbolicFactorGraph(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))( + SymbolicFactor(1, 3))(SymbolicFactor(2, 3))(SymbolicFactor(1)); SymbolicBayesTree::shared_ptr actualBayesTree; SymbolicFactorGraph::shared_ptr actualFactorGraph; @@ -137,12 +133,12 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { /* ************************************************************************* */ TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) { - SymbolicBayesNet expectedBayesNet = - list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3))( - SymbolicConditional(2, 3))(SymbolicConditional(3)); + auto expectedBayesNet = + SymbolicBayesNet(SymbolicConditional(0, 1, 2))(SymbolicConditional( + 1, 2, 3))(SymbolicConditional(2, 3))(SymbolicConditional(3)); - SymbolicBayesNet actual1 = *simpleTestGraph2.marginalMultifrontalBayesNet( - Ordering{0, 1, 2, 3}); + SymbolicBayesNet actual1 = + *simpleTestGraph2.marginalMultifrontalBayesNet(Ordering{0, 1, 2, 3}); EXPECT(assert_equal(expectedBayesNet, actual1)); } @@ -192,7 +188,7 @@ TEST(SymbolicFactorGraph, marginals) { { // jointBayesNet - Ordering ord {0, 4, 3}; + Ordering ord{0, 4, 3}; auto actual = fg.eliminatePartialSequential(ord); SymbolicBayesNet expectedBN; expectedBN.emplace_shared(0, 1, 2); @@ -203,7 +199,7 @@ TEST(SymbolicFactorGraph, marginals) { { // jointBayesNet - Ordering ord {0, 2, 3}; + Ordering ord{0, 2, 3}; auto actual = fg.eliminatePartialSequential(ord); SymbolicBayesNet expectedBN; expectedBN.emplace_shared(0, 1, 2); @@ -302,7 +298,7 @@ TEST(SymbolicFactorGraph, add_factors) { expected.push_factor(1); expected.push_factor(11); expected.push_factor(2); - const FactorIndices expectedIndices {1, 3}; + const FactorIndices expectedIndices{1, 3}; const FactorIndices actualIndices = fg1.add_factors(fg2, true); EXPECT(assert_equal(expected, fg1)); @@ -310,7 +306,7 @@ TEST(SymbolicFactorGraph, add_factors) { expected.push_factor(1); expected.push_factor(2); - const FactorIndices expectedIndices2 {4, 5}; + const FactorIndices expectedIndices2{4, 5}; const FactorIndices actualIndices2 = fg1.add_factors(fg2, false); EXPECT(assert_equal(expected, fg1)); From 9b5321ce034d06c486e312790774492e57f68ac8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 23:58:35 -0800 Subject: [PATCH 13/37] Remove unused boost::assign headers --- examples/CreateSFMExampleData.cpp | 3 --- examples/TimeTBB.cpp | 2 -- gtsam_unstable/discrete/Constraint.h | 1 - gtsam_unstable/partition/tests/testNestedDissection.cpp | 4 ---- gtsam_unstable/slam/DummyFactor.cpp | 4 ---- gtsam_unstable/slam/tests/testSerialization.cpp | 2 -- .../tests/testSmartProjectionPoseFactorRollingShutter.cpp | 2 -- gtsam_unstable/timing/timeDSFvariants.cpp | 2 -- tests/testDoglegOptimizer.cpp | 1 - tests/testGaussianISAM.cpp | 2 -- tests/testManifold.cpp | 4 ---- tests/testNonlinearFactorGraph.cpp | 4 ---- tests/testNonlinearOptimizer.cpp | 2 -- tests/testPCGSolver.cpp | 2 -- tests/testSubgraphSolver.cpp | 3 --- timing/timeGaussianFactor.cpp | 4 +--- timing/timeGaussianFactorGraph.cpp | 2 -- timing/timeSchurFactors.cpp | 3 --- 18 files changed, 1 insertion(+), 46 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 97f4c84dc8..cf7755bae6 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -18,9 +18,6 @@ #include #include -#include - -using namespace boost::assign; using namespace std; using namespace gtsam; diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index 4bc5144f44..acaf3f3042 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -18,13 +18,11 @@ #include #include -#include #include #include using namespace std; using namespace gtsam; -using boost::assign::list_of; #ifdef GTSAM_USE_TBB diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index d0695002d0..691c659fc4 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -21,7 +21,6 @@ #include #include -#include #include #include diff --git a/gtsam_unstable/partition/tests/testNestedDissection.cpp b/gtsam_unstable/partition/tests/testNestedDissection.cpp index acaa5557e0..36ce647c30 100644 --- a/gtsam_unstable/partition/tests/testNestedDissection.cpp +++ b/gtsam_unstable/partition/tests/testNestedDissection.cpp @@ -6,10 +6,6 @@ * Description: unit tests for NestedDissection */ -#include // for operator += -#include // for operator += -#include // for operator += -using namespace boost::assign; #include #include diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index d2da5977b8..f1897cf855 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -7,10 +7,6 @@ #include -#include - -using namespace boost::assign; - namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index 362cf3778f..5244710e85 100644 --- a/gtsam_unstable/slam/tests/testSerialization.cpp +++ b/gtsam_unstable/slam/tests/testSerialization.cpp @@ -17,7 +17,6 @@ #include -#include #include #include @@ -27,7 +26,6 @@ using namespace std; using namespace gtsam; -using namespace boost::assign; namespace fs = boost::filesystem; #ifdef TOPSRCDIR static string topdir = TOPSRCDIR; diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index b88a27c6ca..cad1feae10 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -25,14 +25,12 @@ #include #include -#include #include #include "gtsam/slam/tests/smartFactorScenarios.h" #define DISABLE_TIMING using namespace gtsam; -using namespace boost::assign; using namespace std::placeholders; static const double rankTol = 1.0; diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp index a7b9136f86..3da01bfafa 100644 --- a/gtsam_unstable/timing/timeDSFvariants.cpp +++ b/gtsam_unstable/timing/timeDSFvariants.cpp @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -32,7 +31,6 @@ using namespace std; using namespace gtsam; -using namespace boost::assign; using boost::format; int main(int argc, char* argv[]) { diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index ec41bf6786..e9ae2454b8 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -29,7 +29,6 @@ #include #include -#include // for 'list_of()' #include #include diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 309f0e1e08..1c1ce90d51 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -22,8 +22,6 @@ #include #include -#include // for operator += -using namespace boost::assign; #include namespace br { using namespace boost::adaptors; using namespace boost::range; } diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index a05c4b621a..7d788d1099 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -29,10 +29,6 @@ #undef CHECK #include -#include -using boost::assign::list_of; -using boost::assign::map_list_of; - using namespace std; using namespace gtsam; diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index e1a88d6169..136cd064f2 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -32,10 +32,6 @@ #include -#include -#include -using namespace boost::assign; - /*STL/C++*/ #include diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 295721cc42..cc82892f73 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -33,8 +33,6 @@ #include #include -#include // for operator += -using namespace boost::assign; using boost::adaptors::map_values; #include diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 9d6cc49ac4..558f7c1e48 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -27,8 +27,6 @@ #include #include -#include // for operator += -using namespace boost::assign; #include #include diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 5d8d88775b..69b5fe5f93 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -28,9 +28,6 @@ #include -#include -using namespace boost::assign; - using namespace std; using namespace gtsam; diff --git a/timing/timeGaussianFactor.cpp b/timing/timeGaussianFactor.cpp index 3aecac3c11..feb738439a 100644 --- a/timing/timeGaussianFactor.cpp +++ b/timing/timeGaussianFactor.cpp @@ -22,7 +22,6 @@ using namespace std; #include -#include #include #include @@ -31,7 +30,6 @@ using namespace std; #include using namespace gtsam; -using namespace boost::assign; static const Key _x1_=1, _x2_=2, _l1_=3; @@ -109,7 +107,7 @@ int main() for(int i = 0; i < n; i++) boost::tie(conditional, factor) = - JacobianFactor(combined).eliminate(Ordering(boost::assign::list_of(_x2_))); + JacobianFactor(combined).eliminate(Ordering{_x2_}); long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; diff --git a/timing/timeGaussianFactorGraph.cpp b/timing/timeGaussianFactorGraph.cpp index 2c1e115868..574579f84b 100644 --- a/timing/timeGaussianFactorGraph.cpp +++ b/timing/timeGaussianFactorGraph.cpp @@ -16,14 +16,12 @@ */ #include -#include // for operator += in Ordering #include #include using namespace std; using namespace gtsam; using namespace example; -using namespace boost::assign; /* ************************************************************************* */ // Create a Kalman smoother for t=1:T and optimize diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp index 9e057f8300..370486c5f7 100644 --- a/timing/timeSchurFactors.cpp +++ b/timing/timeSchurFactors.cpp @@ -14,12 +14,9 @@ #include #include -#include -#include #include using namespace std; -using namespace boost::assign; using namespace gtsam; #define SLOW From d3a40fbc7186af920076c7be80cae89fc3749c01 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 23:59:39 -0800 Subject: [PATCH 14/37] Use initializer lists in tests and gtsam_unstable --- gtsam/linear/tests/testNoiseModel.cpp | 6 +- gtsam/slam/tests/testInitializePose3.cpp | 2 +- gtsam/symbolic/SymbolicFactor.h | 8 +- gtsam_unstable/base/tests/testBTree.cpp | 13 +-- gtsam_unstable/base/tests/testDSF.cpp | 43 ++++---- .../discrete/examples/schedulingExample.cpp | 6 +- .../discrete/examples/schedulingQuals12.cpp | 3 +- .../discrete/examples/schedulingQuals13.cpp | 3 +- .../discrete/tests/testLoopyBelief.cpp | 6 +- .../linear/tests/testLinearEquality.cpp | 104 +++++++++--------- .../partition/tests/testFindSeparator.cpp | 36 +++--- .../partition/tests/testGenericGraph.cpp | 40 +++---- gtsam_unstable/slam/BiasedGPSFactor.h | 4 +- .../testSmartStereoProjectionFactorPP.cpp | 6 +- .../testSmartStereoProjectionPoseFactor.cpp | 6 +- tests/smallExample.h | 19 ++-- tests/testExpressionFactor.cpp | 16 ++- tests/testGaussianBayesTreeB.cpp | 15 +-- tests/testGaussianFactorGraphB.cpp | 30 +++-- tests/testGaussianISAM2.cpp | 12 +- tests/testGraph.cpp | 5 +- tests/testSubgraphPreconditioner.cpp | 6 +- timing/timePose2.cpp | 2 +- 23 files changed, 168 insertions(+), 223 deletions(-) diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 442923662f..c382e0f3ce 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -31,8 +31,8 @@ using namespace noiseModel; static const double kSigma = 2, kInverseSigma = 1.0 / kSigma, kVariance = kSigma * kSigma, prc = 1.0 / kVariance; -static const Matrix R = Matrix3::Identity() * kInverseSigma; -static const Matrix kCovariance = Matrix3::Identity() * kVariance; +static const Matrix R = I_3x3 * kInverseSigma; +static const Matrix kCovariance = I_3x3 * kVariance; static const Vector3 kSigmas(kSigma, kSigma, kSigma); /* ************************************************************************* */ @@ -720,7 +720,7 @@ TEST(NoiseModel, NonDiagonalGaussian) const Matrix3 info = R.transpose() * R; const Matrix3 cov = info.inverse(); const Vector3 e(1, 1, 1), white = R * e; - Matrix I = Matrix3::Identity(); + Matrix I = I_3x3; { diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index ed9e6bbbca..37d65320a5 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -155,7 +155,7 @@ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { /* *************************************************************************** */ TEST( InitializePose3, singleGradient ) { Rot3 R1 = Rot3(); - Matrix M = Matrix3::Zero(); + Matrix M = Z_3x3; M(0,1) = -1; M(1,0) = 1; M(2,2) = 1; Rot3 R2 = Rot3(M); double a = 6.010534238540223; diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index d0e8adb55f..208efafb87 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -124,15 +124,15 @@ namespace gtsam { return result; } - /** Constructor from a collection of keys - compatible with boost::assign::list_of and - * boost::assign::cref_list_of */ + /** Constructor from a collection of keys - compatible with boost assign::list_of and + * boost assign::cref_list_of */ template static SymbolicFactor FromKeys(const CONTAINER& keys) { return SymbolicFactor(Base::FromKeys(keys)); } - /** Constructor from a collection of keys - compatible with boost::assign::list_of and - * boost::assign::cref_list_of */ + /** Constructor from a collection of keys - compatible with boost assign::list_of and + * boost assign::cref_list_of */ template static SymbolicFactor::shared_ptr FromKeysShared(const CONTAINER& keys) { return FromIteratorsShared(keys.begin(), keys.end()); diff --git a/gtsam_unstable/base/tests/testBTree.cpp b/gtsam_unstable/base/tests/testBTree.cpp index fcdbd0393a..b74a5e5ea2 100644 --- a/gtsam_unstable/base/tests/testBTree.cpp +++ b/gtsam_unstable/base/tests/testBTree.cpp @@ -17,12 +17,12 @@ */ #include -#include // for += -using namespace boost::assign; #include #include +#include + using namespace std; using namespace gtsam; @@ -147,13 +147,12 @@ TEST( BTree, iterating ) // acid iterator test int sum = 0; - for(const KeyInt& p: tree) -sum += p.second; + for (const KeyInt& p : tree) sum += p.second; LONGS_EQUAL(15,sum) // STL iterator test - list expected, actual; - expected += p1,p2,p3,p4,p5; + auto expected = std::list {p1, p2, p3, p4, p5}; + std::list actual; copy (tree.begin(),tree.end(),back_inserter(actual)); CHECK(actual==expected) } @@ -181,7 +180,7 @@ TEST( BTree, stress ) tree = tree.add(key, value); LONGS_EQUAL(i,tree.size()) CHECK(tree.find(key) == value) - expected += make_pair(key, value); + expected.emplace_back(key, value); } // Check height is log(N) diff --git a/gtsam_unstable/base/tests/testDSF.cpp b/gtsam_unstable/base/tests/testDSF.cpp index c3a59aada0..019963e59d 100644 --- a/gtsam_unstable/base/tests/testDSF.cpp +++ b/gtsam_unstable/base/tests/testDSF.cpp @@ -20,10 +20,6 @@ #include -#include -#include -using namespace boost::assign; - #include using namespace std; @@ -88,7 +84,7 @@ TEST(DSF, makePair) { /* ************************************************************************* */ TEST(DSF, makeList) { DSFInt dsf; - list keys; keys += 5, 6, 7; + list keys{5, 6, 7}; dsf = dsf.makeList(keys); EXPECT(dsf.findSet(5) == dsf.findSet(7)); } @@ -112,7 +108,7 @@ TEST(DSF, sets) { map > sets = dsf.sets(); LONGS_EQUAL(1, sets.size()); - set expected; expected += 5, 6; + set expected{5, 6}; EXPECT(expected == sets[dsf.findSet(5)]); } @@ -127,7 +123,7 @@ TEST(DSF, sets2) { map > sets = dsf.sets(); LONGS_EQUAL(1, sets.size()); - set expected; expected += 5, 6, 7; + set expected{5, 6, 7}; EXPECT(expected == sets[dsf.findSet(5)]); } @@ -141,7 +137,7 @@ TEST(DSF, sets3) { map > sets = dsf.sets(); LONGS_EQUAL(2, sets.size()); - set expected; expected += 5, 6; + set expected{5, 6}; EXPECT(expected == sets[dsf.findSet(5)]); } @@ -152,11 +148,11 @@ TEST(DSF, partition) { dsf = dsf.makeSet(6); dsf = dsf.makeUnion(5,6); - list keys; keys += 5; + list keys{5}; map > partitions = dsf.partition(keys); LONGS_EQUAL(1, partitions.size()); - set expected; expected += 5; + set expected{5}; EXPECT(expected == partitions[dsf.findSet(5)]); } @@ -168,11 +164,11 @@ TEST(DSF, partition2) { dsf = dsf.makeSet(7); dsf = dsf.makeUnion(5,6); - list keys; keys += 7; + list keys{7}; map > partitions = dsf.partition(keys); LONGS_EQUAL(1, partitions.size()); - set expected; expected += 7; + set expected{7}; EXPECT(expected == partitions[dsf.findSet(7)]); } @@ -184,11 +180,11 @@ TEST(DSF, partition3) { dsf = dsf.makeSet(7); dsf = dsf.makeUnion(5,6); - list keys; keys += 5, 7; + list keys{5, 7}; map > partitions = dsf.partition(keys); LONGS_EQUAL(2, partitions.size()); - set expected; expected += 5; + set expected{5}; EXPECT(expected == partitions[dsf.findSet(5)]); } @@ -202,7 +198,7 @@ TEST(DSF, set) { set set = dsf.set(5); LONGS_EQUAL(2, set.size()); - std::set expected; expected += 5, 6; + std::set expected{5, 6}; EXPECT(expected == set); } @@ -217,7 +213,7 @@ TEST(DSF, set2) { set set = dsf.set(5); LONGS_EQUAL(3, set.size()); - std::set expected; expected += 5, 6, 7; + std::set expected{5, 6, 7}; EXPECT(expected == set); } @@ -261,7 +257,7 @@ TEST(DSF, flatten) { /* ************************************************************************* */ TEST(DSF, flatten2) { static string x1("x1"), x2("x2"), x3("x3"), x4("x4"); - list keys; keys += x1,x2,x3,x4; + list keys{x1, x2, x3, x4}; DSF dsf(keys); dsf = dsf.makeUnion(x1,x2); dsf = dsf.makeUnion(x3,x4); @@ -285,13 +281,12 @@ TEST(DSF, mergePairwiseMatches) { Measurement m22(2,2),m23(2,3),m25(2,5),m26(2,6); // in image 2 // Add them all - list measurements; - measurements += m11,m12,m14, m22,m23,m25,m26; + list measurements{m11, m12, m14, m22, m23, m25, m26}; // Create some "matches" typedef pair Match; - list matches; - matches += Match(m11,m22), Match(m12,m23), Match(m14,m25), Match(m14,m26); + list matches{Match(m11, m22), Match(m12, m23), Match(m14, m25), + Match(m14, m26)}; // Merge matches DSF dsf(measurements); @@ -310,15 +305,15 @@ TEST(DSF, mergePairwiseMatches) { // Check that we have three connected components EXPECT_LONGS_EQUAL(3, dsf.numSets()); - set expected1; expected1 += m11,m22; + set expected1{m11,m22}; set actual1 = dsf.set(m11); EXPECT(expected1 == actual1); - set expected2; expected2 += m12,m23; + set expected2{m12,m23}; set actual2 = dsf.set(m12); EXPECT(expected2 == actual2); - set expected3; expected3 += m14,m25,m26; + set expected3{m14,m25,m26}; set actual3 = dsf.set(m14); EXPECT(expected3 == actual3); } diff --git a/gtsam_unstable/discrete/examples/schedulingExample.cpp b/gtsam_unstable/discrete/examples/schedulingExample.cpp index c9024e018e..bf5f2f25cf 100644 --- a/gtsam_unstable/discrete/examples/schedulingExample.cpp +++ b/gtsam_unstable/discrete/examples/schedulingExample.cpp @@ -214,8 +214,7 @@ void sampleSolutions() { vector samplers(7); // Given the time-slots, we can create 7 independent samplers - vector slots; - slots += 16, 17, 11, 2, 0, 5, 9; // given slots + vector slots{16, 17, 11, 2, 0, 5, 9}; // given slots for (size_t i = 0; i < 7; i++) samplers[i] = createSampler(i, slots[i], schedulers); @@ -296,8 +295,7 @@ void accomodateStudent() { scheduler.print("scheduler"); // rule out all occupied slots - vector slots; - slots += 16, 17, 11, 2, 0, 5, 9, 14; + vector slots{16, 17, 11, 2, 0, 5, 9, 14}; vector slotsAvailable(scheduler.nrTimeSlots(), 1.0); for(size_t s: slots) slotsAvailable[s] = 0; diff --git a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp index d3529cb55e..a15b6f922d 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp @@ -223,8 +223,7 @@ void sampleSolutions() { vector samplers(NRSTUDENTS); // Given the time-slots, we can create NRSTUDENTS independent samplers - vector slots; - slots += 3, 20, 2, 6, 5, 11, 1, 4; // given slots + vector slots{3, 20, 2, 6, 5, 11, 1, 4}; // given slots for (size_t i = 0; i < NRSTUDENTS; i++) samplers[i] = createSampler(i, slots[i], schedulers); diff --git a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp index 27c9596750..e888875a45 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp @@ -248,8 +248,7 @@ void sampleSolutions() { vector samplers(NRSTUDENTS); // Given the time-slots, we can create NRSTUDENTS independent samplers - vector slots; - slots += 12,11,13, 21,16,1, 3,2,6, 7,22,4; // given slots + vector slots{12,11,13, 21,16,1, 3,2,6, 7,22,4}; // given slots for (size_t i = 0; i < NRSTUDENTS; i++) samplers[i] = createSampler(i, slots[i], schedulers); diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index eac0d834e6..0e27b5fbb6 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -11,14 +11,12 @@ #include #include -#include #include #include #include using namespace std; using namespace boost; -using namespace boost::assign; using namespace gtsam; /** @@ -105,7 +103,7 @@ class LoopyBelief { if (debug) subGraph.print("------- Subgraph:"); DiscreteFactor::shared_ptr message; boost::tie(dummyCond, message) = - EliminateDiscrete(subGraph, Ordering(list_of(neighbor))); + EliminateDiscrete(subGraph, Ordering{neighbor}); // store the new factor into messages messages.insert(make_pair(neighbor, message)); if (debug) message->print("------- Message: "); @@ -230,7 +228,7 @@ TEST_UNSAFE(LoopyBelief, construction) { // Map from key to DiscreteKey for building belief factor. // TODO: this is bad! - std::map allKeys = map_list_of(0, C)(1, S)(2, R)(3, W); + std::map allKeys{{0, C}, {1, S}, {2, R}, {3, W}}; // Build graph DecisionTreeFactor pC(C, "0.5 0.5"); diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index 36a2cacd8e..58af66a090 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -15,38 +15,31 @@ * @author Duy-Nguyen Ta **/ -#include +#include #include #include #include -#include - -#include -#include +#include using namespace std; using namespace gtsam; -using namespace boost::assign; -GTSAM_CONCEPT_TESTABLE_INST (LinearEquality) +GTSAM_CONCEPT_TESTABLE_INST(LinearEquality) namespace { namespace simple { // Terms we'll use -const vector > terms = list_of < pair - > (make_pair(5, Matrix3::Identity()))( - make_pair(10, 2 * Matrix3::Identity()))( - make_pair(15, 3 * Matrix3::Identity())); +const vector > terms{ + make_pair(5, I_3x3), make_pair(10, 2 * I_3x3), make_pair(15, 3 * I_3x3)}; // RHS and sigmas const Vector b = (Vector(3) << 1., 2., 3.).finished(); const SharedDiagonal noise = noiseModel::Constrained::All(3); -} -} +} // namespace simple +} // namespace /* ************************************************************************* */ -TEST(LinearEquality, constructors_and_accessors) -{ +TEST(LinearEquality, constructors_and_accessors) { using namespace simple; // Test for using different numbers of terms @@ -66,8 +59,8 @@ TEST(LinearEquality, constructors_and_accessors) // Two term constructor LinearEquality expected( boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0); - LinearEquality actual(terms[0].first, terms[0].second, - terms[1].first, terms[1].second, b, 0); + LinearEquality actual(terms[0].first, terms[0].second, terms[1].first, + terms[1].second, b, 0); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); @@ -79,8 +72,9 @@ TEST(LinearEquality, constructors_and_accessors) // Three term constructor LinearEquality expected( boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0); - LinearEquality actual(terms[0].first, terms[0].second, - terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0); + LinearEquality actual(terms[0].first, terms[0].second, terms[1].first, + terms[1].second, terms[2].first, terms[2].second, b, + 0); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); @@ -92,39 +86,38 @@ TEST(LinearEquality, constructors_and_accessors) /* ************************************************************************* */ TEST(LinearEquality, Hessian_conversion) { - HessianFactor hessian(0, (Matrix(4,4) << - 1.57, 2.695, -1.1, -2.35, - 2.695, 11.3125, -0.65, -10.225, - -1.1, -0.65, 1, 0.5, - -2.35, -10.225, 0.5, 9.25).finished(), - (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), - 73.1725); + HessianFactor hessian( + 0, + (Matrix(4, 4) << 1.57, 2.695, -1.1, -2.35, 2.695, 11.3125, -0.65, -10.225, + -1.1, -0.65, 1, 0.5, -2.35, -10.225, 0.5, 9.25) + .finished(), + (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), 73.1725); try { LinearEquality actual(hessian); EXPECT(false); - } - catch (const std::runtime_error& exception) { + } catch (const std::runtime_error& exception) { EXPECT(true); } } /* ************************************************************************* */ -TEST(LinearEquality, error) -{ +TEST(LinearEquality, error) { LinearEquality factor(simple::terms, simple::b, 0); VectorValues values; values.insert(5, Vector::Constant(3, 1.0)); values.insert(10, Vector::Constant(3, 0.5)); - values.insert(15, Vector::Constant(3, 1.0/3.0)); + values.insert(15, Vector::Constant(3, 1.0 / 3.0)); - Vector expected_unwhitened(3); expected_unwhitened << 2.0, 1.0, 0.0; + Vector expected_unwhitened(3); + expected_unwhitened << 2.0, 1.0, 0.0; Vector actual_unwhitened = factor.unweighted_error(values); EXPECT(assert_equal(expected_unwhitened, actual_unwhitened)); // whitened is meaningless in constraints - Vector expected_whitened(3); expected_whitened = expected_unwhitened; + Vector expected_whitened(3); + expected_whitened = expected_unwhitened; Vector actual_whitened = factor.error_vector(values); EXPECT(assert_equal(expected_whitened, actual_whitened)); @@ -134,13 +127,13 @@ TEST(LinearEquality, error) } /* ************************************************************************* */ -TEST(LinearEquality, matrices_NULL) -{ +TEST(LinearEquality, matrices_NULL) { // Make sure everything works with nullptr noise model LinearEquality factor(simple::terms, simple::b, 0); Matrix AExpected(3, 9); - AExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + AExpected << simple::terms[0].second, simple::terms[1].second, + simple::terms[2].second; Vector rhsExpected = simple::b; Matrix augmentedJacobianExpected(3, 10); augmentedJacobianExpected << AExpected, rhsExpected; @@ -153,24 +146,25 @@ TEST(LinearEquality, matrices_NULL) // Unwhitened Jacobian EXPECT(assert_equal(AExpected, factor.jacobianUnweighted().first)); EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); - EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); + EXPECT(assert_equal(augmentedJacobianExpected, + factor.augmentedJacobianUnweighted())); } /* ************************************************************************* */ -TEST(LinearEquality, matrices) -{ +TEST(LinearEquality, matrices) { // And now witgh a non-unit noise model LinearEquality factor(simple::terms, simple::b, 0); Matrix jacobianExpected(3, 9); - jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + jacobianExpected << simple::terms[0].second, simple::terms[1].second, + simple::terms[2].second; Vector rhsExpected = simple::b; Matrix augmentedJacobianExpected(3, 10); augmentedJacobianExpected << jacobianExpected, rhsExpected; Matrix augmentedHessianExpected = - augmentedJacobianExpected.transpose() * simple::noise->R().transpose() - * simple::noise->R() * augmentedJacobianExpected; + augmentedJacobianExpected.transpose() * simple::noise->R().transpose() * + simple::noise->R() * augmentedJacobianExpected; // Whitened Jacobian EXPECT(assert_equal(jacobianExpected, factor.jacobian().first)); @@ -180,35 +174,37 @@ TEST(LinearEquality, matrices) // Unwhitened Jacobian EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first)); EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); - EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); + EXPECT(assert_equal(augmentedJacobianExpected, + factor.augmentedJacobianUnweighted())); } /* ************************************************************************* */ -TEST(LinearEquality, operators ) -{ +TEST(LinearEquality, operators) { Matrix I = I_2x2; - Vector b = (Vector(2) << 0.2,-0.1).finished(); + Vector b = (Vector(2) << 0.2, -0.1).finished(); LinearEquality lf(1, -I, 2, I, b, 0); VectorValues c; - c.insert(1, (Vector(2) << 10.,20.).finished()); - c.insert(2, (Vector(2) << 30.,60.).finished()); + c.insert(1, (Vector(2) << 10., 20.).finished()); + c.insert(2, (Vector(2) << 30., 60.).finished()); // test A*x - Vector expectedE = (Vector(2) << 20.,40.).finished(); + Vector expectedE = (Vector(2) << 20., 40.).finished(); Vector actualE = lf * c; EXPECT(assert_equal(expectedE, actualE)); // test A^e VectorValues expectedX; - expectedX.insert(1, (Vector(2) << -20.,-40.).finished()); + expectedX.insert(1, (Vector(2) << -20., -40.).finished()); expectedX.insert(2, (Vector(2) << 20., 40.).finished()); VectorValues actualX = VectorValues::Zero(expectedX); lf.transposeMultiplyAdd(1.0, actualE, actualX); EXPECT(assert_equal(expectedX, actualX)); // test gradient at zero - Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); + Matrix A; + Vector b2; + boost::tie(A, b2) = lf.jacobian(); VectorValues expectedG; expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished()); expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished()); @@ -217,16 +213,14 @@ TEST(LinearEquality, operators ) } /* ************************************************************************* */ -TEST(LinearEquality, default_error ) -{ +TEST(LinearEquality, default_error) { LinearEquality f; double actual = f.error(VectorValues()); DOUBLES_EQUAL(0.0, actual, 1e-15); } //* ************************************************************************* */ -TEST(LinearEquality, empty ) -{ +TEST(LinearEquality, empty) { // create an empty factor LinearEquality f; EXPECT(f.empty()); diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index fe49de9289..49796f80d0 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -6,10 +6,6 @@ * Description: unit tests for FindSeparator */ -#include // for operator += -#include // for operator += -#include // for operator += -using namespace boost::assign; #include #include @@ -30,16 +26,16 @@ TEST ( Partition, separatorPartitionByMetis ) graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); - std::vector keys; keys += 0, 1, 2, 3, 4; + std::vector keys{0, 1, 2, 3, 4}; WorkSpace workspace(5); boost::optional actual = separatorPartitionByMetis(graph, keys, workspace, true); CHECK(actual.is_initialized()); - vector A_expected; A_expected += 0, 3; // frontal - vector B_expected; B_expected += 2, 4; // frontal - vector C_expected; C_expected += 1; // separator + vector A_expected{0, 3}; // frontal + vector B_expected{2, 4}; // frontal + vector C_expected{1}; // separator CHECK(A_expected == actual->A); CHECK(B_expected == actual->B); CHECK(C_expected == actual->C); @@ -55,16 +51,16 @@ TEST ( Partition, separatorPartitionByMetis2 ) graph.push_back(boost::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 5, 6; + std::vector keys{1, 2, 3, 5, 6}; WorkSpace workspace(8); boost::optional actual = separatorPartitionByMetis(graph, keys, workspace, true); CHECK(actual.is_initialized()); - vector A_expected; A_expected += 1, 5; // frontal - vector B_expected; B_expected += 3, 6; // frontal - vector C_expected; C_expected += 2; // separator + vector A_expected{1, 5}; // frontal + vector B_expected{3, 6}; // frontal + vector C_expected{2}; // separator CHECK(A_expected == actual->A); CHECK(B_expected == actual->B); CHECK(C_expected == actual->C); @@ -78,15 +74,15 @@ TEST ( Partition, edgePartitionByMetis ) graph.push_back(boost::make_shared(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D)); graph.push_back(boost::make_shared(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D)); graph.push_back(boost::make_shared(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D)); - std::vector keys; keys += 0, 1, 2, 3; + std::vector keys{0, 1, 2, 3}; WorkSpace workspace(6); boost::optional actual = edgePartitionByMetis(graph, keys, workspace, true); CHECK(actual.is_initialized()); - vector A_expected; A_expected += 0, 1; // frontal - vector B_expected; B_expected += 2, 3; // frontal + vector A_expected{0, 1}; // frontal + vector B_expected{2, 3}; // frontal vector C_expected; // separator // for(const size_t a: actual->A) // cout << a << " "; @@ -109,14 +105,14 @@ TEST ( Partition, edgePartitionByMetis2 ) graph.push_back(boost::make_shared(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D, 1)); graph.push_back(boost::make_shared(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D, 20)); graph.push_back(boost::make_shared(3, 4, 3, NODE_POSE_3D, NODE_POSE_3D, 1)); - std::vector keys; keys += 0, 1, 2, 3, 4; + std::vector keys{0, 1, 2, 3, 4}; WorkSpace workspace(6); boost::optional actual = edgePartitionByMetis(graph, keys, workspace, true); CHECK(actual.is_initialized()); - vector A_expected; A_expected += 0, 1; // frontal - vector B_expected; B_expected += 2, 3, 4; // frontal + vector A_expected{0, 1}; // frontal + vector B_expected{2, 3, 4}; // frontal vector C_expected; // separator CHECK(A_expected == actual->A); CHECK(B_expected == actual->B); @@ -133,7 +129,7 @@ TEST ( Partition, findSeparator ) graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); - std::vector keys; keys += 0, 1, 2, 3, 4; + std::vector keys{0, 1, 2, 3, 4}; WorkSpace workspace(5); int minNodesPerMap = -1; @@ -159,7 +155,7 @@ TEST ( Partition, findSeparator2 ) graph.push_back(boost::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 5, 6; + std::vector keys{1, 2, 3, 5, 6}; WorkSpace workspace(8); int minNodesPerMap = -1; diff --git a/gtsam_unstable/partition/tests/testGenericGraph.cpp b/gtsam_unstable/partition/tests/testGenericGraph.cpp index 8a32837f4c..dc7d260351 100644 --- a/gtsam_unstable/partition/tests/testGenericGraph.cpp +++ b/gtsam_unstable/partition/tests/testGenericGraph.cpp @@ -10,10 +10,6 @@ #include -#include // for operator += -#include // for operator += -#include // for operator += -using namespace boost::assign; #include #include @@ -44,13 +40,13 @@ TEST ( GenerciGraph, findIslands ) graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9; + std::vector keys{1, 2, 3, 4, 5, 6, 7, 8, 9}; WorkSpace workspace(10); // from 0 to 9 list > islands = findIslands(graph, keys, workspace, 7, 2); LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 1, 2, 3, 7, 8; - vector island2; island2 += 4, 5, 6, 9; + vector island1{1, 2, 3, 7, 8}; + vector island2{4, 5, 6, 9}; CHECK(island1 == islands.front()); CHECK(island2 == islands.back()); } @@ -77,12 +73,12 @@ TEST( GenerciGraph, findIslands2 ) graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8; + std::vector keys{1, 2, 3, 4, 5, 6, 7, 8}; WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here list > islands = findIslands(graph, keys, workspace, 7, 2); LONGS_EQUAL(1, islands.size()); - vector island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8; + vector island1{1, 2, 3, 4, 5, 6, 7, 8}; CHECK(island1 == islands.front()); } @@ -99,13 +95,13 @@ TEST ( GenerciGraph, findIslands3 ) graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5, 6; + std::vector keys{1, 2, 3, 4, 5, 6}; WorkSpace workspace(7); // from 0 to 9 list > islands = findIslands(graph, keys, workspace, 7, 2); LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 1, 5; - vector island2; island2 += 2, 3, 4, 6; + vector island1{1, 5}; + vector island2{2, 3, 4, 6}; CHECK(island1 == islands.front()); CHECK(island2 == islands.back()); } @@ -119,13 +115,13 @@ TEST ( GenerciGraph, findIslands4 ) GenericGraph2D graph; graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - std::vector keys; keys += 3, 4, 7; + std::vector keys{3, 4, 7}; WorkSpace workspace(8); // from 0 to 7 list > islands = findIslands(graph, keys, workspace, 7, 2); LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 3, 4; - vector island2; island2 += 7; + vector island1{3, 4}; + vector island2{7}; CHECK(island1 == islands.front()); CHECK(island2 == islands.back()); } @@ -147,13 +143,13 @@ TEST ( GenerciGraph, findIslands5 ) graph.push_back(boost::make_shared(1, NODE_POSE_2D, 3, NODE_POSE_2D)); graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5; + std::vector keys{1, 2, 3, 4, 5}; WorkSpace workspace(6); // from 0 to 5 list > islands = findIslands(graph, keys, workspace, 7, 2); LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 1, 3, 5; - vector island2; island2 += 2, 4; + vector island1{1, 3, 5}; + vector island2{2, 4}; CHECK(island1 == islands.front()); CHECK(island2 == islands.back()); } @@ -235,8 +231,8 @@ TEST ( GenericGraph, reduceGenericGraph2 ) /* ************************************************************************* */ TEST ( GenerciGraph, hasCommonCamera ) { - std::set cameras1; cameras1 += 1, 2, 3, 4, 5; - std::set cameras2; cameras2 += 8, 7, 6, 5; + std::set cameras1{1, 2, 3, 4, 5}; + std::set cameras2{8, 7, 6, 5}; bool actual = hasCommonCamera(cameras1, cameras2); CHECK(actual); } @@ -244,8 +240,8 @@ TEST ( GenerciGraph, hasCommonCamera ) /* ************************************************************************* */ TEST ( GenerciGraph, hasCommonCamera2 ) { - std::set cameras1; cameras1 += 1, 3, 5, 7; - std::set cameras2; cameras2 += 2, 4, 6, 8, 10; + std::set cameras1{1, 3, 5, 7}; + std::set cameras2{2, 4, 6, 8, 10}; bool actual = hasCommonCamera(cameras1, cameras2); CHECK(!actual); } diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index 6f0aa605b8..07706a6a5d 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -78,9 +78,9 @@ namespace gtsam { if (H1 || H2){ H1->resize(3,6); // jacobian wrt pose - (*H1) << Matrix3::Zero(), pose.rotation().matrix(); + (*H1) << Z_3x3, pose.rotation().matrix(); H2->resize(3,3); // jacobian wrt bias - (*H2) << Matrix3::Identity(); + (*H2) << I_3x3; } return pose.translation() + bias - measured_; } diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp index 1dbd256664..095bedfab2 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp @@ -23,12 +23,10 @@ #include #include #include -#include #include #include using namespace std; -using namespace boost::assign; using namespace gtsam; namespace { @@ -289,9 +287,7 @@ TEST( SmartProjectionPoseFactor, noiseless_error_multipleExtrinsics_missingMeasu EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // The following are generically exercising the triangulation - CameraSet cams; - cams += w_Camera_cam1; - cams += w_Camera_cam2; + CameraSet cams{w_Camera_cam1, w_Camera_cam2}; TriangulationResult result = factor1.triangulateSafe(cams); CHECK(result); EXPECT(assert_equal(landmark, *result, 1e-7)); diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index fc56b1a9f4..9f61d4fb0f 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -24,12 +24,10 @@ #include #include #include -#include #include #include using namespace std; -using namespace boost::assign; using namespace gtsam; namespace { @@ -220,9 +218,7 @@ TEST( SmartProjectionPoseFactor, noiselessWithMissingMeasurements ) { double actualError2 = factor1.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); - CameraSet cams; - cams += level_camera; - cams += level_camera_right; + CameraSet cams{level_camera, level_camera_right}; TriangulationResult result = factor1.triangulateSafe(cams); CHECK(result); EXPECT(assert_equal(landmark, *result, 1e-7)); diff --git a/tests/smallExample.h b/tests/smallExample.h index 38b202c418..7439a5436a 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -26,7 +26,6 @@ #include #include #include -#include namespace gtsam { namespace example { @@ -223,10 +222,9 @@ inline Values createValues() { /* ************************************************************************* */ inline VectorValues createVectorValues() { using namespace impl; - VectorValues c = boost::assign::pair_list_of - (_l1_, Vector2(0.0, -1.0)) - (_x1_, Vector2(0.0, 0.0)) - (_x2_, Vector2(1.5, 0.0)); + VectorValues c {{_l1_, Vector2(0.0, -1.0)}, + {_x1_, Vector2(0.0, 0.0)}, + {_x2_, Vector2(1.5, 0.0)}}; return c; } @@ -547,9 +545,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() { /* ************************************************************************* */ inline VectorValues createSingleConstraintValues() { using namespace impl; - VectorValues config = boost::assign::pair_list_of - (_x_, Vector2(1.0, -1.0)) - (_y_, Vector2(0.2, 0.1)); + VectorValues config{{_x_, Vector2(1.0, -1.0)}, {_y_, Vector2(0.2, 0.1)}}; return config; } @@ -611,10 +607,9 @@ inline GaussianFactorGraph createMultiConstraintGraph() { /* ************************************************************************* */ inline VectorValues createMultiConstraintValues() { using namespace impl; - VectorValues config = boost::assign::pair_list_of - (_x_, Vector2(-2.0, 2.0)) - (_y_, Vector2(-0.1, 0.4)) - (_z_, Vector2(-4.0, 5.0)); + VectorValues config{{_x_, Vector2(-2.0, 2.0)}, + {_y_, Vector2(-0.1, 0.4)}, + {_z_, Vector2(-4.0, 5.0)}}; return config; } diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 9a4e01c46f..bb35ada199 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -27,8 +27,6 @@ #include #include -#include -using boost::assign::list_of; using namespace std::placeholders; using namespace std; @@ -507,7 +505,7 @@ TEST(Expression, testMultipleCompositions) { // Leaf, key = 1 // Leaf, key = 2 Expression sum1_(Combine(1, 2), v1_, v2_); - EXPECT(sum1_.keys() == list_of(1)(2)); + EXPECT((sum1_.keys() == std::set{1, 2})); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance); // BinaryExpression(3,4) @@ -516,7 +514,7 @@ TEST(Expression, testMultipleCompositions) { // Leaf, key = 2 // Leaf, key = 1 Expression sum2_(Combine(3, 4), sum1_, v1_); - EXPECT(sum2_.keys() == list_of(1)(2)); + EXPECT((sum2_.keys() == std::set{1, 2})); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); // BinaryExpression(5,6) @@ -529,7 +527,7 @@ TEST(Expression, testMultipleCompositions) { // Leaf, key = 1 // Leaf, key = 2 Expression sum3_(Combine(5, 6), sum1_, sum2_); - EXPECT(sum3_.keys() == list_of(1)(2)); + EXPECT((sum3_.keys() == std::set{1, 2})); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); } @@ -558,19 +556,19 @@ TEST(Expression, testMultipleCompositions2) { Expression v3_(Key(3)); Expression sum1_(Combine(4,5), v1_, v2_); - EXPECT(sum1_.keys() == list_of(1)(2)); + EXPECT((sum1_.keys() == std::set{1, 2})); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance); Expression sum2_(combine3, v1_, v2_, v3_); - EXPECT(sum2_.keys() == list_of(1)(2)(3)); + EXPECT((sum2_.keys() == std::set{1, 2, 3})); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); Expression sum3_(combine3, v3_, v2_, v1_); - EXPECT(sum3_.keys() == list_of(1)(2)(3)); + EXPECT((sum3_.keys() == std::set{1, 2, 3})); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); Expression sum4_(combine3, sum1_, sum2_, sum3_); - EXPECT(sum4_.keys() == list_of(1)(2)(3)); + EXPECT((sum4_.keys() == std::set{1, 2, 3})); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance); } diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index a321aa25de..fedc759454 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -26,9 +26,6 @@ #include -#include // for operator += -using namespace boost::assign; - using namespace std; using namespace gtsam; using namespace example; @@ -241,10 +238,10 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) const Matrix I = I_2x2, A = -0.00429185*I; // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7) - GaussianBayesNet expected1 = list_of + GaussianBayesNet expected1; // Why does the sign get flipped on the prior? - (GaussianConditional(X(1), Z_2x1, I/sigmax7, X(7), A/sigmax7)) - (GaussianConditional(X(7), Z_2x1, -1*I/sigmax7)); + expected1.emplace_shared(X(1), Z_2x1, I/sigmax7, X(7), A/sigmax7); + expected1.emplace_shared(X(7), Z_2x1, -1*I/sigmax7); GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7)); EXPECT(assert_equal(expected1, actual1, tol)); @@ -260,9 +257,9 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) // Check the joint density P(x1,x4), i.e. with a root variable double sig14 = 0.784465; Matrix A14 = -0.0769231*I; - GaussianBayesNet expected3 = list_of - (GaussianConditional(X(1), Z_2x1, I/sig14, X(4), A14/sig14)) - (GaussianConditional(X(4), Z_2x1, I/sigmax4)); + GaussianBayesNet expected3; + expected3.emplace_shared(X(1), Z_2x1, I/sig14, X(4), A14/sig14); + expected3.emplace_shared(X(4), Z_2x1, I/sigmax4); GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4)); EXPECT(assert_equal(expected3,actual3,tol)); diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 1f5ec63506..a6aa7bfc56 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -27,10 +27,6 @@ #include #include -#include // for operator += -#include // for operator += -#include // for operator += -using namespace boost::assign; #include namespace br { using namespace boost::range; using namespace boost::adaptors; } @@ -73,7 +69,7 @@ TEST(GaussianFactorGraph, eliminateOne_x1) { GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianConditional::shared_ptr conditional; - auto result = fg.eliminatePartialSequential(Ordering(list_of(X(1)))); + auto result = fg.eliminatePartialSequential(Ordering{X(1)}); conditional = result.first->front(); // create expected Conditional Gaussian @@ -89,7 +85,7 @@ TEST(GaussianFactorGraph, eliminateOne_x2) { Ordering ordering; ordering += X(2), L(1), X(1); GaussianFactorGraph fg = createGaussianFactorGraph(); - auto actual = EliminateQR(fg, Ordering(list_of(X(2)))).first; + auto actual = EliminateQR(fg, Ordering{X(2)}).first; // create expected Conditional Gaussian double sigma = 0.0894427; @@ -105,7 +101,7 @@ TEST(GaussianFactorGraph, eliminateOne_l1) { Ordering ordering; ordering += L(1), X(1), X(2); GaussianFactorGraph fg = createGaussianFactorGraph(); - auto actual = EliminateQR(fg, Ordering(list_of(L(1)))).first; + auto actual = EliminateQR(fg, Ordering{L(1)}).first; // create expected Conditional Gaussian double sigma = sqrt(2.0) / 10.; @@ -121,7 +117,7 @@ TEST(GaussianFactorGraph, eliminateOne_x1_fast) { GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianConditional::shared_ptr conditional; JacobianFactor::shared_ptr remaining; - boost::tie(conditional, remaining) = EliminateQR(fg, Ordering(list_of(X(1)))); + boost::tie(conditional, remaining) = EliminateQR(fg, Ordering{X(1)}); // create expected Conditional Gaussian Matrix I = 15 * I_2x2, R11 = I, S12 = -0.111111 * I, S13 = -0.444444 * I; @@ -144,7 +140,7 @@ TEST(GaussianFactorGraph, eliminateOne_x1_fast) { /* ************************************************************************* */ TEST(GaussianFactorGraph, eliminateOne_x2_fast) { GaussianFactorGraph fg = createGaussianFactorGraph(); - auto actual = EliminateQR(fg, Ordering(list_of(X(2)))).first; + auto actual = EliminateQR(fg, Ordering{X(2)}).first; // create expected Conditional Gaussian double sigma = 0.0894427; @@ -158,7 +154,7 @@ TEST(GaussianFactorGraph, eliminateOne_x2_fast) { /* ************************************************************************* */ TEST(GaussianFactorGraph, eliminateOne_l1_fast) { GaussianFactorGraph fg = createGaussianFactorGraph(); - auto actual = EliminateQR(fg, Ordering(list_of(L(1)))).first; + auto actual = EliminateQR(fg, Ordering{L(1)}).first; // create expected Conditional Gaussian double sigma = sqrt(2.0) / 10.; @@ -272,10 +268,10 @@ TEST(GaussianFactorGraph, multiplication) { VectorValues x = createCorrectDelta(); Errors actual = A * x; Errors expected; - expected += Vector2(-1.0, -1.0); - expected += Vector2(2.0, -1.0); - expected += Vector2(0.0, 1.0); - expected += Vector2(-1.0, 1.5); + expected.push_back(Vector2(-1.0, -1.0)); + expected.push_back(Vector2(2.0, -1.0)); + expected.push_back(Vector2(0.0, 1.0)); + expected.push_back(Vector2(-1.0, 1.5)); EXPECT(assert_equal(expected, actual)); } @@ -287,9 +283,9 @@ TEST(GaussianFactorGraph, elimination) { Matrix Ap = I_1x1, An = I_1x1 * -1; Vector b = (Vector(1) << 0.0).finished(); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1, 2.0); - fg += JacobianFactor(X(1), An, X(2), Ap, b, sigma); - fg += JacobianFactor(X(1), Ap, b, sigma); - fg += JacobianFactor(X(2), Ap, b, sigma); + fg.emplace_shared(X(1), An, X(2), Ap, b, sigma); + fg.emplace_shared(X(1), Ap, b, sigma); + fg.emplace_shared(X(2), Ap, b, sigma); // Eliminate Ordering ordering; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index e8e916f046..e7113e0fa9 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -24,9 +24,7 @@ #include -#include #include -using namespace boost::assign; namespace br { using namespace boost::adaptors; using namespace boost::range; } using namespace std; @@ -686,7 +684,7 @@ TEST(ISAM2, marginalizeLeaves1) { isam.update(factors, values, FactorIndices(), constrainedKeys); - FastList leafKeys = list_of(0); + std::list leafKeys {0}; EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -716,7 +714,7 @@ TEST(ISAM2, marginalizeLeaves2) { isam.update(factors, values, FactorIndices(), constrainedKeys); - FastList leafKeys = list_of(0); + std::list leafKeys {0}; EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -755,7 +753,7 @@ TEST(ISAM2, marginalizeLeaves3) { isam.update(factors, values, FactorIndices(), constrainedKeys); - FastList leafKeys = list_of(0); + std::list leafKeys {0}; EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -780,7 +778,7 @@ TEST(ISAM2, marginalizeLeaves4) { isam.update(factors, values, FactorIndices(), constrainedKeys); - FastList leafKeys = list_of(1); + std::list leafKeys {1}; EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -791,7 +789,7 @@ TEST(ISAM2, marginalizeLeaves5) ISAM2 isam = createSlamlikeISAM2(); // Marginalize - FastList marginalizeKeys = list_of(0); + std::list marginalizeKeys {0}; EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys)); } diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index ad7cabb993..64c86c4c4c 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -27,8 +27,6 @@ #include #include -#include // for operator += -using namespace boost::assign; #include @@ -47,8 +45,7 @@ TEST ( Ordering, predecessorMap2Keys ) { p_map.insert(4,3); p_map.insert(5,1); - list expected; - expected += 4,5,3,2,1; + list expected{4, 5, 3, 2, 1}; list actual = predecessorMap2Keys(p_map); LONGS_EQUAL((long)expected.size(), (long)actual.size()); diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index c5b4e42ecf..8abd547951 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -29,10 +29,8 @@ #include -#include #include #include -using namespace boost::assign; #include @@ -170,8 +168,8 @@ TEST(SubgraphPreconditioner, system) { const double alpha = 0.5; Errors e1, e2; for (size_t i = 0; i < 13; i++) { - e1 += i < 9 ? Vector2(1, 1) : Vector2(0, 0); - e2 += i >= 9 ? Vector2(1, 1) : Vector2(0, 0); + e1.push_back(i < 9 ? Vector2(1, 1) : Vector2(0, 0)); + e2.push_back(i >= 9 ? Vector2(1, 1) : Vector2(0, 0)); } Vector ee1(13 * 2), ee2(13 * 2); ee1 << Vector::Ones(9 * 2), Vector::Zero(4 * 2); diff --git a/timing/timePose2.cpp b/timing/timePose2.cpp index 6af3e01637..bf04b9e937 100644 --- a/timing/timePose2.cpp +++ b/timing/timePose2.cpp @@ -97,7 +97,7 @@ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2, s, -c, dt2, 0.0, 0.0,-1.0; } - if (H2) *H2 = Matrix3::Identity(); + if (H2) *H2 = I_3x3; return Pose2(R,t); } From 276394d1d81402e58b8bc6f4260eae3071870e42 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 11:13:09 -0800 Subject: [PATCH 15/37] Moved ListOf helper and MakeKeys to common header. --- gtsam/symbolic/tests/symbolicExampleGraphs.h | 63 +++++++--- .../symbolic/tests/testSymbolicBayesTree.cpp | 110 ++++++------------ .../tests/testSymbolicEliminationTree.cpp | 79 +++++-------- 3 files changed, 108 insertions(+), 144 deletions(-) diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index fa90f54efe..47d40bb103 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2023, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file symbolicExampleGraphs.cpp + * @file symbolicExampleGraphs.h * @date sept 15, 2012 * @author Frank Dellaert * @author Michael Kaess @@ -30,6 +30,25 @@ namespace gtsam { namespace { + // A small helper class to replace Boost's `list_of` function. + template + class ListOf { + public: + ListOf(const T& c) { result.push_back(c); } + + ListOf& operator()(const T& c) { + result.push_back(c); + return *this; + } + + operator std::vector() { return result; } + + private: + std::vector result; + }; + + using MakeKeys = ListOf; + const SymbolicFactorGraph simpleTestGraph1 { boost::make_shared(0,1), boost::make_shared(0,2), @@ -100,23 +119,33 @@ namespace gtsam { boost::make_shared(_L_, _B_), boost::make_shared(_B_)}; + using sharedClique = SymbolicBayesTreeClique::shared_ptr; + using Children = ListOf; + + inline sharedClique LeafClique(const KeyVector& keys, + DenseIndex nrFrontals) { + return boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(keys, nrFrontals))); + } + + inline sharedClique NodeClique(const KeyVector& keys, DenseIndex nrFrontals, + const std::vector& children) { + sharedClique clique = LeafClique(keys, nrFrontals); + clique->children.assign(children.begin(), children.end()); + for (auto&& child : children) child->parent_ = clique; + return clique; + } + SymbolicBayesTree __asiaBayesTree() { SymbolicBayesTree result; - result.insertRoot(boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(KeyVector{_E_, _L_, _B_}, 3)))); - result.addClique(boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(KeyVector{_S_, _B_, _L_}, 1))), - result.roots().front()); - result.addClique(boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(KeyVector{_T_, _E_, _L_}, 1))), - result.roots().front()); - result.addClique(boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(KeyVector{_X_, _E_}, 1))), - result.roots().front()); + result.insertRoot(LeafClique(KeyVector{_E_, _L_, _B_}, 3)); + result.addClique(LeafClique(KeyVector{_S_, _B_, _L_}, 1), + result.roots().front()); + result.addClique(LeafClique(KeyVector{_T_, _E_, _L_}, 1), + result.roots().front()); + result.addClique(LeafClique(KeyVector{_X_, _E_}, 1), + result.roots().front()); return result; } diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index c5ea5a792e..0cbcb42648 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -34,48 +34,6 @@ using namespace gtsam::symbol_shorthand; static bool debug = false; -using sharedClique = SymbolicBayesTreeClique::shared_ptr; - -template -class ListOf { - public: - ListOf(const T& c) { result.push_back(c); } - - ListOf& operator()(const T& c) { - result.push_back(c); - return *this; - } - - operator std::vector() { return result; } - - private: - std::vector result; -}; - -using MakeKeys = ListOf; -using MakeCliques = ListOf; - -namespace { -/* ************************************************************************* */ -// Helper functions for below -sharedClique MakeClique(const KeyVector& keys, DenseIndex nrFrontals) { - return boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(keys, nrFrontals))); -} - -sharedClique MakeClique(const KeyVector& keys, DenseIndex nrFrontals, - const std::vector& children) { - sharedClique clique = boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(keys, nrFrontals))); - clique->children.assign(children.begin(), children.end()); - for (auto&& child : children) child->parent_ = clique; - return clique; -} - -} // namespace - /* ************************************************************************* */ TEST(SymbolicBayesTree, clear) { SymbolicBayesTree bayesTree = asiaBayesTree; @@ -107,14 +65,14 @@ TEST(SymbolicBayesTree, clique_structure) { graph.emplace_shared(X(5), L(3)); SymbolicBayesTree expected; - expected.insertRoot(MakeClique( + expected.insertRoot(NodeClique( MakeKeys(X(2))(X(3)), 2, - MakeCliques(MakeClique( + Children(NodeClique( MakeKeys(X(4))(X(3)), 1, - MakeCliques(MakeClique( + Children(NodeClique( MakeKeys(X(5))(L(2))(X(4)), 2, - MakeCliques(MakeClique(MakeKeys(L(3))(X(4))(X(5)), 1))))))( - MakeClique(MakeKeys(X(1))(L(1))(X(2)), 2)))); + Children(LeafClique(MakeKeys(L(3))(X(4))(X(5)), 1))))))( + LeafClique(MakeKeys(X(1))(L(1))(X(2)), 2)))); Ordering order{X(1), L(3), L(1), X(5), X(2), L(2), X(4), X(3)}; @@ -135,12 +93,12 @@ TEST(BayesTree, removePath) { _F_ = F(0); SymbolicBayesTree bayesTreeOrig; - bayesTreeOrig.insertRoot(MakeClique( + bayesTreeOrig.insertRoot(NodeClique( MakeKeys(_A_)(_B_), 2, - MakeCliques(MakeClique(MakeKeys(_C_)(_A_), 1, - MakeCliques(MakeClique(MakeKeys(_D_)(_C_), 1))))( - MakeClique(MakeKeys(_E_)(_B_), 1, - MakeCliques(MakeClique(MakeKeys(_F_)(_E_), 1)))))); + Children(NodeClique(MakeKeys(_C_)(_A_), 1, + Children(LeafClique(MakeKeys(_D_)(_C_), 1))))( + NodeClique(MakeKeys(_E_)(_B_), 1, + Children(LeafClique(MakeKeys(_F_)(_E_), 1)))))); SymbolicBayesTree bayesTree = bayesTreeOrig; @@ -519,8 +477,8 @@ TEST(SymbolicBayesTree, thinTree) { /* ************************************************************************* */ TEST(SymbolicBayesTree, forest_joint) { // Create forest - sharedClique root1 = MakeClique(MakeKeys(1), 1); - sharedClique root2 = MakeClique(MakeKeys(2), 1); + sharedClique root1 = LeafClique(MakeKeys(1), 1); + sharedClique root2 = LeafClique(MakeKeys(2), 1); SymbolicBayesTree bayesTree; bayesTree.insertRoot(root1); bayesTree.insertRoot(root2); @@ -605,24 +563,24 @@ TEST(SymbolicBayesTree, linear_smoother_shortcuts) { TEST(SymbolicBayesTree, complicatedMarginal) { // Create the conditionals to go in the BayesTree sharedClique cur; - sharedClique root = MakeClique(MakeKeys(11)(12), 2); + sharedClique root = LeafClique(MakeKeys(11)(12), 2); cur = root; - root->children.push_back(MakeClique(MakeKeys(9)(10)(11)(12), 2)); + root->children.push_back(LeafClique(MakeKeys(9)(10)(11)(12), 2)); root->children.back()->parent_ = root; - root->children.push_back(MakeClique(MakeKeys(7)(8)(11), 2)); + root->children.push_back(LeafClique(MakeKeys(7)(8)(11), 2)); root->children.back()->parent_ = root; cur = root->children.back(); - cur->children.push_back(MakeClique(MakeKeys(5)(6)(7)(8), 2)); + cur->children.push_back(LeafClique(MakeKeys(5)(6)(7)(8), 2)); cur->children.back()->parent_ = cur; cur = cur->children.back(); - cur->children.push_back(MakeClique(MakeKeys(3)(4)(6), 2)); + cur->children.push_back(LeafClique(MakeKeys(3)(4)(6), 2)); cur->children.back()->parent_ = cur; - cur->children.push_back(MakeClique(MakeKeys(1)(2)(5), 2)); + cur->children.push_back(LeafClique(MakeKeys(1)(2)(5), 2)); cur->children.back()->parent_ = cur; // Create Bayes Tree @@ -707,12 +665,12 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // | | | - P( 0 | 1 5) SymbolicBayesTree expected; expected.insertRoot( - MakeClique(MakeKeys(4)(2)(3), 3, - MakeCliques(MakeClique( + NodeClique(MakeKeys(4)(2)(3), 3, + Children(NodeClique( MakeKeys(1)(2)(4), 1, - MakeCliques(MakeClique( + Children(NodeClique( MakeKeys(5)(1)(4), 1, - MakeCliques(MakeClique(MakeKeys(0)(1)(5), 1)))))))); + Children(LeafClique(MakeKeys(0)(1)(5), 1)))))))); SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); @@ -737,23 +695,23 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // | - P( 2 | 1 3) SymbolicBayesTree expected; #if defined(__APPLE__) - expected.insertRoot(MakeClique( + expected.insertRoot(NodeClique( MakeKeys(1)(0)(3), 3, - MakeCliques(MakeClique(MakeKeys(4)(0)(3), 1, - MakeCliques(MakeClique(MakeKeys(5)(0)(4), 1))))( - MakeClique(MakeKeys(2)(1)(3), 1)))); + Children(NodeClique(MakeKeys(4)(0)(3), 1, + Children(LeafClique(MakeKeys(5)(0)(4), 1))))( + LeafClique(MakeKeys(2)(1)(3), 1)))); #elif defined(_WIN32) - expected.insertRoot(MakeClique( + expected.insertRoot(NodeClique( MakeKeys(3)(5)(2), 3, - MakeCliques(MakeClique(MakeKeys(4)(3)(5), 1, - MakeCliques(MakeClique(MakeKeys(0)(2)(5), 1))))( - MakeClique(MakeKeys(1)(0)(2), 1)))); + Children(NodeClique(MakeKeys(4)(3)(5), 1, + Children(LeafClique(MakeKeys(0)(2)(5), 1))))( + LeafClique(MakeKeys(1)(0)(2), 1)))); #else - expected.insertRoot(MakeClique( + expected.insertRoot(NodeClique( MakeKeys(2)(4)(1), 3, - MakeCliques(MakeClique(MakeKeys(0)(1)(4), 1, - MakeCliques(MakeClique(MakeKeys(5)(0)(4), 1))))( - MakeClique(MakeKeys(3)(2)(4), 1)))); + Children(NodeClique(MakeKeys(0)(1)(4), 1, + Children(LeafClique(MakeKeys(5)(0)(4), 1))))( + LeafClique(MakeKeys(3)(2)(4), 1)))); #endif SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 9234d8a514..e1c0d9cf88 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -73,43 +73,22 @@ class EliminationTreeTester { } }; -template -static sharedNode MakeNode(Key key, const FACTORS& factors) { - sharedNode node = boost::make_shared(); +static sharedNode Leaf(Key key, const SymbolicFactorGraph& factors) { + sharedNode node(new SymbolicEliminationTree::Node()); node->key = key; - SymbolicFactorGraph factorsAsGraph = factors; - node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); + node->factors = factors; return node; } -template -static sharedNode MakeNode(Key key, const FACTORS& factors, - const std::vector& children) { - sharedNode node = boost::make_shared(); - node->key = key; - SymbolicFactorGraph factorsAsGraph = factors; - node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); +static sharedNode Node(Key key, const SymbolicFactorGraph& factors, + const std::vector& children) { + sharedNode node = Leaf(key, factors); node->children.assign(children.begin(), children.end()); return node; } -template -class ListOf { - public: - ListOf(Class&& c) { result.push_back(c); } - - ListOf& operator()(Class&& c) { - result.push_back(c); - return *this; - } - - operator std::vector() { return result; } - - private: - std::vector result; -}; - -using Nodes = ListOf; +// Use list_of replacement defined in symbolicExampleGraphs.h +using ChildNodes = ListOf; /* ************************************************************************* */ TEST(EliminationTree, Create) { @@ -143,28 +122,26 @@ TEST(EliminationTree, Create2) { graph += SymbolicFactor(X(4), L(3)); graph += SymbolicFactor(X(5), L(3)); - SymbolicEliminationTree expected = - EliminationTreeTester::MakeTree(Nodes(MakeNode( - X(3), SymbolicFactorGraph(), - Nodes(MakeNode( - X(2), SymbolicFactorGraph(SymbolicFactor(X(2), X(3))), - Nodes(MakeNode( - L(1), SymbolicFactorGraph(SymbolicFactor(X(2), L(1))), - Nodes(MakeNode( - X(1), SymbolicFactorGraph(SymbolicFactor(X(1), L(1)))( - SymbolicFactor(X(1), X(2)))))))))( - MakeNode( - X(4), SymbolicFactorGraph(SymbolicFactor(X(3), X(4))), - Nodes(MakeNode( - L(2), SymbolicFactorGraph(SymbolicFactor(X(4), L(2))), - Nodes(MakeNode( - X(5), - SymbolicFactorGraph(SymbolicFactor(X(4), X(5)))( - SymbolicFactor(L(2), X(5))), - Nodes(MakeNode( - L(3), - SymbolicFactorGraph(SymbolicFactor(X(4), L(3)))( - SymbolicFactor(X(5), L(3)))))))))))))); + SymbolicEliminationTree expected = EliminationTreeTester::MakeTree(ChildNodes( + Node(X(3), SymbolicFactorGraph(), + ChildNodes(Node( + X(2), SymbolicFactorGraph(SymbolicFactor(X(2), X(3))), + ChildNodes(Node( + L(1), SymbolicFactorGraph(SymbolicFactor(X(2), L(1))), + ChildNodes(Leaf( + X(1), SymbolicFactorGraph(SymbolicFactor(X(1), L(1)))( + SymbolicFactor(X(1), X(2)))))))))( + Node(X(4), SymbolicFactorGraph(SymbolicFactor(X(3), X(4))), + ChildNodes(Node( + L(2), SymbolicFactorGraph(SymbolicFactor(X(4), L(2))), + ChildNodes(Node( + X(5), + SymbolicFactorGraph(SymbolicFactor(X(4), X(5)))( + SymbolicFactor(L(2), X(5))), + ChildNodes(Leaf( + L(3), + SymbolicFactorGraph(SymbolicFactor(X(4), L(3)))( + SymbolicFactor(X(5), L(3)))))))))))))); const Ordering order{X(1), L(3), L(1), X(5), X(2), L(2), X(4), X(3)}; SymbolicEliminationTree actual(graph, order); From edec6f3355e515a79da015ca0fb351312d475e4c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 11:40:14 -0800 Subject: [PATCH 16/37] Renamed MakeKeys to Keys and cleaned up tests. --- gtsam/symbolic/tests/symbolicExampleGraphs.h | 2 +- .../symbolic/tests/testSymbolicBayesTree.cpp | 132 +++++++++--------- 2 files changed, 69 insertions(+), 65 deletions(-) diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index 47d40bb103..99470f1e6c 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -47,7 +47,7 @@ namespace gtsam { std::vector result; }; - using MakeKeys = ListOf; + using Keys = ListOf; const SymbolicFactorGraph simpleTestGraph1 { boost::make_shared(0,1), diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 0cbcb42648..81345858ed 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -65,14 +65,14 @@ TEST(SymbolicBayesTree, clique_structure) { graph.emplace_shared(X(5), L(3)); SymbolicBayesTree expected; - expected.insertRoot(NodeClique( - MakeKeys(X(2))(X(3)), 2, - Children(NodeClique( - MakeKeys(X(4))(X(3)), 1, - Children(NodeClique( - MakeKeys(X(5))(L(2))(X(4)), 2, - Children(LeafClique(MakeKeys(L(3))(X(4))(X(5)), 1))))))( - LeafClique(MakeKeys(X(1))(L(1))(X(2)), 2)))); + expected.insertRoot( + NodeClique(Keys(X(2))(X(3)), 2, + Children(NodeClique( + Keys(X(4))(X(3)), 1, + Children(NodeClique( + Keys(X(5))(L(2))(X(4)), 2, + Children(LeafClique(Keys(L(3))(X(4))(X(5)), 1))))))( + LeafClique(Keys(X(1))(L(1))(X(2)), 2)))); Ordering order{X(1), L(3), L(1), X(5), X(2), L(2), X(4), X(3)}; @@ -93,12 +93,9 @@ TEST(BayesTree, removePath) { _F_ = F(0); SymbolicBayesTree bayesTreeOrig; - bayesTreeOrig.insertRoot(NodeClique( - MakeKeys(_A_)(_B_), 2, - Children(NodeClique(MakeKeys(_C_)(_A_), 1, - Children(LeafClique(MakeKeys(_D_)(_C_), 1))))( - NodeClique(MakeKeys(_E_)(_B_), 1, - Children(LeafClique(MakeKeys(_F_)(_E_), 1)))))); + auto left = NodeClique(Keys(_C_)(_A_), 1, {LeafClique(Keys(_D_)(_C_), 1)}); + auto right = NodeClique(Keys(_E_)(_B_), 1, {LeafClique(Keys(_F_)(_E_), 1)}); + bayesTreeOrig.insertRoot(NodeClique(Keys(_A_)(_B_), 2, {left, right})); SymbolicBayesTree bayesTree = bayesTreeOrig; @@ -192,10 +189,13 @@ void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, /* ************************************************************************* */ TEST(BayesTree, shortcutCheck) { const Key _A_ = 6, _B_ = 5, _C_ = 4, _D_ = 3, _E_ = 2, _F_ = 1, _G_ = 0; - auto chain = - SymbolicFactorGraph(SymbolicFactor(_A_))(SymbolicFactor(_B_, _A_))( - SymbolicFactor(_C_, _A_))(SymbolicFactor(_D_, _C_))(SymbolicFactor( - _E_, _B_))(SymbolicFactor(_F_, _E_))(SymbolicFactor(_G_, _F_)); + auto chain = SymbolicFactorGraph(SymbolicFactor(_A_)) // + (SymbolicFactor(_B_, _A_)) // + (SymbolicFactor(_C_, _A_)) // + (SymbolicFactor(_D_, _C_)) // + (SymbolicFactor(_E_, _B_)) // + (SymbolicFactor(_F_, _E_)) // + (SymbolicFactor(_G_, _F_)); Ordering ordering{_G_, _F_, _E_, _D_, _C_, _B_, _A_}; SymbolicBayesTree bayesTree = *chain.eliminateMultifrontal(ordering); @@ -243,14 +243,12 @@ TEST(BayesTree, removeTop) { // Remove the contaminated part of the Bayes tree SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(MakeKeys(_B_)(_S_), &bn, &orphans); + bayesTree.removeTop(Keys(_B_)(_S_), &bn, &orphans); // Check expected outcome SymbolicBayesNet expected; - expected += - SymbolicConditional::FromKeys(MakeKeys(_E_)(_L_)(_B_), 3); - expected += - SymbolicConditional::FromKeys(MakeKeys(_S_)(_B_)(_L_), 1); + expected += SymbolicConditional::FromKeys(Keys(_E_)(_L_)(_B_), 3); + expected += SymbolicConditional::FromKeys(Keys(_S_)(_B_)(_L_), 1); CHECK(assert_equal(expected, bn)); SymbolicBayesTree::Cliques expectedOrphans = @@ -262,7 +260,7 @@ TEST(BayesTree, removeTop) { // boost::shared_ptr newFactor2(new IndexFactor(_B_)); SymbolicBayesNet bn2; SymbolicBayesTree::Cliques orphans2; - bayesTree.removeTop(MakeKeys(_B_), &bn2, &orphans2); + bayesTree.removeTop(Keys(_B_), &bn2, &orphans2); SymbolicFactorGraph factors2(bn2); SymbolicFactorGraph expected2; CHECK(assert_equal(expected2, factors2)); @@ -283,12 +281,12 @@ TEST(BayesTree, removeTop2) { // Remove the contaminated part of the Bayes tree SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(MakeKeys(_T_), &bn, &orphans); + bayesTree.removeTop(Keys(_T_), &bn, &orphans); // Check expected outcome auto expected = SymbolicBayesNet( - SymbolicConditional::FromKeys(MakeKeys(_E_)(_L_)(_B_), 3))( - SymbolicConditional::FromKeys(MakeKeys(_T_)(_E_)(_L_), 1)); + SymbolicConditional::FromKeys(Keys(_E_)(_L_)(_B_), 3))( + SymbolicConditional::FromKeys(Keys(_T_)(_E_)(_L_), 1)); CHECK(assert_equal(expected, bn)); SymbolicBayesTree::Cliques expectedOrphans = @@ -307,10 +305,10 @@ TEST(BayesTree, removeTop3) { // remove all SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(MakeKeys(L(5))(X(4))(X(2))(X(3)), &bn, &orphans); + bayesTree.removeTop(Keys(L(5))(X(4))(X(2))(X(3)), &bn, &orphans); auto expectedBn = SymbolicBayesNet( - SymbolicConditional::FromKeys(MakeKeys(X(4))(L(5)), 2))( + SymbolicConditional::FromKeys(Keys(X(4))(L(5)), 2))( SymbolicConditional(X(2), X(4)))(SymbolicConditional(X(3), X(2))); EXPECT(assert_equal(expectedBn, bn)); EXPECT(orphans.empty()); @@ -326,10 +324,10 @@ TEST(BayesTree, removeTop4) { // remove all SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(MakeKeys(X(2))(L(5))(X(4))(X(3)), &bn, &orphans); + bayesTree.removeTop(Keys(X(2))(L(5))(X(4))(X(3)), &bn, &orphans); auto expectedBn = SymbolicBayesNet( - SymbolicConditional::FromKeys(MakeKeys(X(4))(L(5)), 2))( + SymbolicConditional::FromKeys(Keys(X(4))(L(5)), 2))( SymbolicConditional(X(2), X(4)))(SymbolicConditional(X(3), X(2))); EXPECT(assert_equal(expectedBn, bn)); EXPECT(orphans.empty()); @@ -346,7 +344,7 @@ TEST(BayesTree, removeTop5) { // Remove nonexistant SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(MakeKeys(X(10)), &bn, &orphans); + bayesTree.removeTop(Keys(X(10)), &bn, &orphans); SymbolicBayesNet expectedBn; EXPECT(assert_equal(expectedBn, bn)); @@ -355,7 +353,7 @@ TEST(BayesTree, removeTop5) { /* ************************************************************************* */ TEST(SymbolicBayesTree, thinTree) { - // create a thin-tree Bayesnet, a la Jean-Guillaume + // create a thin-tree Bayes net, a la Jean-Guillaume SymbolicBayesNet bayesNet; bayesNet.emplace_shared(14); @@ -477,8 +475,8 @@ TEST(SymbolicBayesTree, thinTree) { /* ************************************************************************* */ TEST(SymbolicBayesTree, forest_joint) { // Create forest - sharedClique root1 = LeafClique(MakeKeys(1), 1); - sharedClique root2 = LeafClique(MakeKeys(2), 1); + sharedClique root1 = LeafClique(Keys(1), 1); + sharedClique root2 = LeafClique(Keys(2), 1); SymbolicBayesTree bayesTree; bayesTree.insertRoot(root1); bayesTree.insertRoot(root2); @@ -563,24 +561,24 @@ TEST(SymbolicBayesTree, linear_smoother_shortcuts) { TEST(SymbolicBayesTree, complicatedMarginal) { // Create the conditionals to go in the BayesTree sharedClique cur; - sharedClique root = LeafClique(MakeKeys(11)(12), 2); + sharedClique root = LeafClique(Keys(11)(12), 2); cur = root; - root->children.push_back(LeafClique(MakeKeys(9)(10)(11)(12), 2)); + root->children.push_back(LeafClique(Keys(9)(10)(11)(12), 2)); root->children.back()->parent_ = root; - root->children.push_back(LeafClique(MakeKeys(7)(8)(11), 2)); + root->children.push_back(LeafClique(Keys(7)(8)(11), 2)); root->children.back()->parent_ = root; cur = root->children.back(); - cur->children.push_back(LeafClique(MakeKeys(5)(6)(7)(8), 2)); + cur->children.push_back(LeafClique(Keys(5)(6)(7)(8), 2)); cur->children.back()->parent_ = cur; cur = cur->children.back(); - cur->children.push_back(LeafClique(MakeKeys(3)(4)(6), 2)); + cur->children.push_back(LeafClique(Keys(3)(4)(6), 2)); cur->children.back()->parent_ = cur; - cur->children.push_back(LeafClique(MakeKeys(1)(2)(5), 2)); + cur->children.push_back(LeafClique(Keys(1)(2)(5), 2)); cur->children.back()->parent_ = cur; // Create Bayes Tree @@ -664,13 +662,16 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // | | - P( 5 | 1 4) // | | | - P( 0 | 1 5) SymbolicBayesTree expected; - expected.insertRoot( - NodeClique(MakeKeys(4)(2)(3), 3, - Children(NodeClique( - MakeKeys(1)(2)(4), 1, - Children(NodeClique( - MakeKeys(5)(1)(4), 1, - Children(LeafClique(MakeKeys(0)(1)(5), 1)))))))); + expected.insertRoot( // + NodeClique( + Keys(4)(2)(3), 3, + Children( // + NodeClique( + Keys(1)(2)(4), 1, + Children( // + NodeClique(Keys(5)(1)(4), 1, + Children( // + LeafClique(Keys(0)(1)(5), 1)))))))); SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); @@ -680,7 +681,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // METIS { Ordering ordering = Ordering::Create(Ordering::METIS, sfg); -// Linux and Mac split differently when using mettis +// Linux and Mac split differently when using Metis #if defined(__APPLE__) EXPECT(assert_equal(Ordering{5, 4, 2, 1, 0, 3}, ordering)); #elif defined(_WIN32) @@ -695,23 +696,26 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // | - P( 2 | 1 3) SymbolicBayesTree expected; #if defined(__APPLE__) - expected.insertRoot(NodeClique( - MakeKeys(1)(0)(3), 3, - Children(NodeClique(MakeKeys(4)(0)(3), 1, - Children(LeafClique(MakeKeys(5)(0)(4), 1))))( - LeafClique(MakeKeys(2)(1)(3), 1)))); + expected.insertRoot( + NodeClique(Keys(1)(0)(3), 3, + Children( // + NodeClique(Keys(4)(0)(3), 1, // + {LeafClique(Keys(5)(0)(4), 1)}))( + LeafClique(Keys(2)(1)(3), 1)))); #elif defined(_WIN32) - expected.insertRoot(NodeClique( - MakeKeys(3)(5)(2), 3, - Children(NodeClique(MakeKeys(4)(3)(5), 1, - Children(LeafClique(MakeKeys(0)(2)(5), 1))))( - LeafClique(MakeKeys(1)(0)(2), 1)))); + expected.insertRoot( + NodeClique(Keys(3)(5)(2), 3, + Children( // + NodeClique(Keys(4)(3)(5), 1, // + {LeafClique(Keys(0)(2)(5), 1)}))( + LeafClique(Keys(1)(0)(2), 1)))); #else - expected.insertRoot(NodeClique( - MakeKeys(2)(4)(1), 3, - Children(NodeClique(MakeKeys(0)(1)(4), 1, - Children(LeafClique(MakeKeys(5)(0)(4), 1))))( - LeafClique(MakeKeys(3)(2)(4), 1)))); + expected.insertRoot( + NodeClique(Keys(2)(4)(1), 3, + Children( // + NodeClique(Keys(0)(1)(4), 1, // + {LeafClique(Keys(5)(0)(4), 1)}))( + LeafClique(Keys(3)(2)(4), 1)))); #endif SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); From 5164d6fc55579841c43b12bfff68914cfd4be231 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 11:57:40 -0800 Subject: [PATCH 17/37] Cleaned up etree tests --- .../tests/testSymbolicEliminationTree.cpp | 81 +++++++++++-------- 1 file changed, 49 insertions(+), 32 deletions(-) diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index e1c0d9cf88..4998cb9f1d 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -76,7 +76,7 @@ class EliminationTreeTester { static sharedNode Leaf(Key key, const SymbolicFactorGraph& factors) { sharedNode node(new SymbolicEliminationTree::Node()); node->key = key; - node->factors = factors; + node->factors.assign(factors.begin(), factors.end()); return node; } @@ -111,37 +111,54 @@ TEST(EliminationTree, Create2) { // \ | // l3 SymbolicFactorGraph graph; - graph += SymbolicFactor(X(1), L(1)); - graph += SymbolicFactor(X(1), X(2)); - graph += SymbolicFactor(X(2), L(1)); - graph += SymbolicFactor(X(2), X(3)); - graph += SymbolicFactor(X(3), X(4)); - graph += SymbolicFactor(X(4), L(2)); - graph += SymbolicFactor(X(4), X(5)); - graph += SymbolicFactor(L(2), X(5)); - graph += SymbolicFactor(X(4), L(3)); - graph += SymbolicFactor(X(5), L(3)); - - SymbolicEliminationTree expected = EliminationTreeTester::MakeTree(ChildNodes( - Node(X(3), SymbolicFactorGraph(), - ChildNodes(Node( - X(2), SymbolicFactorGraph(SymbolicFactor(X(2), X(3))), - ChildNodes(Node( - L(1), SymbolicFactorGraph(SymbolicFactor(X(2), L(1))), - ChildNodes(Leaf( - X(1), SymbolicFactorGraph(SymbolicFactor(X(1), L(1)))( - SymbolicFactor(X(1), X(2)))))))))( - Node(X(4), SymbolicFactorGraph(SymbolicFactor(X(3), X(4))), - ChildNodes(Node( - L(2), SymbolicFactorGraph(SymbolicFactor(X(4), L(2))), - ChildNodes(Node( - X(5), - SymbolicFactorGraph(SymbolicFactor(X(4), X(5)))( - SymbolicFactor(L(2), X(5))), - ChildNodes(Leaf( - L(3), - SymbolicFactorGraph(SymbolicFactor(X(4), L(3)))( - SymbolicFactor(X(5), L(3)))))))))))))); + auto binary = [](Key j1, Key j2) -> SymbolicFactor { + return SymbolicFactor(j1, j2); + }; + graph += binary(X(1), L(1)); + graph += binary(X(1), X(2)); + graph += binary(X(2), L(1)); + graph += binary(X(2), X(3)); + graph += binary(X(3), X(4)); + graph += binary(X(4), L(2)); + graph += binary(X(4), X(5)); + graph += binary(L(2), X(5)); + graph += binary(X(4), L(3)); + graph += binary(X(5), L(3)); + + SymbolicEliminationTree expected = EliminationTreeTester::MakeTree( // + ChildNodes( // + Node(X(3), SymbolicFactorGraph(), + ChildNodes( // + Node(X(2), SymbolicFactorGraph(binary(X(2), X(3))), + ChildNodes( // + Node(L(1), SymbolicFactorGraph(binary(X(2), L(1))), + ChildNodes( // + Leaf(X(1), SymbolicFactorGraph( + binary(X(1), L(1)))( + binary(X(1), X(2)))))))))( + Node(X(4), SymbolicFactorGraph(binary(X(3), X(4))), + ChildNodes( // + Node(L(2), SymbolicFactorGraph(binary(X(4), L(2))), + ChildNodes( // + Node(X(5), + SymbolicFactorGraph(binary( + X(4), X(5)))(binary(L(2), X(5))), + ChildNodes( // + Leaf(L(3), + SymbolicFactorGraph( + binary(X(4), L(3)))( + binary(X(5), L(3))) // + ) // + ) // + ) // + ) // + ) // + ) // + ) // + ) // + ) // + ) // + ); const Ordering order{X(1), L(3), L(1), X(5), X(2), L(2), X(4), X(3)}; SymbolicEliminationTree actual(graph, order); From ddf86d8f2515cf68facfa7de7623a5a17f4c9d08 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 11:57:54 -0800 Subject: [PATCH 18/37] Fix compilation issue on Linux --- gtsam/base/tests/testDSFMap.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/tests/testDSFMap.cpp b/gtsam/base/tests/testDSFMap.cpp index 2e4b4f146b..3fd556832e 100644 --- a/gtsam/base/tests/testDSFMap.cpp +++ b/gtsam/base/tests/testDSFMap.cpp @@ -109,7 +109,7 @@ TEST(DSFMap, mergePairwiseMatches2) { /* ************************************************************************* */ TEST(DSFMap, sets){ // Create some "matches" - typedef const std::pair Match; + using Match = std::pair; const std::list matches{{1, 2}, {2, 3}, {4, 5}, {4, 6}}; // Merge matches From 638726fca5c1b7ad707970fb3795f06d5fea0d0c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 13:00:27 -0800 Subject: [PATCH 19/37] An attempt to fix Windows issue. --- gtsam/base/FastSet.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index e93f056f67..1fceebad5b 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -58,6 +58,8 @@ class FastSet: public std::set, using Base::Base; // Inherit the set constructors + FastSet() = default; ///< Default constructor + /** Constructor from a iterable container, passes through to base class */ template explicit FastSet(const INPUTCONTAINER& container) : From 73754f271a7f636f97f4b1a05b7d519358177aa3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 13:50:43 -0800 Subject: [PATCH 20/37] Fix typo --- gtsam/symbolic/tests/symbolicExampleGraphs.h | 2 -- gtsam/symbolic/tests/testSymbolicBayesTree.cpp | 4 +++- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index 99470f1e6c..2a9e71083e 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -47,8 +47,6 @@ namespace gtsam { std::vector result; }; - using Keys = ListOf; - const SymbolicFactorGraph simpleTestGraph1 { boost::make_shared(0,1), boost::make_shared(0,2), diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 81345858ed..1d92404a82 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -34,6 +34,8 @@ using namespace gtsam::symbol_shorthand; static bool debug = false; +using Keys = ListOf; // Create Keys a la list_of + /* ************************************************************************* */ TEST(SymbolicBayesTree, clear) { SymbolicBayesTree bayesTree = asiaBayesTree; @@ -687,7 +689,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { #elif defined(_WIN32) EXPECT(assert_equal(Ordering{4, 3, 1, 0, 5, 2}, ordering)); #else - EXPECT(assert_equal(Ordering{3, 2, 0, 5, 4, 1}, ordering)); + EXPECT(assert_equal(Ordering{3, 2, 5, 0, 4, 1}, ordering)); #endif // - P( 1 0 3) From 15802e58f98b1b7474d621fd19a80f36e711da79 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 21:09:31 -0800 Subject: [PATCH 21/37] Address review comments --- gtsam/base/tests/testVerticalBlockMatrix.cpp | 3 +- gtsam/geometry/CameraSet.h | 289 +++++++++--------- gtsam/inference/tests/testOrdering.cpp | 4 +- .../linear/tests/testGaussianFactorGraph.cpp | 2 +- .../tests/testSymbolicEliminationTree.cpp | 3 +- 5 files changed, 158 insertions(+), 143 deletions(-) diff --git a/gtsam/base/tests/testVerticalBlockMatrix.cpp b/gtsam/base/tests/testVerticalBlockMatrix.cpp index e6630427ae..758d776c85 100644 --- a/gtsam/base/tests/testVerticalBlockMatrix.cpp +++ b/gtsam/base/tests/testVerticalBlockMatrix.cpp @@ -24,8 +24,7 @@ using namespace gtsam; -const std::list L{3, 2, 1}; -const std::vector dimensions(L.begin(), L.end()); +const std::vector dimensions{3, 2, 1}; //***************************************************************************** TEST(VerticalBlockMatrix, Constructor1) { diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 0fbb50f021..a814fea87e 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -18,12 +18,13 @@ #pragma once -#include -#include // for Cheirality exception -#include -#include #include +#include +#include +#include // for Cheirality exception +#include #include + #include namespace gtsam { @@ -31,70 +32,67 @@ namespace gtsam { /** * @brief A set of cameras, all with their own calibration */ -template -class CameraSet : public std::vector > { - -protected: - using Base = std::vector>; - - /** - * 2D measurement and noise model for each of the m views - * The order is kept the same as the keys that we use to create the factor. - */ - typedef typename CAMERA::Measurement Z; - typedef typename CAMERA::MeasurementVector ZVector; - - static const int D = traits::dimension; ///< Camera dimension - static const int ZDim = traits::dimension; ///< Measurement dimension - - /// Make a vector of re-projection errors - static Vector ErrorVector(const ZVector& predicted, const ZVector& measured) { - // Check size - size_t m = predicted.size(); - if (measured.size() != m) - throw std::runtime_error("CameraSet::errors: size mismatch"); - - // Project and fill error vector - Vector b(ZDim * m); - for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - Vector bi = traits::Local(measured[i], predicted[i]); - if (ZDim == 3 && std::isnan(bi(1))) { // if it is a stereo point and the - // right pixel is missing (nan) - bi(1) = 0; - } - b.segment(row) = bi; - } - return b; +template +class CameraSet : public std::vector> { + protected: + using Base = std::vector>; + + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + typedef typename CAMERA::Measurement Z; + typedef typename CAMERA::MeasurementVector ZVector; + + static const int D = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + + /// Make a vector of re-projection errors + static Vector ErrorVector(const ZVector& predicted, const ZVector& measured) { + // Check size + size_t m = predicted.size(); + if (measured.size() != m) + throw std::runtime_error("CameraSet::errors: size mismatch"); + + // Project and fill error vector + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + Vector bi = traits::Local(measured[i], predicted[i]); + if (ZDim == 3 && std::isnan(bi(1))) { // if it is a stereo point and the + // right pixel is missing (nan) + bi(1) = 0; + } + b.segment(row) = bi; + } + return b; } -public: - using Base::Base; // Inherit the vector constructors + public: + using Base::Base; // Inherit the vector constructors - /// Destructor - virtual ~CameraSet() = default; + /// Destructor + virtual ~CameraSet() = default; - /// Definitions for blocks of F - using MatrixZD = Eigen::Matrix; - using FBlocks = std::vector>; + /// Definitions for blocks of F + using MatrixZD = Eigen::Matrix; + using FBlocks = std::vector>; - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter useful for printing Symbols - */ - virtual void print(const std::string& s = "") const { - std::cout << s << "CameraSet, cameras = \n"; - for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s); + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + virtual void print(const std::string& s = "") const { + std::cout << s << "CameraSet, cameras = \n"; + for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s); } /// equals bool equals(const CameraSet& p, double tol = 1e-9) const { - if (this->size() != p.size()) - return false; + if (this->size() != p.size()) return false; bool camerasAreEqual = true; for (size_t i = 0; i < this->size(); i++) { - if (this->at(i).equals(p.at(i), tol) == false) - camerasAreEqual = false; + if (this->at(i).equals(p.at(i), tol) == false) camerasAreEqual = false; break; } return camerasAreEqual; @@ -106,11 +104,10 @@ class CameraSet : public std::vector > * matrix this function returns the diagonal blocks. * throws CheiralityException */ - template - ZVector project2(const POINT& point, // - boost::optional Fs = boost::none, // - boost::optional E = boost::none) const { - + template + ZVector project2(const POINT& point, // + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { static const int N = FixedDimension::value; // Allocate result @@ -135,19 +132,19 @@ class CameraSet : public std::vector > } /// Calculate vector [project2(point)-z] of re-projection errors - template + template Vector reprojectionError(const POINT& point, const ZVector& measured, - boost::optional Fs = boost::none, // - boost::optional E = boost::none) const { + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { return ErrorVector(project2(point, Fs, E), measured); } /** - * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix - * G = F' * F - F' * E * P * E' * F - * g = F' * (b - E * P * E' * b) - * Fixed size version - */ + * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix + * G = F' * F - F' * E * P * E' * F + * g = F' * (b - E * P * E' * b) + * Fixed size version + */ template // N = 2 or 3 (point dimension), ND is the camera dimension static SymmetricBlockMatrix SchurComplement( @@ -158,38 +155,47 @@ class CameraSet : public std::vector > // a single point is observed in m cameras size_t m = Fs.size(); - // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) + // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column + // with info vector) size_t M1 = ND * m + 1; - std::vector dims(m + 1); // this also includes the b term + std::vector dims(m + 1); // this also includes the b term std::fill(dims.begin(), dims.end() - 1, ND); dims.back() = 1; SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); // Blockwise Schur complement - for (size_t i = 0; i < m; i++) { // for each camera + for (size_t i = 0; i < m; i++) { // for each camera const Eigen::Matrix& Fi = Fs[i]; const auto FiT = Fi.transpose(); - const Eigen::Matrix Ei_P = // + const Eigen::Matrix Ei_P = // E.block(ZDim * i, 0, ZDim, N) * P; // D = (Dx2) * ZDim - augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(ZDim * i) // F' * b - - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + augmentedHessian.setOffDiagonalBlock( + i, m, + FiT * b.segment(ZDim * i) // F' * b + - + FiT * + (Ei_P * + (E.transpose() * + b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian.setDiagonalBlock(i, FiT - * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); + augmentedHessian.setDiagonalBlock( + i, + FiT * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); // upper triangular part of the hessian - for (size_t j = i + 1; j < m; j++) { // for each camera + for (size_t j = i + 1; j < m; j++) { // for each camera const Eigen::Matrix& Fj = Fs[j]; // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian.setOffDiagonalBlock(i, j, -FiT - * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); + augmentedHessian.setOffDiagonalBlock( + i, j, + -FiT * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); } - } // end of for over cameras + } // end of for over cameras augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); return augmentedHessian; @@ -297,20 +303,21 @@ class CameraSet : public std::vector > * g = F' * (b - E * P * E' * b) * Fixed size version */ - template // N = 2 or 3 - static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - return SchurComplement(Fs, E, P, b); + template // N = 2 or 3 + static SymmetricBlockMatrix SchurComplement( + const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, + const Vector& b) { + return SchurComplement(Fs, E, P, b); } /// Computes Point Covariance P, with lambda parameter - template // N = 2 or 3 (point dimension) + template // N = 2 or 3 (point dimension) static void ComputePointCovariance(Eigen::Matrix& P, - const Matrix& E, double lambda, bool diagonalDamping = false) { - + const Matrix& E, double lambda, + bool diagonalDamping = false) { Matrix EtE = E.transpose() * E; - if (diagonalDamping) { // diagonal of the hessian + if (diagonalDamping) { // diagonal of the hessian EtE.diagonal() += lambda * EtE.diagonal(); } else { DenseIndex n = E.cols(); @@ -322,7 +329,7 @@ class CameraSet : public std::vector > /// Computes Point Covariance P, with lambda parameter, dynamic version static Matrix PointCov(const Matrix& E, const double lambda = 0.0, - bool diagonalDamping = false) { + bool diagonalDamping = false) { if (E.cols() == 2) { Matrix2 P2; ComputePointCovariance<2>(P2, E, lambda, diagonalDamping); @@ -339,8 +346,9 @@ class CameraSet : public std::vector > * Dynamic version */ static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks, - const Matrix& E, const Vector& b, const double lambda = 0.0, - bool diagonalDamping = false) { + const Matrix& E, const Vector& b, + const double lambda = 0.0, + bool diagonalDamping = false) { if (E.cols() == 2) { Matrix2 P; ComputePointCovariance<2>(P, E, lambda, diagonalDamping); @@ -353,17 +361,17 @@ class CameraSet : public std::vector > } /** - * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, - * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. + * Applies Schur complement (exploiting block structure) to get a smart factor + * on cameras, and adds the contribution of the smart factor to a + * pre-allocated augmented Hessian. */ - template // N = 2 or 3 (point dimension) - static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, - const Eigen::Matrix& P, const Vector& b, - const KeyVector& allKeys, const KeyVector& keys, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) { - - assert(keys.size()==Fs.size()); - assert(keys.size()<=allKeys.size()); + template // N = 2 or 3 (point dimension) + static void UpdateSchurComplement( + const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, + const Vector& b, const KeyVector& allKeys, const KeyVector& keys, + /*output ->*/ SymmetricBlockMatrix& augmentedHessian) { + assert(keys.size() == Fs.size()); + assert(keys.size() <= allKeys.size()); FastMap KeySlotMap; for (size_t slot = 0; slot < allKeys.size(); slot++) @@ -374,39 +382,49 @@ class CameraSet : public std::vector > // g = F' * (b - E * P * E' * b) // a single point is observed in m cameras - size_t m = Fs.size(); // cameras observing current point - size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group - assert(allKeys.size()==M); + size_t m = Fs.size(); // cameras observing current point + size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group + assert(allKeys.size() == M); // Blockwise Schur complement - for (size_t i = 0; i < m; i++) { // for each camera in the current factor + for (size_t i = 0; i < m; i++) { // for each camera in the current factor const MatrixZD& Fi = Fs[i]; const auto FiT = Fi.transpose(); - const Eigen::Matrix Ei_P = E.template block( - ZDim * i, 0) * P; + const Eigen::Matrix Ei_P = + E.template block(ZDim * i, 0) * P; // D = (DxZDim) * (ZDim) // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) - // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) - // Key cameraKey_i = this->keys_[i]; + // we should map those to a slot in the local (grouped) hessian + // (0,1,2,3,4) Key cameraKey_i = this->keys_[i]; DenseIndex aug_i = KeySlotMap.at(keys[i]); // information vector - store previous vector // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal(); // add contribution of current factor - augmentedHessian.updateOffDiagonalBlock(aug_i, M, - FiT * b.segment(ZDim * i) // F' * b - - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + augmentedHessian.updateOffDiagonalBlock( + aug_i, M, + FiT * b.segment(ZDim * i) // F' * b + - + FiT * + (Ei_P * + (E.transpose() * + b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) // (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) // add contribution of current factor - // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for now... - augmentedHessian.updateDiagonalBlock(aug_i, - ((FiT * (Fi - Ei_P * E.template block(ZDim * i, 0).transpose() * Fi))).eval()); + // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for + // now... + augmentedHessian.updateDiagonalBlock( + aug_i, + ((FiT * + (Fi - + Ei_P * E.template block(ZDim * i, 0).transpose() * Fi))) + .eval()); // upper triangular part of the hessian - for (size_t j = i + 1; j < m; j++) { // for each camera + for (size_t j = i + 1; j < m; j++) { // for each camera const MatrixZD& Fj = Fs[j]; DenseIndex aug_j = KeySlotMap.at(keys[j]); @@ -415,39 +433,38 @@ class CameraSet : public std::vector > // off diagonal block - store previous block // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal(); // add contribution of current factor - augmentedHessian.updateOffDiagonalBlock(aug_i, aug_j, - -FiT * (Ei_P * E.template block(ZDim * j, 0).transpose() * Fj)); + augmentedHessian.updateOffDiagonalBlock( + aug_i, aug_j, + -FiT * (Ei_P * E.template block(ZDim * j, 0).transpose() * + Fj)); } - } // end of for over cameras + } // end of for over cameras augmentedHessian.diagonalBlock(M)(0, 0) += b.squaredNorm(); } -private: - + private: /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & (*this); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar&(*this); } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; -template +template const int CameraSet::D; -template +template const int CameraSet::ZDim; -template -struct traits > : public Testable > { -}; +template +struct traits> : public Testable> {}; -template -struct traits > : public Testable > { -}; +template +struct traits> : public Testable> {}; -} // \ namespace gtsam +} // namespace gtsam diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 0ec120fe14..761c330b46 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -44,7 +44,7 @@ TEST(Ordering, constrained_ordering) { // unconstrained version { Ordering actual = Ordering::Colamd(symbolicGraph); - Ordering expected = Ordering({0, 1, 2, 3, 4, 5}); + Ordering expected{0, 1, 2, 3, 4, 5}; EXPECT(assert_equal(expected, actual)); } @@ -102,7 +102,7 @@ TEST(Ordering, grouped_constrained_ordering) { constraints[5] = 2; Ordering actual = Ordering::ColamdConstrained(symbolicGraph, constraints); - Ordering expected = {0, 1, 3, 2, 4, 5}; + Ordering expected{0, 1, 3, 2, 4, 5}; EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index ca9b31a1b8..ad25f3e59a 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -222,7 +222,7 @@ TEST(GaussianFactorGraph, gradient) { VectorValues solution = fg.optimize(); VectorValues actual2 = fg.gradient(solution); EXPECT(assert_equal(VectorValues::Zero(solution), actual2)); - } +} /* ************************************************************************* */ TEST(GaussianFactorGraph, transposeMultiplication) { diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 4998cb9f1d..ce5279d7b6 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -96,8 +96,7 @@ TEST(EliminationTree, Create) { EliminationTreeTester::buildHardcodedTree(simpleTestGraph1); // Build from factor graph - Ordering order; - order += 0, 1, 2, 3, 4; + const Ordering order{0, 1, 2, 3, 4}; SymbolicEliminationTree actual(simpleTestGraph1, order); CHECK(assert_equal(expected, actual)); From fa3a8103c5ea4069c55910cbcc0bd613d68d4b10 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 21:21:44 -0800 Subject: [PATCH 22/37] Fix issues with gcc compiler. --- gtsam/base/FastList.h | 3 +++ gtsam/symbolic/tests/symbolicExampleGraphs.h | 5 +++-- .../symbolic/tests/testSymbolicBayesTree.cpp | 19 +++++++------------ 3 files changed, 13 insertions(+), 14 deletions(-) diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index 47f8133118..29ecd7dbc9 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -56,6 +56,9 @@ class FastList: public std::list l) : Base(l) {} + #ifdef GTSAM_ALLOCATOR_BOOSTPOOL /** Copy constructor from a standard STL container */ FastList(const std::list& x) { diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index 2a9e71083e..e3350dc5a3 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -120,14 +120,15 @@ namespace gtsam { using sharedClique = SymbolicBayesTreeClique::shared_ptr; using Children = ListOf; - inline sharedClique LeafClique(const KeyVector& keys, + inline sharedClique LeafClique(const std::vector& keys, DenseIndex nrFrontals) { return boost::make_shared( boost::make_shared( SymbolicConditional::FromKeys(keys, nrFrontals))); } - inline sharedClique NodeClique(const KeyVector& keys, DenseIndex nrFrontals, + inline sharedClique NodeClique(const std::vector& keys, + DenseIndex nrFrontals, const std::vector& children) { sharedClique clique = LeafClique(keys, nrFrontals); clique->children.assign(children.begin(), children.end()); diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 1d92404a82..c6daada1bf 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -106,8 +106,7 @@ TEST(BayesTree, removePath) { SymbolicFactorGraph expected; expected.emplace_shared(_A_, _B_); expected.emplace_shared(_C_, _A_); - SymbolicBayesTree::Cliques expectedOrphans = - std::list{bayesTree[_D_], bayesTree[_E_]}; + SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_D_], bayesTree[_E_]}; SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; @@ -123,8 +122,7 @@ TEST(BayesTree, removePath) { SymbolicFactorGraph expected2; expected2.emplace_shared(_A_, _B_); expected2.emplace_shared(_E_, _B_); - SymbolicBayesTree::Cliques expectedOrphans2 = - std::list{bayesTree[_F_], bayesTree[_C_]}; + SymbolicBayesTree::Cliques expectedOrphans2{bayesTree[_F_], bayesTree[_C_]}; SymbolicBayesNet bn2; SymbolicBayesTree::Cliques orphans2; @@ -149,8 +147,8 @@ TEST(BayesTree, removePath2) { SymbolicFactorGraph expected; expected.emplace_shared(_E_, _L_, _B_); CHECK(assert_equal(expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans = - std::list{bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]}; + SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_T_], + bayesTree[_X_]}; CHECK(assert_container_equal(expectedOrphans | indirected, orphans | indirected)); } @@ -170,8 +168,7 @@ TEST(BayesTree, removePath3) { expected.emplace_shared(_E_, _L_, _B_); expected.emplace_shared(_T_, _E_, _L_); CHECK(assert_equal(expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans = - std::list{bayesTree[_S_], bayesTree[_X_]}; + SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_X_]}; CHECK(assert_container_equal(expectedOrphans | indirected, orphans | indirected)); } @@ -253,8 +250,7 @@ TEST(BayesTree, removeTop) { expected += SymbolicConditional::FromKeys(Keys(_S_)(_B_)(_L_), 1); CHECK(assert_equal(expected, bn)); - SymbolicBayesTree::Cliques expectedOrphans = - std::list{bayesTree[_T_], bayesTree[_X_]}; + SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_T_], bayesTree[_X_]}; CHECK(assert_container_equal(expectedOrphans | indirected, orphans | indirected)); @@ -291,8 +287,7 @@ TEST(BayesTree, removeTop2) { SymbolicConditional::FromKeys(Keys(_T_)(_E_)(_L_), 1)); CHECK(assert_equal(expected, bn)); - SymbolicBayesTree::Cliques expectedOrphans = - std::list{bayesTree[_S_], bayesTree[_X_]}; + SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_X_]}; CHECK(assert_container_equal(expectedOrphans | indirected, orphans | indirected)); } From 1e99f68f7051154cfd49db739cf7bbeb42cbbfe4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 21:22:25 -0800 Subject: [PATCH 23/37] Renamed ListOf to something more descriptive --- gtsam/symbolic/tests/symbolicExampleGraphs.h | 8 ++++---- gtsam/symbolic/tests/testSymbolicBayesTree.cpp | 2 +- gtsam/symbolic/tests/testSymbolicEliminationTree.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index e3350dc5a3..e0cf363392 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -32,11 +32,11 @@ namespace gtsam { // A small helper class to replace Boost's `list_of` function. template - class ListOf { + class ChainedVector { public: - ListOf(const T& c) { result.push_back(c); } + ChainedVector(const T& c) { result.push_back(c); } - ListOf& operator()(const T& c) { + ChainedVector& operator()(const T& c) { result.push_back(c); return *this; } @@ -118,7 +118,7 @@ namespace gtsam { boost::make_shared(_B_)}; using sharedClique = SymbolicBayesTreeClique::shared_ptr; - using Children = ListOf; + using Children = ChainedVector; inline sharedClique LeafClique(const std::vector& keys, DenseIndex nrFrontals) { diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index c6daada1bf..458579c60d 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -34,7 +34,7 @@ using namespace gtsam::symbol_shorthand; static bool debug = false; -using Keys = ListOf; // Create Keys a la list_of +using Keys = ChainedVector; // Create Keys a la list_of /* ************************************************************************* */ TEST(SymbolicBayesTree, clear) { diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index ce5279d7b6..9b750cfcab 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -88,7 +88,7 @@ static sharedNode Node(Key key, const SymbolicFactorGraph& factors, } // Use list_of replacement defined in symbolicExampleGraphs.h -using ChildNodes = ListOf; +using ChildNodes = ChainedVector; /* ************************************************************************* */ TEST(EliminationTree, Create) { From 9a6b0ddaa0fbaeeb44dffd709b5f5926e1ba61bd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 21:36:20 -0800 Subject: [PATCH 24/37] Add FactorGraph-inst.h --- gtsam/inference/BayesTreeCliqueBase-inst.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index dfcc7c318e..6da7d0fe4a 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -17,6 +17,7 @@ #pragma once #include +#include #include namespace gtsam { From c486472ca137df0d8bc20e56db7be9c52b7ea6ed Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 22:41:29 -0800 Subject: [PATCH 25/37] Attempts at fixing CI gcc/windows --- gtsam/linear/Errors.h | 3 +++ gtsam/symbolic/tests/symbolicExampleGraphs.h | 11 ++++------- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index 41ba441443..f5bea5501b 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -39,6 +39,9 @@ namespace gtsam { using Base::Base; // inherit constructors + /** Default constructor */ + Errors() = default; + /** break V into pieces according to its start indices */ Errors(const VectorValues&V); diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index e0cf363392..4f98188dd3 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -138,13 +138,10 @@ namespace gtsam { SymbolicBayesTree __asiaBayesTree() { SymbolicBayesTree result; - result.insertRoot(LeafClique(KeyVector{_E_, _L_, _B_}, 3)); - result.addClique(LeafClique(KeyVector{_S_, _B_, _L_}, 1), - result.roots().front()); - result.addClique(LeafClique(KeyVector{_T_, _E_, _L_}, 1), - result.roots().front()); - result.addClique(LeafClique(KeyVector{_X_, _E_}, 1), - result.roots().front()); + result.insertRoot(LeafClique({_E_, _L_, _B_}, 3)); + result.addClique(LeafClique({_S_, _B_, _L_}, 1), result.roots().front()); + result.addClique(LeafClique({_T_, _E_, _L_}, 1), result.roots().front()); + result.addClique(LeafClique({_X_, _E_}, 1), result.roots().front()); return result; } From 36d6b706671279263288b978152445ea636fd3ad Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Jan 2023 19:21:54 -0800 Subject: [PATCH 26/37] Fix gcc compilation errors by using correct allocator. --- gtsam/symbolic/tests/symbolicExampleGraphs.h | 48 +++++++++++-------- .../symbolic/tests/testSymbolicBayesTree.cpp | 2 - 2 files changed, 28 insertions(+), 22 deletions(-) diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index 4f98188dd3..716f502a4b 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include #include @@ -32,8 +33,10 @@ namespace gtsam { // A small helper class to replace Boost's `list_of` function. template - class ChainedVector { - public: + struct ChainedVector { + using Result = std::vector::type>; + Result result; + ChainedVector(const T& c) { result.push_back(c); } ChainedVector& operator()(const T& c) { @@ -41,10 +44,7 @@ namespace gtsam { return *this; } - operator std::vector() { return result; } - - private: - std::vector result; + operator Result() { return result; } }; const SymbolicFactorGraph simpleTestGraph1 { @@ -76,18 +76,18 @@ namespace gtsam { boost::make_shared(2,3)}; /* ************************************************************************* * - * 2 3 - * 0 1 : 2 - ****************************************************************************/ + * 2 3 + * 0 1 : 2 + ****************************************************************************/ SymbolicBayesTree __simpleChainBayesTree() { SymbolicBayesTree result; result.insertRoot(boost::make_shared( - boost::make_shared( + boost::make_shared( SymbolicConditional::FromKeys(KeyVector{2,3}, 2)))); result.addClique(boost::make_shared( - boost::make_shared( + boost::make_shared( SymbolicConditional::FromKeys(KeyVector{0,1,2}, 2))), - result.roots().front()); + result.roots().front()); return result; } @@ -95,9 +95,12 @@ namespace gtsam { /* ************************************************************************* */ // Keys for ASIA example from the tutorial with A and D evidence - const Key _X_=gtsam::symbol_shorthand::X(0), _T_=gtsam::symbol_shorthand::T(0), - _S_=gtsam::symbol_shorthand::S(0), _E_=gtsam::symbol_shorthand::E(0), - _L_=gtsam::symbol_shorthand::L(0), _B_=gtsam::symbol_shorthand::B(0); + const Key _X_ = symbol_shorthand::X(0), + _T_ = symbol_shorthand::T(0), + _S_ = symbol_shorthand::S(0), + _E_ = symbol_shorthand::E(0), + _L_ = symbol_shorthand::L(0), + _B_ = symbol_shorthand::B(0); // Factor graph for Asia example const SymbolicFactorGraph asiaGraph = { @@ -117,25 +120,30 @@ namespace gtsam { boost::make_shared(_L_, _B_), boost::make_shared(_B_)}; + /* ************************************************************************* */ + // Allow creating Cliques and Keys in `list_of` chaining style: using sharedClique = SymbolicBayesTreeClique::shared_ptr; using Children = ChainedVector; + using Keys = ChainedVector; - inline sharedClique LeafClique(const std::vector& keys, + inline sharedClique LeafClique(const Keys::Result& keys, DenseIndex nrFrontals) { return boost::make_shared( boost::make_shared( SymbolicConditional::FromKeys(keys, nrFrontals))); } - inline sharedClique NodeClique(const std::vector& keys, + inline sharedClique NodeClique(const Keys::Result& keys, DenseIndex nrFrontals, - const std::vector& children) { + const Children::Result& children) { sharedClique clique = LeafClique(keys, nrFrontals); clique->children.assign(children.begin(), children.end()); for (auto&& child : children) child->parent_ = clique; return clique; } + /* ************************************************************************* */ + // BayesTree for Asia example SymbolicBayesTree __asiaBayesTree() { SymbolicBayesTree result; result.insertRoot(LeafClique({_E_, _L_, _B_}, 3)); @@ -149,5 +157,5 @@ namespace gtsam { /* ************************************************************************* */ const Ordering asiaOrdering{_X_, _T_, _S_, _E_, _L_, _B_}; - } -} + } // namespace +} // namespace gtsam diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 458579c60d..0095c9293f 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -34,8 +34,6 @@ using namespace gtsam::symbol_shorthand; static bool debug = false; -using Keys = ChainedVector; // Create Keys a la list_of - /* ************************************************************************* */ TEST(SymbolicBayesTree, clear) { SymbolicBayesTree bayesTree = asiaBayesTree; From 3b369e5d8eded11f3c71cc8a9c3e2a2e4aecca03 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Jan 2023 20:09:50 -0800 Subject: [PATCH 27/37] Define Errors as a typedef to FastList. --- gtsam/linear/Errors.cpp | 44 +++++++------- gtsam/linear/Errors.h | 78 +++++++++++-------------- gtsam/linear/SubgraphPreconditioner.cpp | 4 +- 3 files changed, 56 insertions(+), 70 deletions(-) diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index e7d893d25e..92f7bb4b81 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -26,16 +26,18 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Errors::Errors(const VectorValues& V) { - for(const Vector& e: V | boost::adaptors::map_values) { - push_back(e); +Errors createErrors(const VectorValues& V) { + Errors result; + for (const Vector& e : V | boost::adaptors::map_values) { + result.push_back(e); } + return result; } /* ************************************************************************* */ -void Errors::print(const std::string& s) const { +void print(const Errors& e, const string& s) { cout << s << endl; - for(const Vector& v: *this) + for(const Vector& v: e) gtsam::print(v); } @@ -48,50 +50,49 @@ struct equalsVector : public std::function { } }; -bool Errors::equals(const Errors& expected, double tol) const { - if( size() != expected.size() ) return false; - return equal(begin(),end(),expected.begin(),equalsVector(tol)); +bool equality(const Errors& actual, const Errors& expected, double tol) { + if (actual.size() != expected.size()) return false; + return equal(actual.begin(), actual.end(), expected.begin(), + equalsVector(tol)); } /* ************************************************************************* */ -Errors Errors::operator+(const Errors& b) const { +Errors operator+(const Errors& a, const Errors& b) { #ifndef NDEBUG - size_t m = size(); + size_t m = a.size(); if (b.size()!=m) throw(std::invalid_argument("Errors::operator+: incompatible sizes")); #endif Errors result; Errors::const_iterator it = b.begin(); - for(const Vector& ai: *this) + for(const Vector& ai: a) result.push_back(ai + *(it++)); return result; } /* ************************************************************************* */ -Errors Errors::operator-(const Errors& b) const { +Errors operator-(const Errors& a, const Errors& b) { #ifndef NDEBUG - size_t m = size(); + size_t m = a.size(); if (b.size()!=m) throw(std::invalid_argument("Errors::operator-: incompatible sizes")); #endif Errors result; Errors::const_iterator it = b.begin(); - for(const Vector& ai: *this) + for(const Vector& ai: a) result.push_back(ai - *(it++)); return result; } /* ************************************************************************* */ -Errors Errors::operator-() const { +Errors operator-(const Errors& a) { Errors result; - for(const Vector& ai: *this) + for(const Vector& ai: a) result.push_back(-ai); return result; } - - /* ************************************************************************* */ double dot(const Errors& a, const Errors& b) { #ifndef NDEBUG @@ -102,7 +103,7 @@ double dot(const Errors& a, const Errors& b) { double result = 0.0; Errors::const_iterator it = b.begin(); for(const Vector& ai: a) - result += gtsam::dot(ai, *(it++)); + result += gtsam::dot(ai, *(it++)); return result; } @@ -113,11 +114,6 @@ void axpy(double alpha, const Errors& x, Errors& y) { yi += alpha * (*(it++)); } -/* ************************************************************************* */ -void print(const Errors& a, const string& s) { - a.print(s); -} - /* ************************************************************************* */ } // gtsam diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index f5bea5501b..faf83d4d11 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -20,64 +20,54 @@ #pragma once #include -#include #include +#include #include namespace gtsam { - // Forward declarations - class VectorValues; - - /** vector of errors */ - class GTSAM_EXPORT Errors : public FastList { - - using Base = FastList; - - public: - - using Base::Base; // inherit constructors - - /** Default constructor */ - Errors() = default; - - /** break V into pieces according to its start indices */ - Errors(const VectorValues&V); +// Forward declarations +class VectorValues; - /** print */ - void print(const std::string& s = "Errors") const; +/// Errors is a vector of errors. +using Errors = FastList; - /** equals, for unit testing */ - bool equals(const Errors& expected, double tol=1e-9) const; +/// Break V into pieces according to its start indices. +GTSAM_EXPORT Errors createErrors(const VectorValues& V); - /** Addition */ - Errors operator+(const Errors& b) const; +/// Print an Errors instance. +GTSAM_EXPORT void print(const Errors& e, const std::string& s = "Errors"); - /** subtraction */ - Errors operator-(const Errors& b) const; +// Check equality for unit testing. +GTSAM_EXPORT bool equality(const Errors& actual, const Errors& expected, + double tol = 1e-9); - /** negation */ - Errors operator-() const ; +/// Addition. +GTSAM_EXPORT Errors operator+(const Errors& a, const Errors& b); - }; // Errors +/// Subtraction. +GTSAM_EXPORT Errors operator-(const Errors& a, const Errors& b); - /** - * dot product - */ - GTSAM_EXPORT double dot(const Errors& a, const Errors& b); +/// Negation. +GTSAM_EXPORT Errors operator-(const Errors& a); - /** - * BLAS level 2 style - */ - GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y); +/// Dot product. +GTSAM_EXPORT double dot(const Errors& a, const Errors& b); - /** print with optional string */ - GTSAM_EXPORT void print(const Errors& a, const std::string& s = "Error"); +/// BLAS level 2 style AXPY, `y := alpha*x + y` +GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y); - /// traits - template<> - struct traits : public Testable { - }; +/// traits +template <> +struct traits { + static void Print(const Errors& e, const std::string& str = "") { + print(e, str); + } + static bool Equals(const Errors& actual, const Errors& expected, + double tol = 1e-8) { + return equality(actual, expected, tol); + } +}; -} //\ namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 6689cdbed4..4e761a3fd3 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -110,7 +110,7 @@ VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { /* ************************************************************************* */ double SubgraphPreconditioner::error(const VectorValues& y) const { - Errors e(y); + Errors e = createErrors(y); VectorValues x = this->x(y); Errors e2 = Ab2_.gaussianErrors(x); return 0.5 * (dot(e, e) + dot(e2,e2)); @@ -129,7 +129,7 @@ VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const { /* ************************************************************************* */ // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] Errors SubgraphPreconditioner::operator*(const VectorValues &y) const { - Errors e(y); + Errors e = createErrors(y); VectorValues x = Rc1_.backSubstitute(y); /* x=inv(R1)*y */ Errors e2 = Ab2_ * x; /* A2*x */ e.splice(e.end(), e2); From a9409ac9e27aa6bd08dcf0643c4590761dce5d12 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Jan 2023 22:42:03 -0800 Subject: [PATCH 28/37] Kill errors wrapper - internal class not used in any tests. --- gtsam/linear/linear.i | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 4f9e4174f5..d733340c90 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -625,17 +625,6 @@ virtual class GaussianBayesTree { gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; }; -#include -class Errors { - //Constructors - Errors(); - Errors(const gtsam::VectorValues& V); - - //Testable - void print(string s = "Errors"); - bool equals(const gtsam::Errors& expected, double tol) const; -}; - #include class GaussianISAM { //Constructor From 98f3c9676fd95a69bf12907922b2b5766a0e0316 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Jan 2023 22:42:51 -0800 Subject: [PATCH 29/37] Replace blanket inclusion of constructors (giving trouble on Windows) with initializer-list constructor. --- gtsam/linear/GaussianFactorGraph.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 7469d96d5a..ce475e1009 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -86,7 +86,13 @@ namespace gtsam { /** Default constructor */ GaussianFactorGraph() {} - using Base::Base; // Inherit constructors + /** + * Construct from an initializer lists of GaussianFactor shared pointers. + * Example: + * GaussianFactorGraph graph = { factor1, factor2, factor3 }; + */ + GaussianFactorGraph(std::initializer_list factors) : Base(factors) {} + /** Construct from iterator over factors */ template From b51f176232ea1e8f9c02de3fb40aad2f37495852 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Jan 2023 22:43:14 -0800 Subject: [PATCH 30/37] Simplify initializer list constructors. --- gtsam/inference/FactorGraph.h | 6 ++---- gtsam/linear/VectorValues.h | 7 ++----- gtsam/symbolic/SymbolicBayesNet.h | 7 ++----- gtsam/symbolic/SymbolicFactorGraph.h | 7 +++---- 4 files changed, 9 insertions(+), 18 deletions(-) diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 915ae273af..68dc79d3fd 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -166,10 +166,8 @@ class FactorGraph { * FactorGraph fg = {make_shared(), ...}; */ template > - FactorGraph( - std::initializer_list> sharedFactors) { - for (auto&& f : sharedFactors) factors_.push_back(f); - } + FactorGraph(std::initializer_list> sharedFactors) + : factors_(sharedFactors) {} /// @} /// @name Adding Single Factors diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 6a645150f2..42916f619c 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -92,11 +92,8 @@ namespace gtsam { VectorValues() {} /// Construct from initializer list. - VectorValues(std::initializer_list> init) { - for (const auto& p : init) { - values_.insert(p); // Insert key-value pair into map - } - } + VectorValues(std::initializer_list> init) + : values_(init.begin(), init.end()) {} /** Merge two VectorValues into one, this is more efficient than inserting * elements one by one. */ diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 43f1edd692..643feee3f4 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -64,11 +64,8 @@ namespace gtsam { * Constructor that takes an initializer list of shared pointers. * FactorGraph fg = {make_shared(), ...}; */ - SymbolicBayesNet( - std::initializer_list> - sharedFactors) { - for (auto&& f : sharedFactors) factors_.push_back(f); - } + SymbolicBayesNet(std::initializer_list> sharedFactors) + : Base() {} /// Construct from a single conditional SymbolicBayesNet(SymbolicConditional&& c) { diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index aeb9e3b84b..1d4292cbfb 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -91,10 +91,9 @@ namespace gtsam { * Constructor that takes an initializer list of shared pointers. * FactorGraph fg = {make_shared(), ...}; */ - SymbolicFactorGraph(std::initializer_list> - sharedFactors) { - for (auto&& f : sharedFactors) factors_.push_back(f); - } + SymbolicFactorGraph( + std::initializer_list> sharedFactors) + : Base(sharedFactors) {} /// Construct from a single factor SymbolicFactorGraph(SymbolicFactor&& c) { From 0e01ea6b5dcf7204ed0ff09a2f86c7f1796f354e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 00:00:03 -0800 Subject: [PATCH 31/37] Fix issue with initializer constructors --- gtsam/inference/BayesNet.h | 6 ++++++ gtsam/linear/GaussianBayesNet.h | 6 ++---- gtsam/symbolic/SymbolicBayesNet.h | 6 +++--- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index d562f3ae6e..4704d28738 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -50,6 +50,12 @@ class BayesNet : public FactorGraph { BayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} + /** + * Constructor that takes an initializer list of shared pointers. + * BayesNet bn = {make_shared(), ...}; + */ + BayesNet(std::initializer_list conditionals): Base(conditionals) {} + /// @} public: diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index a63cfa6c70..62366b602f 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -71,10 +71,8 @@ namespace gtsam { */ template GaussianBayesNet( - std::initializer_list > - sharedConditionals) { - for (auto&& gc : sharedConditionals) push_back(gc); - } + std::initializer_list > conditionals) + : Base(conditionals) {} /// Destructor virtual ~GaussianBayesNet() = default; diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 643feee3f4..ef1505108a 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -62,10 +62,10 @@ namespace gtsam { /** * Constructor that takes an initializer list of shared pointers. - * FactorGraph fg = {make_shared(), ...}; + * SymbolicBayesNet bn = {make_shared(), ...}; */ - SymbolicBayesNet(std::initializer_list> sharedFactors) - : Base() {} + SymbolicBayesNet(std::initializer_list> conditionals) + : Base(conditionals) {} /// Construct from a single conditional SymbolicBayesNet(SymbolicConditional&& c) { From 353a5b35741b88eed775671e40e205e9cb917391 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 06:48:51 -0800 Subject: [PATCH 32/37] Forgot to add correct type in elimination tree (needed for compilation with tbb). --- gtsam/symbolic/tests/testSymbolicEliminationTree.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 9b750cfcab..2b7ab6e822 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -73,6 +73,7 @@ class EliminationTreeTester { } }; +// Create a leaf node. static sharedNode Leaf(Key key, const SymbolicFactorGraph& factors) { sharedNode node(new SymbolicEliminationTree::Node()); node->key = key; @@ -80,16 +81,17 @@ static sharedNode Leaf(Key key, const SymbolicFactorGraph& factors) { return node; } +// Use list_of replacement defined in symbolicExampleGraphs.h +using ChildNodes = ChainedVector; + +// Create a node with children. static sharedNode Node(Key key, const SymbolicFactorGraph& factors, - const std::vector& children) { + const ChildNodes::Result& children) { sharedNode node = Leaf(key, factors); node->children.assign(children.begin(), children.end()); return node; } -// Use list_of replacement defined in symbolicExampleGraphs.h -using ChildNodes = ChainedVector; - /* ************************************************************************* */ TEST(EliminationTree, Create) { SymbolicEliminationTree expected = From 874f0deeee06c80cf9e9d37ac0211bd8ab4fbf19 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 08:58:31 -0800 Subject: [PATCH 33/37] One last omission --- gtsam/symbolic/tests/testSymbolicEliminationTree.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 2b7ab6e822..cb370f5539 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -31,6 +31,9 @@ using namespace gtsam::symbol_shorthand; using namespace std; using sharedNode = SymbolicEliminationTree::sharedNode; +// Use list_of replacement defined in symbolicExampleGraphs.h +using ChildNodes = ChainedVector; + class EliminationTreeTester { public: // build hardcoded tree @@ -65,8 +68,7 @@ class EliminationTreeTester { return tree; } - static SymbolicEliminationTree MakeTree( - const std::vector& roots) { + static SymbolicEliminationTree MakeTree(const ChildNodes::Result& roots) { SymbolicEliminationTree et; et.roots_.assign(roots.begin(), roots.end()); return et; @@ -81,9 +83,6 @@ static sharedNode Leaf(Key key, const SymbolicFactorGraph& factors) { return node; } -// Use list_of replacement defined in symbolicExampleGraphs.h -using ChildNodes = ChainedVector; - // Create a node with children. static sharedNode Node(Key key, const SymbolicFactorGraph& factors, const ChildNodes::Result& children) { From 6a035b88d731d879ff957ea36c2771f4e276062d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 09:35:58 -0800 Subject: [PATCH 34/37] Two more tbb compile issues --- gtsam/hybrid/tests/TinyHybridExample.h | 2 +- gtsam/linear/tests/testErrors.cpp | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h index c34d55bf02..ad13749116 100644 --- a/gtsam/hybrid/tests/TinyHybridExample.h +++ b/gtsam/hybrid/tests/TinyHybridExample.h @@ -57,7 +57,7 @@ inline HybridBayesNet createHybridBayesNet(int num_measurements = 1, // Add prior on mode. const size_t nrModes = manyModes ? num_measurements : 1; - for (int i = 0; i < nrModes; i++) { + for (const size_t i = 0; i < nrModes; i++) { bayesNet.emplace_back(new DiscreteConditional({M(i), 2}, "4/6")); } return bayesNet; diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index 415cd9496b..2075dc5bca 100644 --- a/gtsam/linear/tests/testErrors.cpp +++ b/gtsam/linear/tests/testErrors.cpp @@ -24,12 +24,11 @@ using namespace gtsam; /* ************************************************************************* */ TEST(Errors, arithmetic) { - Errors e = std::list{Vector2(1.0, 2.0), Vector3(3.0, 4.0, 5.0)}; + Errors e = {Vector2(1.0, 2.0), Vector3(3.0, 4.0, 5.0)}; DOUBLES_EQUAL(1 + 4 + 9 + 16 + 25, dot(e, e), 1e-9); axpy(2.0, e, e); - const Errors expected = - std::list{Vector2(3.0, 6.0), Vector3(9.0, 12.0, 15.0)}; + const Errors expected = {Vector2(3.0, 6.0), Vector3(9.0, 12.0, 15.0)}; CHECK(assert_equal(expected, e)); } From 416cb65f6bc2c2b688092b3e4c575590bf07472c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 09:58:29 -0800 Subject: [PATCH 35/37] Remove const, use size_t everywhere. --- gtsam/hybrid/tests/TinyHybridExample.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h index ad13749116..0ef8955c4f 100644 --- a/gtsam/hybrid/tests/TinyHybridExample.h +++ b/gtsam/hybrid/tests/TinyHybridExample.h @@ -36,12 +36,12 @@ const DiscreteKey mode{M(0), 2}; * num_measurements is the number of measurements of the continuous variable x0. * If manyModes is true, then we introduce one mode per measurement. */ -inline HybridBayesNet createHybridBayesNet(int num_measurements = 1, +inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1, bool manyModes = false) { HybridBayesNet bayesNet; // Create Gaussian mixture z_i = x0 + noise for each measurement. - for (int i = 0; i < num_measurements; i++) { + for (size_t i = 0; i < num_measurements; i++) { const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode; bayesNet.emplace_back( new GaussianMixture({Z(i)}, {X(0)}, {mode_i}, @@ -57,7 +57,7 @@ inline HybridBayesNet createHybridBayesNet(int num_measurements = 1, // Add prior on mode. const size_t nrModes = manyModes ? num_measurements : 1; - for (const size_t i = 0; i < nrModes; i++) { + for (size_t i = 0; i < nrModes; i++) { bayesNet.emplace_back(new DiscreteConditional({M(i), 2}, "4/6")); } return bayesNet; @@ -70,7 +70,7 @@ inline HybridBayesNet createHybridBayesNet(int num_measurements = 1, * the generative Bayes net model HybridBayesNet::Example(num_measurements) */ inline HybridGaussianFactorGraph createHybridGaussianFactorGraph( - int num_measurements = 1, + size_t num_measurements = 1, boost::optional measurements = boost::none, bool manyModes = false) { auto bayesNet = createHybridBayesNet(num_measurements, manyModes); From 61ff43fada64ef5693bb89f24b648c4f7efc5738 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 11:05:21 -0800 Subject: [PATCH 36/37] Yet another issue with gcc-tbb --- gtsam/linear/tests/testGaussianFactorGraph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index ad25f3e59a..0d7003ccbc 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -228,8 +228,8 @@ TEST(GaussianFactorGraph, gradient) { TEST(GaussianFactorGraph, transposeMultiplication) { GaussianFactorGraph A = createSimpleGaussianFactorGraph(); - Errors e = std::list{Vector2(0.0, 0.0), Vector2(15.0, 0.0), - Vector2(0.0, -5.0), Vector2(-7.5, -5.0)}; + Errors e = {Vector2(0.0, 0.0), Vector2(15.0, 0.0), Vector2(0.0, -5.0), + Vector2(-7.5, -5.0)}; VectorValues expected; expected.insert(1, Vector2(-37.5, -50.0)); From d41d4c224d309a493c7446c50bb84c1a1c05350c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 13:16:39 -0800 Subject: [PATCH 37/37] Another gcc issue, switch to FastList --- tests/testGaussianISAM2.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index e7113e0fa9..8dbf3fff6d 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -684,7 +684,7 @@ TEST(ISAM2, marginalizeLeaves1) { isam.update(factors, values, FactorIndices(), constrainedKeys); - std::list leafKeys {0}; + FastList leafKeys {0}; EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -714,7 +714,7 @@ TEST(ISAM2, marginalizeLeaves2) { isam.update(factors, values, FactorIndices(), constrainedKeys); - std::list leafKeys {0}; + FastList leafKeys {0}; EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -753,7 +753,7 @@ TEST(ISAM2, marginalizeLeaves3) { isam.update(factors, values, FactorIndices(), constrainedKeys); - std::list leafKeys {0}; + FastList leafKeys {0}; EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -778,7 +778,7 @@ TEST(ISAM2, marginalizeLeaves4) { isam.update(factors, values, FactorIndices(), constrainedKeys); - std::list leafKeys {1}; + FastList leafKeys {1}; EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -789,7 +789,7 @@ TEST(ISAM2, marginalizeLeaves5) ISAM2 isam = createSlamlikeISAM2(); // Marginalize - std::list marginalizeKeys {0}; + FastList marginalizeKeys {0}; EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys)); }