Convert to initializer lists

release/4.3a0
Frank Dellaert 2023-01-07 15:47:01 -08:00
parent 80137792c9
commit d3380bc065
5 changed files with 61 additions and 102 deletions

View File

@ -22,11 +22,8 @@
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/assign/std.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace boost::assign;
namespace example { namespace example {
SymbolicFactorGraph symbolicChain() { SymbolicFactorGraph symbolicChain() {
@ -47,33 +44,33 @@ TEST(Ordering, constrained_ordering) {
// unconstrained version // unconstrained version
{ {
Ordering actual = Ordering::Colamd(symbolicGraph); 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)); EXPECT(assert_equal(expected, actual));
} }
// constrained version - push one set to the end // constrained version - push one set to the end
{ {
Ordering actual = Ordering::ColamdConstrainedLast(symbolicGraph, list_of(2)(4)); Ordering actual = Ordering::ColamdConstrainedLast(symbolicGraph, {2, 4});
Ordering expected = Ordering(list_of(0)(1)(5)(3)(4)(2)); Ordering expected = Ordering({0, 1, 5, 3, 4, 2});
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
// constrained version - push one set to the start // constrained version - push one set to the start
{ {
Ordering actual = Ordering::ColamdConstrainedFirst(symbolicGraph, list_of(2)(4)); Ordering actual = Ordering::ColamdConstrainedFirst(symbolicGraph, {2, 4});
Ordering expected = Ordering(list_of(2)(4)(0)(1)(3)(5)); Ordering expected = Ordering({2, 4, 0, 1, 3, 5});
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
// Make sure giving empty constraints does not break the code // Make sure giving empty constraints does not break the code
{ {
Ordering actual = Ordering::ColamdConstrainedLast(symbolicGraph, {}); 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)); EXPECT(assert_equal(expected, actual));
} }
{ {
Ordering actual = Ordering::ColamdConstrainedFirst(symbolicGraph, {}); 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)); EXPECT(assert_equal(expected, actual));
} }
@ -81,11 +78,11 @@ TEST(Ordering, constrained_ordering) {
SymbolicFactorGraph emptyGraph; SymbolicFactorGraph emptyGraph;
Ordering empty; Ordering empty;
{ {
Ordering actual = Ordering::ColamdConstrainedLast(emptyGraph, list_of(2)(4)); Ordering actual = Ordering::ColamdConstrainedLast(emptyGraph, {2, 4});
EXPECT(assert_equal(empty, actual)); 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)); EXPECT(assert_equal(empty, actual));
} }
} }
@ -105,7 +102,7 @@ TEST(Ordering, grouped_constrained_ordering) {
constraints[5] = 2; constraints[5] = 2;
Ordering actual = Ordering::ColamdConstrained(symbolicGraph, constraints); 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)); EXPECT(assert_equal(expected, actual));
} }
@ -139,9 +136,11 @@ TEST(Ordering, csr_format) {
MetisIndex mi(symbolicGraph); MetisIndex mi(symbolicGraph);
vector<int> xadjExpected, adjExpected; const vector<int> xadjExpected{0, 2, 5, 8, 11, 13, 16, 20,
xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; 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; 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(xadjExpected == mi.xadj());
EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected.size() == mi.adj().size());
@ -161,9 +160,8 @@ TEST(Ordering, csr_format_2) {
MetisIndex mi(symbolicGraph); MetisIndex mi(symbolicGraph);
vector<int> xadjExpected, adjExpected; const std::vector<int> xadjExpected{0, 1, 4, 6, 8, 10},
xadjExpected += 0, 1, 4, 6, 8, 10; adjExpected{1, 0, 2, 4, 1, 3, 2, 4, 1, 3};
adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3;
EXPECT(xadjExpected == mi.xadj()); EXPECT(xadjExpected == mi.xadj());
EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected.size() == mi.adj().size());
@ -183,9 +181,8 @@ TEST(Ordering, csr_format_3) {
MetisIndex mi(symbolicGraph); MetisIndex mi(symbolicGraph);
vector<int> xadjExpected, adjExpected; const std::vector<int> xadjExpected{0, 1, 4, 6, 8, 10},
xadjExpected += 0, 1, 4, 6, 8, 10; adjExpected{1, 0, 2, 4, 1, 3, 2, 4, 1, 3};
adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3;
//size_t minKey = mi.minKey(); //size_t minKey = mi.minKey();
vector<int> adjAcutal = mi.adj(); vector<int> adjAcutal = mi.adj();
@ -202,24 +199,18 @@ TEST(Ordering, csr_format_3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Ordering, AppendVector) { TEST(Ordering, AppendVector) {
using symbol_shorthand::X; using symbol_shorthand::X;
KeyVector keys{X(0), X(1), X(2)};
Ordering actual; Ordering actual;
KeyVector keys = {X(0), X(1), X(2)};
actual += keys; actual += keys;
Ordering expected; Ordering expected{X(0), X(1), X(2)};
expected += X(0);
expected += X(1);
expected += X(2);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Ordering, Contains) { TEST(Ordering, Contains) {
using symbol_shorthand::X; using symbol_shorthand::X;
Ordering ordering; Ordering ordering{X(0), X(1), X(2)};
ordering += X(0);
ordering += X(1);
ordering += X(2);
EXPECT(ordering.contains(X(1))); EXPECT(ordering.contains(X(1)));
EXPECT(!ordering.contains(X(4))); EXPECT(!ordering.contains(X(4)));
@ -239,9 +230,8 @@ TEST(Ordering, csr_format_4) {
MetisIndex mi(symbolicGraph); MetisIndex mi(symbolicGraph);
vector<int> xadjExpected, adjExpected; const vector<int> xadjExpected{0, 1, 3, 5, 7, 9, 10},
xadjExpected += 0, 1, 3, 5, 7, 9, 10; adjExpected{1, 0, 2, 1, 3, 2, 4, 3, 5, 4};
adjExpected += 1, 0, 2, 1, 3, 2, 4, 3, 5, 4;
vector<int> adjAcutal = mi.adj(); vector<int> adjAcutal = mi.adj();
vector<int> xadjActual = mi.xadj(); vector<int> xadjActual = mi.xadj();
@ -274,9 +264,7 @@ TEST(Ordering, metis) {
MetisIndex mi(symbolicGraph); MetisIndex mi(symbolicGraph);
vector<int> xadjExpected, adjExpected; const vector<int> xadjExpected{0, 1, 3, 4}, adjExpected{1, 0, 2, 1};
xadjExpected += 0, 1, 3, 4;
adjExpected += 1, 0, 2, 1;
EXPECT(xadjExpected == mi.xadj()); EXPECT(xadjExpected == mi.xadj());
EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected.size() == mi.adj().size());
@ -303,7 +291,7 @@ TEST(Ordering, MetisLoop) {
// | - P( 4 | 0 3) // | - P( 4 | 0 3)
// | | - P( 5 | 0 4) // | | - P( 5 | 0 4)
// | - P( 2 | 1 3) // | - 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)); EXPECT(assert_equal(expected, actual));
} }
#elif defined(_WIN32) #elif defined(_WIN32)
@ -313,7 +301,7 @@ TEST(Ordering, MetisLoop) {
// | - P( 3 | 5 2) // | - P( 3 | 5 2)
// | | - P( 4 | 5 3) // | | - P( 4 | 5 3)
// | - P( 1 | 0 2) // | - 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)); EXPECT(assert_equal(expected, actual));
} }
#else #else
@ -323,7 +311,7 @@ TEST(Ordering, MetisLoop) {
// | - P( 2 | 4 1) // | - P( 2 | 4 1)
// | | - P( 3 | 4 2) // | | - P( 3 | 4 2)
// | - P( 5 | 0 1) // | - 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)); EXPECT(assert_equal(expected, actual));
} }
#endif #endif
@ -347,7 +335,7 @@ TEST(Ordering, MetisSingleNode) {
symbolicGraph.push_factor(7); symbolicGraph.push_factor(7);
Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph); Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph);
Ordering expected = Ordering(list_of(7)); Ordering expected = Ordering({7});
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
#endif #endif
@ -365,7 +353,7 @@ TEST(Ordering, Create) {
//| | | - P( 1 | 2) //| | | - P( 1 | 2)
//| | | | - P( 0 | 1) //| | | | - P( 0 | 1)
Ordering actual = Ordering::Create(Ordering::COLAMD, symbolicGraph); 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)); EXPECT(assert_equal(expected, actual));
} }
@ -376,7 +364,7 @@ TEST(Ordering, Create) {
//- P( 1 0 2) //- P( 1 0 2)
//| - P( 3 4 | 2) //| - P( 3 4 | 2)
//| | - P( 5 | 4) //| | - 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)); EXPECT(assert_equal(expected, actual));
} }
#endif #endif

View File

@ -22,11 +22,8 @@
#include <gtsam/inference/VariableSlots.h> #include <gtsam/inference/VariableSlots.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h> #include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <boost/assign/std/vector.hpp>
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
using namespace boost::assign;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(VariableSlots, constructor) { TEST(VariableSlots, constructor) {
@ -41,12 +38,12 @@ TEST(VariableSlots, constructor) {
static const size_t none = numeric_limits<size_t>::max(); static const size_t none = numeric_limits<size_t>::max();
VariableSlots expected((SymbolicFactorGraph())); VariableSlots expected((SymbolicFactorGraph()));
expected[0] += none, 0, 0, none; expected[0] = {none, 0, 0, none};
expected[1] += none, 1, none, none; expected[1] = {none, 1, none, none};
expected[2] += 0, none, 1, none; expected[2] = {0, none, 1, none};
expected[3] += 1, none, none, none; expected[3] = {1, none, none, none};
expected[5] += none, none, none, 0; expected[5] = {none, none, none, 0};
expected[9] += none, none, none, 1; expected[9] = {none, none, none, 1};
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }

View File

@ -15,9 +15,6 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <boost/assign/std/list.hpp> // for +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/linear/Errors.h> #include <gtsam/linear/Errors.h>
@ -26,16 +23,13 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Errors, arithmetic ) TEST(Errors, arithmetic) {
{ Errors e{Vector2(1.0, 2.0), Vector3(3.0, 4.0, 5.0)};
Errors e; DOUBLES_EQUAL(1 + 4 + 9 + 16 + 25, dot(e, e), 1e-9);
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); axpy(2.0, e, e);
Errors expected; const Errors expected{Vector2(3.0, 6.0), Vector3(9.0, 12.0, 15.0)};
expected += Vector2(3.0,6.0), Vector3(9.0,12.0,15.0); CHECK(assert_equal(expected, e));
CHECK(assert_equal(expected,e));
} }
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -26,11 +26,8 @@
#include <gtsam/linear/GaussianDensity.h> #include <gtsam/linear/GaussianDensity.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/assign/list_of.hpp>
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
@ -38,7 +35,6 @@
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
using namespace boost::assign;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::Y; using symbol_shorthand::Y;
@ -64,11 +60,7 @@ TEST(GaussianConditional, constructor)
Vector d = Vector2(1.0, 2.0); Vector d = Vector2(1.0, 2.0);
SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector2(3.0, 4.0)); SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector2(3.0, 4.0));
vector<pair<Key, Matrix> > terms = pair_list_of vector<pair<Key, Matrix> > terms = {{1, R}, {3, S1}, {5, S2}, {7, S3}};
(1, R)
(3, S1)
(5, S2)
(7, S3);
GaussianConditional actual(terms, 1, d, s); GaussianConditional actual(terms, 1, d, s);
@ -223,14 +215,10 @@ TEST( GaussianConditional, solve )
Vector sx1(2); sx1 << 1.0, 1.0; Vector sx1(2); sx1 << 1.0, 1.0;
Vector sl1(2); sl1 << 1.0, 1.0; Vector sl1(2); sl1 << 1.0, 1.0;
VectorValues expected = map_list_of VectorValues expected = {{1, expectedX} {2, sx1} {10, sl1}};
(1, expectedX)
(2, sx1)
(10, sl1);
VectorValues solution = map_list_of VectorValues solution = {{2, sx1}, // parents
(2, sx1) // parents {10, sl1}};
(10, sl1);
solution.insert(cg.solve(solution)); solution.insert(cg.solve(solution));
EXPECT(assert_equal(expected, solution, tol)); EXPECT(assert_equal(expected, solution, tol));
@ -254,12 +242,10 @@ TEST( GaussianConditional, solve_simple )
Vector sx1 = Vector2(9.0, 10.0); Vector sx1 = Vector2(9.0, 10.0);
// elimination order: 1, 2 // elimination order: 1, 2
VectorValues actual = map_list_of VectorValues actual = {{2, sx1}}; // parent
(2, sx1); // parent
VectorValues expected = map_list_of<Key, Vector> VectorValues expected = {
(2, sx1) {2, sx1}, {1, (Vector(4) << -3.1, -3.4, -11.9, -13.2).finished()}};
(1, (Vector(4) << -3.1,-3.4,-11.9,-13.2).finished());
// verify indices/size // verify indices/size
EXPECT_LONGS_EQUAL(2, (long)cg.size()); EXPECT_LONGS_EQUAL(2, (long)cg.size());
@ -290,13 +276,10 @@ TEST( GaussianConditional, solve_multifrontal )
Vector sl1 = Vector2(9.0, 10.0); Vector sl1 = Vector2(9.0, 10.0);
// elimination order; _x_, _x1_, _l1_ // elimination order; _x_, _x1_, _l1_
VectorValues actual = map_list_of VectorValues actual = {{10, sl1}}; // parent
(10, sl1); // parent
VectorValues expected = map_list_of<Key, Vector> VectorValues expected = {
(1, Vector2(-3.1,-3.4)) {1, Vector2(-3.1, -3.4)}, {2, Vector2(-11.9, -13.2)}, {10, sl1}};
(2, Vector2(-11.9,-13.2))
(10, sl1);
// verify indices/size // verify indices/size
EXPECT_LONGS_EQUAL(3, (long)cg.size()); EXPECT_LONGS_EQUAL(3, (long)cg.size());
@ -330,13 +313,10 @@ TEST( GaussianConditional, solveTranspose ) {
// 2 = 1 2 // 2 = 1 2
// 5 1 1 3 // 5 1 1 3
VectorValues VectorValues x = {{1, (Vector(1) << 2.).finished()},
x = map_list_of<Key, Vector> {2, (Vector(1) << 5.).finished()}},
(1, (Vector(1) << 2.).finished()) y = {{1, (Vector(1) << 2.).finished()},
(2, (Vector(1) << 5.).finished()), {2, (Vector(1) << 3.).finished()}};
y = map_list_of<Key, Vector>
(1, (Vector(1) << 2.).finished())
(2, (Vector(1) << 3.).finished());
// test functional version // test functional version
VectorValues actual = cbn.backSubstituteTranspose(x); 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 Vector2 b(20, 40), x0(1, 2), x1(3, 4), x2(5, 6);
const double sigma = 3; 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 = auto conditional1 =
GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma); GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);

View File

@ -802,9 +802,9 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) {
Implicit9& implicitSchurFactor = Implicit9& implicitSchurFactor =
dynamic_cast<Implicit9&>(*gaussianImplicitSchurFactor); dynamic_cast<Implicit9&>(*gaussianImplicitSchurFactor);
VectorValues x = map_list_of(c1, VectorValues x{
(Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished())(c2, {c1, (Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished()},
(Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished()); {c2, (Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished()}};
VectorValues yExpected, yActual; VectorValues yExpected, yActual;
double alpha = 1.0; double alpha = 1.0;