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

use emplace_shared

release/4.3a0
Frank Dellaert 2023-02-06 00:40:32 -08:00
parent eb37866e92
commit 7afccbc446
21 changed files with 218 additions and 222 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/base/tests/testKruskal.cpp
View File

@ -37,12 +37,12 @@ gtsam::GaussianFactorGraph makeTestGaussianFactorGraph() {
Matrix I = I_2x2;
Vector2 b(0, 0);
const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
gfg += JacobianFactor(X(1), I, X(2), I, b, model);
gfg += JacobianFactor(X(1), I, X(3), I, b, model);
gfg += JacobianFactor(X(1), I, X(4), I, b, model);
gfg += JacobianFactor(X(2), I, X(3), I, b, model);
gfg += JacobianFactor(X(2), I, X(4), I, b, model);
gfg += JacobianFactor(X(3), I, X(4), I, b, model);
gfg.emplace_shared<JacobianFactor>(X(1), I, X(2), I, b, model);
gfg.emplace_shared<JacobianFactor>(X(1), I, X(3), I, b, model);
gfg.emplace_shared<JacobianFactor>(X(1), I, X(4), I, b, model);
gfg.emplace_shared<JacobianFactor>(X(2), I, X(3), I, b, model);
gfg.emplace_shared<JacobianFactor>(X(2), I, X(4), I, b, model);
gfg.emplace_shared<JacobianFactor>(X(3), I, X(4), I, b, model);
return gfg;
}
@ -53,12 +53,12 @@ gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() {
NonlinearFactorGraph nfg;
const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
nfg += BetweenFactor(X(1), X(2), Rot3(), model);
nfg += BetweenFactor(X(1), X(3), Rot3(), model);
nfg += BetweenFactor(X(1), X(4), Rot3(), model);
nfg += BetweenFactor(X(2), X(3), Rot3(), model);
nfg += BetweenFactor(X(2), X(4), Rot3(), model);
nfg += BetweenFactor(X(3), X(4), Rot3(), model);
nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(2), Rot3(), model);
nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(3), Rot3(), model);
nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(4), Rot3(), model);
nfg.emplace_shared<BetweenFactor<Rot3>>(X(2), X(3), Rot3(), model);
nfg.emplace_shared<BetweenFactor<Rot3>>(X(2), X(4), Rot3(), model);
nfg.emplace_shared<BetweenFactor<Rot3>>(X(3), X(4), Rot3(), model);
return nfg;
}

26
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -277,15 +277,15 @@ TEST( GaussianBayesNet, backSubstituteTransposeNoisy )
TEST( GaussianBayesNet, DeterminantTest )
{
GaussianBayesNet cbn;
cbn += GaussianConditional(
cbn.emplace_shared<GaussianConditional>(
0, Vector2(3.0, 4.0), (Matrix2() << 1.0, 3.0, 0.0, 4.0).finished(),
1, (Matrix2() << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
cbn += GaussianConditional(
cbn.emplace_shared<GaussianConditional>(
1, Vector2(5.0, 6.0), (Matrix2() << 1.0, 1.0, 0.0, 3.0).finished(),
2, (Matrix2() << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
cbn += GaussianConditional(
cbn.emplace_shared<GaussianConditional>(
3, Vector2(7.0, 8.0), (Matrix2() << 1.0, 1.0, 0.0, 5.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
double expectedDeterminant = 60.0 / 64.0;
@ -308,22 +308,22 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
// Create an arbitrary Bayes Net
GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
gbn.emplace_shared<GaussianConditional>(
0, Vector2(1.0,2.0), (Matrix2() << 3.0,4.0,0.0,6.0).finished(),
3, (Matrix2() << 7.0,8.0,9.0,10.0).finished(),
4, (Matrix2() << 11.0,12.0,13.0,14.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, (Matrix2() << 11.0,12.0,13.0,14.0).finished());
gbn.emplace_shared<GaussianConditional>(
1, Vector2(15.0,16.0), (Matrix2() << 17.0,18.0,0.0,20.0).finished(),
2, (Matrix2() << 21.0,22.0,23.0,24.0).finished(),
4, (Matrix2() << 25.0,26.0,27.0,28.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, (Matrix2() << 25.0,26.0,27.0,28.0).finished());
gbn.emplace_shared<GaussianConditional>(
2, Vector2(29.0,30.0), (Matrix2() << 31.0,32.0,0.0,34.0).finished(),
3, (Matrix2() << 35.0,36.0,37.0,38.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
3, (Matrix2() << 35.0,36.0,37.0,38.0).finished());
gbn.emplace_shared<GaussianConditional>(
3, Vector2(39.0,40.0), (Matrix2() << 41.0,42.0,0.0,44.0).finished(),
4, (Matrix2() << 45.0,46.0,47.0,48.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished()));
4, (Matrix2() << 45.0,46.0,47.0,48.0).finished());
gbn.emplace_shared<GaussianConditional>(
4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished());
// Compute the Hessian numerically
Matrix hessian = numericalHessian<Vector10>(

25
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -299,10 +299,10 @@ TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) {
GaussianFactorGraph fg;
Key x1 = 2, x2 = 0, l1 = 1;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2);
fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
fg.emplace_shared<JacobianFactor>(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
fg.emplace_shared<JacobianFactor>(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2);
fg.emplace_shared<JacobianFactor>(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
fg.emplace_shared<JacobianFactor>(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
// create corresponding Bayes tree:
std::shared_ptr<gtsam::GaussianBayesTree> bt = fg.eliminateMultifrontal();
@ -327,15 +327,14 @@ TEST(GaussianBayesTree, LogDeterminant) {
GaussianFactorGraph fg;
Key x1 = X(1), x2 = X(2), l1 = L(1);
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2);
fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0),
unit2);
fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
fg +=
JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
fg += JacobianFactor(x3, 10 * I_2x2, x2, -10 * I_2x2, Vector2(2.0, -1.0),
unit2);
fg += JacobianFactor(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2);
fg.emplace_shared<JacobianFactor>(x1, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2);
fg.emplace_shared<JacobianFactor>(x2, 10 * I_2x2, x1, -10 * I_2x2,
Vector2(2.0, -1.0), unit2);
fg.emplace_shared<JacobianFactor>(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
fg.emplace_shared<JacobianFactor>(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
fg.emplace_shared<JacobianFactor>(x3, 10 * I_2x2, x2, -10 * I_2x2,
Vector2(2.0, -1.0), unit2);
fg.emplace_shared<JacobianFactor>(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2);
// create corresponding Bayes net and Bayes tree:
std::shared_ptr<gtsam::GaussianBayesNet> bn = fg.eliminateSequential();

17
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -51,11 +51,10 @@ TEST(GaussianFactorGraph, initialization) {
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
fg +=
JacobianFactor(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2),
JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2),
JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2),
JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
fg.emplace_shared<JacobianFactor>(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2);
fg.emplace_shared<JacobianFactor>(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2);
fg.emplace_shared<JacobianFactor>(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2);
fg.emplace_shared<JacobianFactor>(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
EXPECT_LONGS_EQUAL(4, (long)fg.size());
@ -186,13 +185,13 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() {
Key x1 = 2, x2 = 0, l1 = 1;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
fg.emplace_shared<JacobianFactor>(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2);
fg.emplace_shared<JacobianFactor>(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
fg.emplace_shared<JacobianFactor>(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
fg.emplace_shared<JacobianFactor>(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
return fg;
}

4
gtsam/linear/tests/testRegularHessianFactor.cpp
View File

@ -66,11 +66,11 @@ TEST(RegularHessianFactor, Constructors)
// Test constructor from Gaussian Factor Graph
GaussianFactorGraph gfg;
gfg += jf;
gfg.push_back(jf);
RegularHessianFactor<2> factor4(gfg);
EXPECT(assert_equal(factor, factor4));
GaussianFactorGraph gfg2;
gfg2 += factor;
gfg2.push_back(factor);
RegularHessianFactor<2> factor5(gfg);
EXPECT(assert_equal(factor, factor5));

8
tests/smallExample.h
View File

@ -274,16 +274,16 @@ inline GaussianFactorGraph createGaussianFactorGraph() {
GaussianFactorGraph fg;
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg += JacobianFactor(X(1), 10*I_2x2, -1.0*Vector::Ones(2));
fg.emplace_shared<JacobianFactor>(X(1), 10*I_2x2, -1.0*Vector::Ones(2));
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg += JacobianFactor(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0));
fg.emplace_shared<JacobianFactor>(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0));
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg += JacobianFactor(X(1), -5*I_2x2, L(1), 5*I_2x2, Vector2(0.0, 1.0));
fg.emplace_shared<JacobianFactor>(X(1), -5*I_2x2, L(1), 5*I_2x2, Vector2(0.0, 1.0));
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg += JacobianFactor(X(2), -5*I_2x2, L(1), 5*I_2x2, Vector2(-1.0, 1.5));
fg.emplace_shared<JacobianFactor>(X(2), -5*I_2x2, L(1), 5*I_2x2, Vector2(-1.0, 1.5));
return fg;
}

30
tests/testBoundingConstraint.cpp
View File

@ -144,9 +144,9 @@ TEST( testBoundingConstraint, unary_simple_optimization1) {
NonlinearFactorGraph graph;
Symbol x1('x',1);
graph += iq2D::PoseXInequality(x1, 1.0, true);
graph += iq2D::PoseYInequality(x1, 2.0, true);
graph += simulated2D::Prior(start_pt, soft_model2, x1);
graph.emplace_shared<iq2D::PoseXInequality>(x1, 1.0, true);
graph.emplace_shared<iq2D::PoseYInequality>(x1, 2.0, true);
graph.emplace_shared<simulated2D::Prior>(start_pt, soft_model2, x1);
Values initValues;
initValues.insert(x1, start_pt);
@ -165,9 +165,9 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
Point2 start_pt(2.0, 3.0);
NonlinearFactorGraph graph;
graph += iq2D::PoseXInequality(key, 1.0, false);
graph += iq2D::PoseYInequality(key, 2.0, false);
graph += simulated2D::Prior(start_pt, soft_model2, key);
graph.emplace_shared<iq2D::PoseXInequality>(key, 1.0, false);
graph.emplace_shared<iq2D::PoseYInequality>(key, 2.0, false);
graph.emplace_shared<simulated2D::Prior>(start_pt, soft_model2, key);
Values initValues;
initValues.insert(key, start_pt);
@ -224,9 +224,9 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
Symbol x1('x',1), x2('x',2);
NonlinearFactorGraph graph;
graph += simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1);
graph += simulated2D::Prior(pt2_init, soft_model2_alt, x2);
graph += iq2D::PoseMaxDistConstraint(x1, x2, 2.0);
graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityConstraint>(pt1, x1);
graph.emplace_shared<simulated2D::Prior>(pt2_init, soft_model2_alt, x2);
graph.emplace_shared<iq2D::PoseMaxDistConstraint>(x1, x2, 2.0);
Values initial_state;
initial_state.insert(x1, pt1);
@ -250,12 +250,12 @@ TEST( testBoundingConstraint, avoid_demo) {
Point2 odo(2.0, 0.0);
NonlinearFactorGraph graph;
graph += simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1);
graph += simulated2D::Odometry(odo, soft_model2_alt, x1, x2);
graph += iq2D::LandmarkAvoid(x2, l1, radius);
graph += simulated2D::equality_constraints::UnaryEqualityPointConstraint(l1_pt, l1);
graph += simulated2D::Odometry(odo, soft_model2_alt, x2, x3);
graph += simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3);
graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityConstraint>(x1_pt, x1);
graph.emplace_shared<simulated2D::Odometry>(odo, soft_model2_alt, x1, x2);
graph.emplace_shared<iq2D::LandmarkAvoid>(x2, l1, radius);
graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityPointConstraint>(l1_pt, l1);
graph.emplace_shared<simulated2D::Odometry>(odo, soft_model2_alt, x2, x3);
graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityConstraint>(x3_pt, x3);
Values init, expected;
init.insert(x1, x1_pt);

40
tests/testDoglegOptimizer.cpp
View File

@ -42,22 +42,22 @@ using symbol_shorthand::L;
TEST(DoglegOptimizer, ComputeBlend) {
// Create an arbitrary Bayes Net
GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
gbn.emplace_shared<GaussianConditional>(
0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(),
3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(),
4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished());
gbn.emplace_shared<GaussianConditional>(
1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(),
2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(),
4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished());
gbn.emplace_shared<GaussianConditional>(
2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(),
3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished());
gbn.emplace_shared<GaussianConditional>(
3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(),
4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished()));
4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished());
gbn.emplace_shared<GaussianConditional>(
4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished());
// Compute steepest descent point
VectorValues xu = gbn.optimizeGradientSearch();
@ -78,22 +78,22 @@ TEST(DoglegOptimizer, ComputeBlend) {
TEST(DoglegOptimizer, ComputeDoglegPoint) {
// Create an arbitrary Bayes Net
GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
gbn.emplace_shared<GaussianConditional>(
0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(),
3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(),
4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished());
gbn.emplace_shared<GaussianConditional>(
1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(),
2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(),
4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished());
gbn.emplace_shared<GaussianConditional>(
2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(),
3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished());
gbn.emplace_shared<GaussianConditional>(
3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(),
4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished()));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished()));
4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished());
gbn.emplace_shared<GaussianConditional>(
4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished());
// Compute dogleg point for different deltas

4
tests/testGaussianBayesTreeB.cpp
View File

@ -75,7 +75,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
double sigma3 = 0.61808;
Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished();
GaussianBayesNet expected3;
expected3 += GaussianConditional(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3);
expected3.emplace_shared<GaussianConditional>(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3);
GaussianBayesTree::sharedClique C3 = bayesTree[X(4)];
GaussianBayesNet actual3 = C3->shortcut(R);
EXPECT(assert_equal(expected3,actual3,tol));
@ -84,7 +84,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
double sigma4 = 0.661968;
Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished();
GaussianBayesNet expected4;
expected4 += GaussianConditional(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4);
expected4.emplace_shared<GaussianConditional>(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4);
GaussianBayesTree::sharedClique C4 = bayesTree[X(3)];
GaussianBayesNet actual4 = C4->shortcut(R);
EXPECT(assert_equal(expected4,actual4,tol));

8
tests/testGaussianFactorGraphB.cpp
View File

@ -427,10 +427,10 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) {
0.0, 1., 0.0,
-1.2246468e-16, 0.0, -1),
Point3(0.511832102, 8.42819594, 5.76841725)), priorModel);
factors += ProjectionFactor(Point2(333.648615, 98.61535), measModel, xC1, l32, K);
factors += ProjectionFactor(Point2(218.508, 83.8022039), measModel, xC1, l41, K);
factors += RangeFactor<Pose3,Point3>(xC1, l32, relElevation, elevationModel);
factors += RangeFactor<Pose3,Point3>(xC1, l41, relElevation, elevationModel);
factors.emplace_shared<ProjectionFactor>(Point2(333.648615, 98.61535), measModel, xC1, l32, K);
factors.emplace_shared<ProjectionFactor>(Point2(218.508, 83.8022039), measModel, xC1, l41, K);
factors.emplace_shared<RangeFactor<Pose3,Point3>>(xC1, l32, relElevation, elevationModel);
factors.emplace_shared<RangeFactor<Pose3,Point3>>(xC1, l41, relElevation, elevationModel);
// Check that sigmas are correct (i.e., unit)
GaussianFactorGraph lfg = *factors.linearize(initValues);

76
tests/testGaussianISAM2.cpp
View File

@ -74,7 +74,7 @@ ISAM2 createSlamlikeISAM2(
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
NonlinearFactorGraph newfactors;
newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.emplace_shared<BetweenFactor<Pose2>>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -93,9 +93,9 @@ ISAM2 createSlamlikeISAM2(
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
NonlinearFactorGraph newfactors;
newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
newfactors.emplace_shared<BetweenFactor<Pose2>>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
@ -116,7 +116,7 @@ ISAM2 createSlamlikeISAM2(
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
NonlinearFactorGraph newfactors;
newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.emplace_shared<BetweenFactor<Pose2>>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -135,9 +135,9 @@ ISAM2 createSlamlikeISAM2(
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
NonlinearFactorGraph newfactors;
newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.emplace_shared<BetweenFactor<Pose2>>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
@ -230,7 +230,7 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
// Check information
GaussianFactorGraph isamGraph(isam);
isamGraph += isam.roots().front()->cachedFactor_;
isamGraph.push_back(isam.roots().front()->cachedFactor_);
Matrix expectedHessian = fullgraph.linearize(isam.getLinearizationPoint())->augmentedHessian();
Matrix actualHessian = isamGraph.augmentedHessian();
expectedHessian.bottomRightCorner(1,1) = actualHessian.bottomRightCorner(1,1);
@ -249,7 +249,7 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
for (const auto& [key, clique] : isam.nodes()) {
// Compute expected gradient
GaussianFactorGraph jfg;
jfg += clique->conditional();
jfg.push_back(clique->conditional());
VectorValues expectedGradient = jfg.gradientAtZero();
// Compare with actual gradients
DenseIndex variablePosition = 0;
@ -353,7 +353,7 @@ TEST(ISAM2, clone) {
// Modify original isam
NonlinearFactorGraph factors;
factors += BetweenFactor<Pose2>(0, 10,
factors.emplace_shared<BetweenFactor<Pose2>>(0, 10,
isam.calculateEstimate<Pose2>(0).between(isam.calculateEstimate<Pose2>(10)), noiseModel::Unit::Create(3));
isam.update(factors);
@ -439,7 +439,7 @@ TEST(ISAM2, swapFactors)
NonlinearFactorGraph swapfactors;
// swapfactors += BearingRange<Pose2,Point2>(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise; // original factor
swapfactors += BearingRangeFactor<Pose2,Point2>(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise);
swapfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise);
fullgraph.push_back(swapfactors);
isam.update(swapfactors, Values(), toRemove);
}
@ -452,7 +452,7 @@ TEST(ISAM2, swapFactors)
for (const auto& [key, clique]: isam.nodes()) {
// Compute expected gradient
GaussianFactorGraph jfg;
jfg += clique->conditional();
jfg.push_back(clique->conditional());
VectorValues expectedGradient = jfg.gradientAtZero();
// Compare with actual gradients
DenseIndex variablePosition = 0;
@ -507,7 +507,7 @@ TEST(ISAM2, constrained_ordering)
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
NonlinearFactorGraph newfactors;
newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.emplace_shared<BetweenFactor<Pose2>>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -523,9 +523,9 @@ TEST(ISAM2, constrained_ordering)
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
NonlinearFactorGraph newfactors;
newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
newfactors.emplace_shared<BetweenFactor<Pose2>>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
@ -543,7 +543,7 @@ TEST(ISAM2, constrained_ordering)
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
NonlinearFactorGraph newfactors;
newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.emplace_shared<BetweenFactor<Pose2>>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -556,9 +556,9 @@ TEST(ISAM2, constrained_ordering)
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
NonlinearFactorGraph newfactors;
newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.emplace_shared<BetweenFactor<Pose2>>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
@ -576,7 +576,7 @@ TEST(ISAM2, constrained_ordering)
for (const auto& [key, clique]: isam.nodes()) {
// Compute expected gradient
GaussianFactorGraph jfg;
jfg += clique->conditional();
jfg.push_back(clique->conditional());
VectorValues expectedGradient = jfg.gradientAtZero();
// Compare with actual gradients
DenseIndex variablePosition = 0;
@ -665,9 +665,9 @@ TEST(ISAM2, marginalizeLeaves1) {
NonlinearFactorGraph factors;
factors.addPrior(0, 0.0, model);
factors += BetweenFactor<double>(0, 1, 0.0, model);
factors += BetweenFactor<double>(1, 2, 0.0, model);
factors += BetweenFactor<double>(0, 2, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(0, 1, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(1, 2, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(0, 2, 0.0, model);
Values values;
values.insert(0, 0.0);
@ -692,10 +692,10 @@ TEST(ISAM2, marginalizeLeaves2) {
NonlinearFactorGraph factors;
factors.addPrior(0, 0.0, model);
factors += BetweenFactor<double>(0, 1, 0.0, model);
factors += BetweenFactor<double>(1, 2, 0.0, model);
factors += BetweenFactor<double>(0, 2, 0.0, model);
factors += BetweenFactor<double>(2, 3, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(0, 1, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(1, 2, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(0, 2, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(2, 3, 0.0, model);
Values values;
values.insert(0, 0.0);
@ -722,15 +722,15 @@ TEST(ISAM2, marginalizeLeaves3) {
NonlinearFactorGraph factors;
factors.addPrior(0, 0.0, model);
factors += BetweenFactor<double>(0, 1, 0.0, model);
factors += BetweenFactor<double>(1, 2, 0.0, model);
factors += BetweenFactor<double>(0, 2, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(0, 1, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(1, 2, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(0, 2, 0.0, model);
factors += BetweenFactor<double>(2, 3, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(2, 3, 0.0, model);
factors += BetweenFactor<double>(3, 4, 0.0, model);
factors += BetweenFactor<double>(4, 5, 0.0, model);
factors += BetweenFactor<double>(3, 5, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(3, 4, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(4, 5, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(3, 5, 0.0, model);
Values values;
values.insert(0, 0.0);
@ -760,8 +760,8 @@ TEST(ISAM2, marginalizeLeaves4) {
NonlinearFactorGraph factors;
factors.addPrior(0, 0.0, model);
factors += BetweenFactor<double>(0, 2, 0.0, model);
factors += BetweenFactor<double>(1, 2, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(0, 2, 0.0, model);
factors.emplace_shared<BetweenFactor<double>>(1, 2, 0.0, model);
Values values;
values.insert(0, 0.0);

10
tests/testGradientDescentOptimizer.cpp
View File

@ -39,15 +39,15 @@ std::tuple<NonlinearFactorGraph, Values> generateProblem() {
// 2b. Add odometry factors
SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.2, 0.2, 0.1));
graph += BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
graph += BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph += BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph += BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2>>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2>>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2>>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2>>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add pose constraint
SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(
Vector3(0.2, 0.2, 0.1));
graph += BetweenFactor<Pose2>(5, 2, Pose2(2.0, 0.0, M_PI_2),
graph.emplace_shared<BetweenFactor<Pose2>>(5, 2, Pose2(2.0, 0.0, M_PI_2),
constraintUncertainty);
// 3. Create the data structure to hold the initialEstimate estimate to the solution

6
tests/testIterative.cpp
View File

@ -87,8 +87,8 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint )
config.insert(X(2), Pose2(1.5,0.,0.));
NonlinearFactorGraph graph;
graph += NonlinearEquality<Pose2>(X(1), pose1);
graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
graph.emplace_shared<NonlinearEquality<Pose2>>(X(1), pose1);
graph.emplace_shared<BetweenFactor<Pose2>>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
std::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config);
@ -115,7 +115,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
NonlinearFactorGraph graph;
graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
graph.emplace_shared<BetweenFactor<Pose2>>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
std::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config);

32
tests/testMarginals.cpp
View File

@ -59,8 +59,8 @@ TEST(Marginals, planarSLAMmarginals) {
SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
// create between factors to represent odometry
graph += BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise);
graph += BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2>>(x1, x2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2>>(x2, x3, odometry, odometryNoise);
/* add measurements */
// general noisemodel for measurements
@ -75,9 +75,9 @@ TEST(Marginals, planarSLAMmarginals) {
range32 = 2.0;
// create bearing/range factors
graph += BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise);
graph += BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise);
graph += BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise);
graph.emplace_shared<BearingRangeFactor<Pose2, Point2>>(x1, l1, bearing11, range11, measurementNoise);
graph.emplace_shared<BearingRangeFactor<Pose2, Point2>>(x2, l1, bearing21, range21, measurementNoise);
graph.emplace_shared<BearingRangeFactor<Pose2, Point2>>(x3, l2, bearing32, range32, measurementNoise);
// linearization point for marginals
Values soln;
@ -195,9 +195,9 @@ TEST(Marginals, planarSLAMmarginals) {
TEST(Marginals, order) {
NonlinearFactorGraph fg;
fg.addPrior(0, Pose2(), noiseModel::Unit::Create(3));
fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3));
fg += BetweenFactor<Pose2>(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3));
fg += BetweenFactor<Pose2>(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3));
fg.emplace_shared<BetweenFactor<Pose2>>(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3));
fg.emplace_shared<BetweenFactor<Pose2>>(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3));
fg.emplace_shared<BetweenFactor<Pose2>>(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3));
Values vals;
vals.insert(0, Pose2());
@ -208,31 +208,31 @@ TEST(Marginals, order) {
vals.insert(100, Point2(0,1));
vals.insert(101, Point2(1,1));
fg += BearingRangeFactor<Pose2,Point2>(0, 100,
fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(0, 100,
vals.at<Pose2>(0).bearing(vals.at<Point2>(100)),
vals.at<Pose2>(0).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
fg += BearingRangeFactor<Pose2,Point2>(0, 101,
fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(0, 101,
vals.at<Pose2>(0).bearing(vals.at<Point2>(101)),
vals.at<Pose2>(0).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
fg += BearingRangeFactor<Pose2,Point2>(1, 100,
fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(1, 100,
vals.at<Pose2>(1).bearing(vals.at<Point2>(100)),
vals.at<Pose2>(1).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
fg += BearingRangeFactor<Pose2,Point2>(1, 101,
fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(1, 101,
vals.at<Pose2>(1).bearing(vals.at<Point2>(101)),
vals.at<Pose2>(1).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
fg += BearingRangeFactor<Pose2,Point2>(2, 100,
fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(2, 100,
vals.at<Pose2>(2).bearing(vals.at<Point2>(100)),
vals.at<Pose2>(2).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
fg += BearingRangeFactor<Pose2,Point2>(2, 101,
fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(2, 101,
vals.at<Pose2>(2).bearing(vals.at<Point2>(101)),
vals.at<Pose2>(2).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
fg += BearingRangeFactor<Pose2,Point2>(3, 100,
fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(3, 100,
vals.at<Pose2>(3).bearing(vals.at<Point2>(100)),
vals.at<Pose2>(3).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
fg += BearingRangeFactor<Pose2,Point2>(3, 101,
fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(3, 101,
vals.at<Pose2>(3).bearing(vals.at<Point2>(101)),
vals.at<Pose2>(3).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));

44
tests/testNonlinearEquality.cpp
View File

@ -193,11 +193,10 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
Symbol key1('x', 1);
Pose2 feasible1(1.0, 2.0, 3.0);
double error_gain = 500.0;
PoseNLE nle(key1, feasible1, error_gain);
// add to a graph
NonlinearFactorGraph graph;
graph += nle;
graph.emplace_shared<PoseNLE>(key1, feasible1, error_gain);
// initialize away from the ideal
Pose2 initPose(0.0, 2.0, 3.0);
@ -227,16 +226,13 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
Pose2 initPose(0.0, 2.0, 3.0);
init.insert(key1, initPose);
// add nle to a graph
double error_gain = 500.0;
PoseNLE nle(key1, feasible1, error_gain);
// create a soft prior that conflicts
PosePrior prior(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1));
// add to a graph
NonlinearFactorGraph graph;
graph += nle;
graph += prior;
graph.emplace_shared<PoseNLE>(key1, feasible1, error_gain);
// add a soft prior that conflicts
graph.emplace_shared<PosePrior>(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1));
// optimize
Ordering ordering;
@ -440,17 +436,17 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
Symbol x1('x', 1), x2('x', 2);
Symbol l1('l', 1), l2('l', 2);
Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0);
graph += eq2D::UnaryEqualityConstraint(pt_x1, x1);
graph += eq2D::UnaryEqualityConstraint(pt_x2, x2);
graph.emplace_shared<eq2D::UnaryEqualityConstraint>(pt_x1, x1);
graph.emplace_shared<eq2D::UnaryEqualityConstraint>(pt_x2, x2);
Point2 z1(0.0, 5.0);
SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1));
graph += simulated2D::Measurement(z1, sigma, x1, l1);
graph.emplace_shared<simulated2D::Measurement>(z1, sigma, x1, l1);
Point2 z2(-4.0, 0.0);
graph += simulated2D::Measurement(z2, sigma, x2, l2);
graph.emplace_shared<simulated2D::Measurement>(z2, sigma, x2, l2);
graph += eq2D::PointEqualityConstraint(l1, l2);
graph.emplace_shared<eq2D::PointEqualityConstraint>(l1, l2);
Values initialEstimate;
initialEstimate.insert(x1, pt_x1);
@ -480,20 +476,20 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
// constant constraint on x1
Point2 pose1(1.0, 1.0);
graph += eq2D::UnaryEqualityConstraint(pose1, x1);
graph.emplace_shared<eq2D::UnaryEqualityConstraint>(pose1, x1);
SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2, 0.1);
// measurement from x1 to l1
Point2 z1(0.0, 5.0);
graph += simulated2D::Measurement(z1, sigma, x1, l1);
graph.emplace_shared<simulated2D::Measurement>(z1, sigma, x1, l1);
// measurement from x2 to l2
Point2 z2(-4.0, 0.0);
graph += simulated2D::Measurement(z2, sigma, x2, l2);
graph.emplace_shared<simulated2D::Measurement>(z2, sigma, x2, l2);
// equality constraint between l1 and l2
graph += eq2D::PointEqualityConstraint(l1, l2);
graph.emplace_shared<eq2D::PointEqualityConstraint>(l1, l2);
// create an initial estimate
Values initialEstimate;
@ -542,18 +538,18 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
NonlinearFactorGraph graph;
// create equality constraints for poses
graph += NonlinearEquality<Pose3>(key_x1, leftCamera.pose());
graph += NonlinearEquality<Pose3>(key_x2, rightCamera.pose());
graph.emplace_shared<NonlinearEquality<Pose3>>(key_x1, leftCamera.pose());
graph.emplace_shared<NonlinearEquality<Pose3>>(key_x2, rightCamera.pose());
// create factors
SharedDiagonal vmodel = noiseModel::Unit::Create(2);
graph += GenericProjectionFactor<Pose3, Point3, Cal3_S2>(
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
leftCamera.project(landmark), vmodel, key_x1, key_l1, shK);
graph += GenericProjectionFactor<Pose3, Point3, Cal3_S2>(
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
rightCamera.project(landmark), vmodel, key_x2, key_l2, shK);
// add equality constraint saying there is only one point
graph += NonlinearEquality2<Point3>(key_l1, key_l2);
graph.emplace_shared<NonlinearEquality2<Point3>>(key_l1, key_l2);
// create initial data
Point3 landmark1(0.5, 5, 0);

4
tests/testNonlinearFactorGraph.cpp
View File

@ -162,8 +162,8 @@ TEST( NonlinearFactorGraph, rekey )
// updated measurements
Point2 z3(0, -1), z4(-1.5, -1.);
SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
expRekey += simulated2D::Measurement(z3, sigma0_2, X(1), L(4));
expRekey += simulated2D::Measurement(z4, sigma0_2, X(2), L(4));
expRekey.emplace_shared<simulated2D::Measurement>(z3, sigma0_2, X(1), L(4));
expRekey.emplace_shared<simulated2D::Measurement>(z4, sigma0_2, X(2), L(4));
EXPECT(assert_equal(expRekey, actRekey));
}

14
tests/testNonlinearISAM.cpp
View File

@ -36,7 +36,7 @@ TEST(testNonlinearISAM, markov_chain ) {
// create initial graph
Pose2 cur_pose; // start at origin
NonlinearFactorGraph start_factors;
start_factors += NonlinearEquality<Pose2>(0, cur_pose);
start_factors.emplace_shared<NonlinearEquality<Pose2>>(0, cur_pose);
Values init;
Values expected;
@ -50,7 +50,7 @@ TEST(testNonlinearISAM, markov_chain ) {
Pose2 z(1.0, 2.0, 0.1);
for (size_t i=1; i<=nrPoses; ++i) {
NonlinearFactorGraph new_factors;
new_factors += BetweenFactor<Pose2>(i-1, i, z, model);
new_factors.emplace_shared<BetweenFactor<Pose2>>(i-1, i, z, model);
Values new_init;
cur_pose = cur_pose.compose(z);
@ -84,7 +84,7 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) {
// create initial graph
Pose2 cur_pose; // start at origin
NonlinearFactorGraph start_factors;
start_factors += NonlinearEquality<Pose2>(0, cur_pose);
start_factors.emplace_shared<NonlinearEquality<Pose2>>(0, cur_pose);
Values init;
Values expected;
@ -106,7 +106,7 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) {
Pose2 z(1.0, 2.0, 0.1);
for (size_t i=1; i<=nrPoses; ++i) {
NonlinearFactorGraph new_factors;
new_factors += BetweenFactor<Pose2>(i-1, i, z, model3);
new_factors.emplace_shared<BetweenFactor<Pose2>>(i-1, i, z, model3);
Values new_init;
cur_pose = cur_pose.compose(z);
@ -161,7 +161,7 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) {
// create initial graph
Pose2 cur_pose; // start at origin
NonlinearFactorGraph start_factors;
start_factors += NonlinearEquality<Pose2>(0, cur_pose);
start_factors.emplace_shared<NonlinearEquality<Pose2>>(0, cur_pose);
Values init;
Values expected;
@ -183,7 +183,7 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) {
Pose2 z(1.0, 2.0, 0.1);
for (size_t i=1; i<=nrPoses; ++i) {
NonlinearFactorGraph new_factors;
new_factors += BetweenFactor<Pose2>(i-1, i, z, model3);
new_factors.emplace_shared<BetweenFactor<Pose2>>(i-1, i, z, model3);
Values new_init;
cur_pose = cur_pose.compose(z);
@ -204,7 +204,7 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) {
// Reconnect with observation later
if (i == 15) {
new_factors += BearingRangeFactor<Pose2, Point2>(
new_factors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(
i, lm1, cur_pose.bearing(landmark1), cur_pose.range(landmark1), model2);
}

28
tests/testNonlinearOptimizer.cpp
View File

@ -190,7 +190,7 @@ TEST( NonlinearOptimizer, Factorization )
NonlinearFactorGraph graph;
graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
graph.emplace_shared<BetweenFactor<Pose2>>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
Ordering ordering;
ordering.push_back(X(1));
@ -249,9 +249,9 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
NonlinearFactorGraph fg;
fg.addPrior(0, Pose2(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1));
fg += BetweenFactor<Pose2>(0, 1, Pose2(1, 0, M_PI / 2),
fg.emplace_shared<BetweenFactor<Pose2>>(0, 1, Pose2(1, 0, M_PI / 2),
noiseModel::Isotropic::Sigma(3, 1));
fg += BetweenFactor<Pose2>(1, 2, Pose2(1, 0, M_PI / 2),
fg.emplace_shared<BetweenFactor<Pose2>>(1, 2, Pose2(1, 0, M_PI / 2),
noiseModel::Isotropic::Sigma(3, 1));
Values init;
@ -352,10 +352,10 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) {
NonlinearFactorGraph fg;
fg.addPrior(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1));
fg += BetweenFactor<Pose2>(0, 1, Pose2(1,1.1,M_PI/4),
fg.emplace_shared<BetweenFactor<Pose2>>(0, 1, Pose2(1,1.1,M_PI/4),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0),
noiseModel::Isotropic::Sigma(3,1)));
fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0.9,M_PI/2),
fg.emplace_shared<BetweenFactor<Pose2>>(0, 1, Pose2(1,0.9,M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0),
noiseModel::Isotropic::Sigma(3,1)));
@ -383,13 +383,13 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) {
NonlinearFactorGraph fg;
fg.addPrior(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01));
fg += BetweenFactor<Point2>(0, 1, Point2(1,1.8),
fg.emplace_shared<BetweenFactor<Point2>>(0, 1, Point2(1,1.8),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
noiseModel::Isotropic::Sigma(2,1)));
fg += BetweenFactor<Point2>(0, 1, Point2(1,0.9),
fg.emplace_shared<BetweenFactor<Point2>>(0, 1, Point2(1,0.9),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
noiseModel::Isotropic::Sigma(2,1)));
fg += BetweenFactor<Point2>(0, 1, Point2(1,90),
fg.emplace_shared<BetweenFactor<Point2>>(0, 1, Point2(1,90),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
noiseModel::Isotropic::Sigma(2,1)));
@ -417,16 +417,16 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) {
NonlinearFactorGraph fg;
fg.addPrior(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, M_PI/2),
fg.emplace_shared<BetweenFactor<Pose2>>(0, 1, Pose2(0,9, M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 11, M_PI/2),
fg.emplace_shared<BetweenFactor<Pose2>>(0, 1, Pose2(0, 11, M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 10, M_PI/2),
fg.emplace_shared<BetweenFactor<Pose2>>(0, 1, Pose2(0, 10, M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, 0),
fg.emplace_shared<BetweenFactor<Pose2>>(0, 1, Pose2(0,9, 0),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
@ -495,7 +495,7 @@ TEST(NonlinearOptimizer, disconnected_graph) {
NonlinearFactorGraph graph;
graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1));
graph.emplace_shared<BetweenFactor<Pose2>>(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1));
graph.addPrior(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(graph, init).optimize()));
@ -541,7 +541,7 @@ TEST(NonlinearOptimizer, subclass_solver) {
NonlinearFactorGraph graph;
graph.addPrior(X(1), Pose2(0., 0., 0.), noiseModel::Isotropic::Sigma(3, 1));
graph += BetweenFactor<Pose2>(X(1), X(2), Pose2(1.5, 0., 0.),
graph.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.5, 0., 0.),
noiseModel::Isotropic::Sigma(3, 1));
graph.addPrior(X(3), Pose2(3., 0., 0.), noiseModel::Isotropic::Sigma(3, 1));

14
tests/testPCGSolver.cpp
View File

@ -84,13 +84,13 @@ TEST( GaussianFactorGraphSystem, multiply_getb)
// Create a Gaussian Factor Graph
GaussianFactorGraph simpleGFG;
SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3));
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2);
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2);
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
// Create a dummy-preconditioner and a GaussianFactorGraphSystem
DummyPreconditioner dummyPreconditioner;

16
tests/testPreconditioner.cpp
View File

@ -39,7 +39,7 @@ TEST( PCGsolver, verySimpleLinearSystem) {
// Create a Gaussian Factor Graph
GaussianFactorGraph simpleGFG;
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2));
simpleGFG.emplace_shared<JacobianFactor>(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2));
// Exact solution already known
VectorValues exactSolution;
@ -81,13 +81,13 @@ TEST(PCGSolver, simpleLinearSystem) {
GaussianFactorGraph simpleGFG;
//SharedDiagonal unit2 = noiseModel::Unit::Create(2);
SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3));
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2);
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2);
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG.emplace_shared<JacobianFactor>(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
//simpleGFG.print("system");
// Expected solution

10
tests/testRot3Optimization.cpp
View File

@ -31,6 +31,8 @@ using namespace gtsam;
typedef BetweenFactor<Rot3> Between;
typedef NonlinearFactorGraph Graph;
using symbol_shorthand::R;
/* ************************************************************************* */
TEST(Rot3, optimize) {
@ -38,11 +40,11 @@ TEST(Rot3, optimize) {
Values truth;
Values initial;
Graph fg;
fg.addPrior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01));
fg.addPrior(R(0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01));
for(int j=0; j<6; ++j) {
truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j)));
initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
fg += Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), noiseModel::Isotropic::Sigma(3, 0.01));
truth.insert(R(j), Rot3::Rz(M_PI/3.0 * double(j)));
initial.insert(R(j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
fg.emplace_shared<Between>(R(j), R((j+1)%6), Rot3::Rz(M_PI/3.0), noiseModel::Isotropic::Sigma(3, 0.01));
}
Values final = GaussNewtonOptimizer(fg, initial).optimize();