diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 442923662..c382e0f3c 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 ed9e6bbbc..37d65320a 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 d0e8adb55..208efafb8 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 fcdbd0393..b74a5e5ea 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 c3a59aada..019963e59 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 c9024e018..bf5f2f25c 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 d3529cb55..a15b6f922 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 27c959675..e888875a4 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 eac0d834e..0e27b5fbb 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 36a2cacd8..58af66a09 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 fe49de928..49796f80d 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 8a32837f4..dc7d26035 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 6f0aa605b..07706a6a5 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 1dbd25666..095bedfab 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 fc56b1a9f..9f61d4fb0 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 38b202c41..7439a5436 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 9a4e01c46..bb35ada19 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 a321aa25d..fedc75945 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 1f5ec6350..a6aa7bfc5 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 e8e916f04..e7113e0fa 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 ad7cabb99..64c86c4c4 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 c5b4e42ec..8abd54795 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 6af3e0163..bf04b9e93 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); }