12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Use initializer lists in tests and gtsam_unstable

release/4.3a0
Frank Dellaert 2023-01-07 23:59:39 -08:00
parent 9b5321ce03
commit d3a40fbc71
23 changed files with 168 additions and 223 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -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;
{

2
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -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;

8
gtsam/symbolic/SymbolicFactor.h
View File

@ -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<class CONTAINER>
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<class CONTAINER>
static SymbolicFactor::shared_ptr FromKeysShared(const CONTAINER& keys) {
return FromIteratorsShared(keys.begin(), keys.end());

13
gtsam_unstable/base/tests/testBTree.cpp
View File

@ -17,12 +17,12 @@
*/
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp> // for +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/base/BTree.h>
#include <list>
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<KeyInt> expected, actual;
expected += p1,p2,p3,p4,p5;
auto expected = std::list<KeyInt> {p1, p2, p3, p4, p5};
std::list<KeyInt> 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)

43
gtsam_unstable/base/tests/testDSF.cpp
View File

@ -20,10 +20,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/set.hpp>
using namespace boost::assign;
#include <iostream>
using namespace std;
@ -88,7 +84,7 @@ TEST(DSF, makePair) {
/* ************************************************************************* */
TEST(DSF, makeList) {
DSFInt dsf;
list<int> keys; keys += 5, 6, 7;
list<int> keys{5, 6, 7};
dsf = dsf.makeList(keys);
EXPECT(dsf.findSet(5) == dsf.findSet(7));
}
@ -112,7 +108,7 @@ TEST(DSF, sets) {
map<int, set<int> > sets = dsf.sets();
LONGS_EQUAL(1, sets.size());
set<int> expected; expected += 5, 6;
set<int> expected{5, 6};
EXPECT(expected == sets[dsf.findSet(5)]);
}
@ -127,7 +123,7 @@ TEST(DSF, sets2) {
map<int, set<int> > sets = dsf.sets();
LONGS_EQUAL(1, sets.size());
set<int> expected; expected += 5, 6, 7;
set<int> expected{5, 6, 7};
EXPECT(expected == sets[dsf.findSet(5)]);
}
@ -141,7 +137,7 @@ TEST(DSF, sets3) {
map<int, set<int> > sets = dsf.sets();
LONGS_EQUAL(2, sets.size());
set<int> expected; expected += 5, 6;
set<int> 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<int> keys; keys += 5;
list<int> keys{5};
map<int, set<int> > partitions = dsf.partition(keys);
LONGS_EQUAL(1, partitions.size());
set<int> expected; expected += 5;
set<int> 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<int> keys; keys += 7;
list<int> keys{7};
map<int, set<int> > partitions = dsf.partition(keys);
LONGS_EQUAL(1, partitions.size());
set<int> expected; expected += 7;
set<int> 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<int> keys; keys += 5, 7;
list<int> keys{5, 7};
map<int, set<int> > partitions = dsf.partition(keys);
LONGS_EQUAL(2, partitions.size());
set<int> expected; expected += 5;
set<int> expected{5};
EXPECT(expected == partitions[dsf.findSet(5)]);
}
@ -202,7 +198,7 @@ TEST(DSF, set) {
set<int> set = dsf.set(5);
LONGS_EQUAL(2, set.size());
std::set<int> expected; expected += 5, 6;
std::set<int> expected{5, 6};
EXPECT(expected == set);
}
@ -217,7 +213,7 @@ TEST(DSF, set2) {
set<int> set = dsf.set(5);
LONGS_EQUAL(3, set.size());
std::set<int> expected; expected += 5, 6, 7;
std::set<int> 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<string> keys; keys += x1,x2,x3,x4;
list<string> keys{x1, x2, x3, x4};
DSF<string> 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<Measurement> measurements;
measurements += m11,m12,m14, m22,m23,m25,m26;
list<Measurement> measurements{m11, m12, m14, m22, m23, m25, m26};
// Create some "matches"
typedef pair<Measurement,Measurement> Match;
list<Match> matches;
matches += Match(m11,m22), Match(m12,m23), Match(m14,m25), Match(m14,m26);
list<Match> matches{Match(m11, m22), Match(m12, m23), Match(m14, m25),
Match(m14, m26)};
// Merge matches
DSF<Measurement> dsf(measurements);
@ -310,15 +305,15 @@ TEST(DSF, mergePairwiseMatches) {
// Check that we have three connected components
EXPECT_LONGS_EQUAL(3, dsf.numSets());
set<Measurement> expected1; expected1 += m11,m22;
set<Measurement> expected1{m11,m22};
set<Measurement> actual1 = dsf.set(m11);
EXPECT(expected1 == actual1);
set<Measurement> expected2; expected2 += m12,m23;
set<Measurement> expected2{m12,m23};
set<Measurement> actual2 = dsf.set(m12);
EXPECT(expected2 == actual2);
set<Measurement> expected3; expected3 += m14,m25,m26;
set<Measurement> expected3{m14,m25,m26};
set<Measurement> actual3 = dsf.set(m14);
EXPECT(expected3 == actual3);
}

6
gtsam_unstable/discrete/examples/schedulingExample.cpp
View File

@ -214,8 +214,7 @@ void sampleSolutions() {
vector<DiscreteBayesNet::shared_ptr> samplers(7);
// Given the time-slots, we can create 7 independent samplers
vector<size_t> slots;
slots += 16, 17, 11, 2, 0, 5, 9; // given slots
vector<size_t> 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<size_t> slots;
slots += 16, 17, 11, 2, 0, 5, 9, 14;
vector<size_t> slots{16, 17, 11, 2, 0, 5, 9, 14};
vector<double> slotsAvailable(scheduler.nrTimeSlots(), 1.0);
for(size_t s: slots)
slotsAvailable[s] = 0;

3
gtsam_unstable/discrete/examples/schedulingQuals12.cpp
View File

@ -223,8 +223,7 @@ void sampleSolutions() {
vector<DiscreteBayesNet::shared_ptr> samplers(NRSTUDENTS);
// Given the time-slots, we can create NRSTUDENTS independent samplers
vector<size_t> slots;
slots += 3, 20, 2, 6, 5, 11, 1, 4; // given slots
vector<size_t> 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);

3
gtsam_unstable/discrete/examples/schedulingQuals13.cpp
View File

@ -248,8 +248,7 @@ void sampleSolutions() {
vector<DiscreteBayesNet::shared_ptr> samplers(NRSTUDENTS);
// Given the time-slots, we can create NRSTUDENTS independent samplers
vector<size_t> slots;
slots += 12,11,13, 21,16,1, 3,2,6, 7,22,4; // given slots
vector<size_t> 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);

6
gtsam_unstable/discrete/tests/testLoopyBelief.cpp
View File

@ -11,14 +11,12 @@
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/VariableIndex.h>
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/map.hpp>
#include <fstream>
#include <iostream>
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<Key, DiscreteKey> allKeys = map_list_of(0, C)(1, S)(2, R)(3, W);
std::map<Key, DiscreteKey> allKeys{{0, C}, {1, S}, {2, R}, {3, W}};
// Build graph
DecisionTreeFactor pC(C, "0.5 0.5");

90
gtsam_unstable/linear/tests/testLinearEquality.cpp
View File

@ -15,38 +15,31 @@
* @author Duy-Nguyen Ta
**/
#include <gtsam_unstable/linear/LinearEquality.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
#include <gtsam_unstable/linear/LinearEquality.h>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
GTSAM_CONCEPT_TESTABLE_INST(LinearEquality)
namespace {
namespace simple {
// Terms we'll use
const vector<pair<Key, Matrix> > terms = list_of < pair<Key, Matrix>
> (make_pair(5, Matrix3::Identity()))(
make_pair(10, 2 * Matrix3::Identity()))(
make_pair(15, 3 * Matrix3::Identity()));
const vector<pair<Key, Matrix> > 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,26 +86,23 @@ 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;
@ -119,12 +110,14 @@ TEST(LinearEquality, error)
values.insert(10, Vector::Constant(3, 0.5));
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,12 +174,12 @@ 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();
LinearEquality lf(1, -I, 2, I, b, 0);
@ -208,7 +202,9 @@ TEST(LinearEquality, operators )
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());

36
gtsam_unstable/partition/tests/testFindSeparator.cpp
View File

@ -6,10 +6,6 @@
* Description: unit tests for FindSeparator
*/
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
#include <boost/make_shared.hpp>
#include <CppUnitLite/TestHarness.h>
@ -30,16 +26,16 @@ TEST ( Partition, separatorPartitionByMetis )
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 1, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
std::vector<size_t> keys; keys += 0, 1, 2, 3, 4;
std::vector<size_t> keys{0, 1, 2, 3, 4};
WorkSpace workspace(5);
boost::optional<MetisResult> actual = separatorPartitionByMetis<GenericGraph2D>(graph, keys,
workspace, true);
CHECK(actual.is_initialized());
vector<size_t> A_expected; A_expected += 0, 3; // frontal
vector<size_t> B_expected; B_expected += 2, 4; // frontal
vector<size_t> C_expected; C_expected += 1; // separator
vector<size_t> A_expected{0, 3}; // frontal
vector<size_t> B_expected{2, 4}; // frontal
vector<size_t> 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<GenericFactor2D>(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 5, 6;
std::vector<size_t> keys{1, 2, 3, 5, 6};
WorkSpace workspace(8);
boost::optional<MetisResult> actual = separatorPartitionByMetis<GenericGraph2D>(graph, keys,
workspace, true);
CHECK(actual.is_initialized());
vector<size_t> A_expected; A_expected += 1, 5; // frontal
vector<size_t> B_expected; B_expected += 3, 6; // frontal
vector<size_t> C_expected; C_expected += 2; // separator
vector<size_t> A_expected{1, 5}; // frontal
vector<size_t> B_expected{3, 6}; // frontal
vector<size_t> 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<GenericFactor3D>(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D));
std::vector<size_t> keys; keys += 0, 1, 2, 3;
std::vector<size_t> keys{0, 1, 2, 3};
WorkSpace workspace(6);
boost::optional<MetisResult> actual = edgePartitionByMetis<GenericGraph3D>(graph, keys,
workspace, true);
CHECK(actual.is_initialized());
vector<size_t> A_expected; A_expected += 0, 1; // frontal
vector<size_t> B_expected; B_expected += 2, 3; // frontal
vector<size_t> A_expected{0, 1}; // frontal
vector<size_t> B_expected{2, 3}; // frontal
vector<size_t> 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<GenericFactor3D>(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D, 1));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D, 20));
graph.push_back(boost::make_shared<GenericFactor3D>(3, 4, 3, NODE_POSE_3D, NODE_POSE_3D, 1));
std::vector<size_t> keys; keys += 0, 1, 2, 3, 4;
std::vector<size_t> keys{0, 1, 2, 3, 4};
WorkSpace workspace(6);
boost::optional<MetisResult> actual = edgePartitionByMetis<GenericGraph3D>(graph, keys,
workspace, true);
CHECK(actual.is_initialized());
vector<size_t> A_expected; A_expected += 0, 1; // frontal
vector<size_t> B_expected; B_expected += 2, 3, 4; // frontal
vector<size_t> A_expected{0, 1}; // frontal
vector<size_t> B_expected{2, 3, 4}; // frontal
vector<size_t> 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<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 1, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
std::vector<size_t> keys; keys += 0, 1, 2, 3, 4;
std::vector<size_t> 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<GenericFactor2D>(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 5, 6;
std::vector<size_t> keys{1, 2, 3, 5, 6};
WorkSpace workspace(8);
int minNodesPerMap = -1;

40
gtsam_unstable/partition/tests/testGenericGraph.cpp
View File

@ -10,10 +10,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
#include <boost/make_shared.hpp>
#include <map>
@ -44,13 +40,13 @@ TEST ( GenerciGraph, findIslands )
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9;
std::vector<size_t> keys{1, 2, 3, 4, 5, 6, 7, 8, 9};
WorkSpace workspace(10); // from 0 to 9
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 2, 3, 7, 8;
vector<size_t> island2; island2 += 4, 5, 6, 9;
vector<size_t> island1{1, 2, 3, 7, 8};
vector<size_t> 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<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8;
std::vector<size_t> keys{1, 2, 3, 4, 5, 6, 7, 8};
WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(1, islands.size());
vector<size_t> island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8;
vector<size_t> 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<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6;
std::vector<size_t> keys{1, 2, 3, 4, 5, 6};
WorkSpace workspace(7); // from 0 to 9
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 5;
vector<size_t> island2; island2 += 2, 3, 4, 6;
vector<size_t> island1{1, 5};
vector<size_t> 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<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
std::vector<size_t> keys; keys += 3, 4, 7;
std::vector<size_t> keys{3, 4, 7};
WorkSpace workspace(8); // from 0 to 7
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 3, 4;
vector<size_t> island2; island2 += 7;
vector<size_t> island1{3, 4};
vector<size_t> island2{7};
CHECK(island1 == islands.front());
CHECK(island2 == islands.back());
}
@ -147,13 +143,13 @@ TEST ( GenerciGraph, findIslands5 )
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5;
std::vector<size_t> keys{1, 2, 3, 4, 5};
WorkSpace workspace(6); // from 0 to 5
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 3, 5;
vector<size_t> island2; island2 += 2, 4;
vector<size_t> island1{1, 3, 5};
vector<size_t> island2{2, 4};
CHECK(island1 == islands.front());
CHECK(island2 == islands.back());
}
@ -235,8 +231,8 @@ TEST ( GenericGraph, reduceGenericGraph2 )
/* ************************************************************************* */
TEST ( GenerciGraph, hasCommonCamera )
{
std::set<size_t> cameras1; cameras1 += 1, 2, 3, 4, 5;
std::set<size_t> cameras2; cameras2 += 8, 7, 6, 5;
std::set<size_t> cameras1{1, 2, 3, 4, 5};
std::set<size_t> cameras2{8, 7, 6, 5};
bool actual = hasCommonCamera(cameras1, cameras2);
CHECK(actual);
}
@ -244,8 +240,8 @@ TEST ( GenerciGraph, hasCommonCamera )
/* ************************************************************************* */
TEST ( GenerciGraph, hasCommonCamera2 )
{
std::set<size_t> cameras1; cameras1 += 1, 3, 5, 7;
std::set<size_t> cameras2; cameras2 += 2, 4, 6, 8, 10;
std::set<size_t> cameras1{1, 3, 5, 7};
std::set<size_t> cameras2{2, 4, 6, 8, 10};
bool actual = hasCommonCamera(cameras1, cameras2);
CHECK(!actual);
}

4
gtsam_unstable/slam/BiasedGPSFactor.h
View File

@ -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_;
}

6
gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
View File

@ -23,12 +23,10 @@
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <boost/assign/std/vector.hpp>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
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<StereoCamera> cams;
cams += w_Camera_cam1;
cams += w_Camera_cam2;
CameraSet<StereoCamera> cams{w_Camera_cam1, w_Camera_cam2};
TriangulationResult result = factor1.triangulateSafe(cams);
CHECK(result);
EXPECT(assert_equal(landmark, *result, 1e-7));

6
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -24,12 +24,10 @@
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <boost/assign/std/vector.hpp>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
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<StereoCamera> cams;
cams += level_camera;
cams += level_camera_right;
CameraSet<StereoCamera> cams{level_camera, level_camera_right};
TriangulationResult result = factor1.triangulateSafe(cams);
CHECK(result);
EXPECT(assert_equal(landmark, *result, 1e-7));

19
tests/smallExample.h
View File

@ -26,7 +26,6 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/assign/list_of.hpp>
namespace gtsam {
namespace example {
@ -223,10 +222,9 @@ inline Values createValues() {
/* ************************************************************************* */
inline VectorValues createVectorValues() {
using namespace impl;
VectorValues c = boost::assign::pair_list_of<Key, Vector>
(_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<Key, Vector>
(_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<Key, Vector>
(_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;
}

16
tests/testExpressionFactor.cpp
View File

@ -27,8 +27,6 @@
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/expressions.h>
#include <boost/assign/list_of.hpp>
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<double> sum1_(Combine(1, 2), v1_, v2_);
EXPECT(sum1_.keys() == list_of(1)(2));
EXPECT((sum1_.keys() == std::set<Key>{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<double> sum2_(Combine(3, 4), sum1_, v1_);
EXPECT(sum2_.keys() == list_of(1)(2));
EXPECT((sum2_.keys() == std::set<Key>{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<double> sum3_(Combine(5, 6), sum1_, sum2_);
EXPECT(sum3_.keys() == list_of(1)(2));
EXPECT((sum3_.keys() == std::set<Key>{1, 2}));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance);
}
@ -558,19 +556,19 @@ TEST(Expression, testMultipleCompositions2) {
Expression<double> v3_(Key(3));
Expression<double> sum1_(Combine(4,5), v1_, v2_);
EXPECT(sum1_.keys() == list_of(1)(2));
EXPECT((sum1_.keys() == std::set<Key>{1, 2}));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance);
Expression<double> sum2_(combine3, v1_, v2_, v3_);
EXPECT(sum2_.keys() == list_of(1)(2)(3));
EXPECT((sum2_.keys() == std::set<Key>{1, 2, 3}));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance);
Expression<double> sum3_(combine3, v3_, v2_, v1_);
EXPECT(sum3_.keys() == list_of(1)(2)(3));
EXPECT((sum3_.keys() == std::set<Key>{1, 2, 3}));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance);
Expression<double> sum4_(combine3, sum1_, sum2_, sum3_);
EXPECT(sum4_.keys() == list_of(1)(2)(3));
EXPECT((sum4_.keys() == std::set<Key>{1, 2, 3}));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance);
}

15
tests/testGaussianBayesTreeB.cpp
View File

@ -26,9 +26,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // 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<GaussianConditional>(X(1), Z_2x1, I/sigmax7, X(7), A/sigmax7);
expected1.emplace_shared<GaussianConditional>(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<GaussianConditional>(X(1), Z_2x1, I/sig14, X(4), A14/sig14);
expected3.emplace_shared<GaussianConditional>(X(4), Z_2x1, I/sigmax4);
GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4));
EXPECT(assert_equal(expected3,actual3,tol));

30
tests/testGaussianFactorGraphB.cpp
View File

@ -27,10 +27,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
#include <boost/range/adaptor/map.hpp>
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<JacobianFactor>(X(1), An, X(2), Ap, b, sigma);
fg.emplace_shared<JacobianFactor>(X(1), Ap, b, sigma);
fg.emplace_shared<JacobianFactor>(X(2), Ap, b, sigma);
// Eliminate
Ordering ordering;

12
tests/testGaussianISAM2.cpp
View File

@ -24,9 +24,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/map.hpp>
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<Key> leafKeys = list_of(0);
std::list<Key> leafKeys {0};
EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}
@ -716,7 +714,7 @@ TEST(ISAM2, marginalizeLeaves2) {
isam.update(factors, values, FactorIndices(), constrainedKeys);
FastList<Key> leafKeys = list_of(0);
std::list<Key> leafKeys {0};
EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}
@ -755,7 +753,7 @@ TEST(ISAM2, marginalizeLeaves3) {
isam.update(factors, values, FactorIndices(), constrainedKeys);
FastList<Key> leafKeys = list_of(0);
std::list<Key> leafKeys {0};
EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}
@ -780,7 +778,7 @@ TEST(ISAM2, marginalizeLeaves4) {
isam.update(factors, values, FactorIndices(), constrainedKeys);
FastList<Key> leafKeys = list_of(1);
std::list<Key> leafKeys {1};
EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}
@ -791,7 +789,7 @@ TEST(ISAM2, marginalizeLeaves5)
ISAM2 isam = createSlamlikeISAM2();
// Marginalize
FastList<Key> marginalizeKeys = list_of(0);
std::list<Key> marginalizeKeys {0};
EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys));
}

5
tests/testGraph.cpp
View File

@ -27,8 +27,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <iostream>
@ -47,8 +45,7 @@ TEST ( Ordering, predecessorMap2Keys ) {
p_map.insert(4,3);
p_map.insert(5,1);
list<Key> expected;
expected += 4,5,3,2,1;
list<Key> expected{4, 5, 3, 2, 1};
list<Key> actual = predecessorMap2Keys<Key>(p_map);
LONGS_EQUAL((long)expected.size(), (long)actual.size());

6
tests/testSubgraphPreconditioner.cpp
View File

@ -29,10 +29,8 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/tuple/tuple.hpp>
using namespace boost::assign;
#include <fstream>
@ -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);

2
timing/timePose2.cpp
View File

@ -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);
}