diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index d2c3e20fa..4d26bcc63 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -356,12 +356,8 @@ namespace gtsam { this->noiseModel_->WhitenInPlace(A1); this->noiseModel_->WhitenInPlace(A2); this->noiseModel_->whitenInPlace(b); - if(var1 < var2) - return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, - A2, b, noiseModel::Unit::Create(b.size()))); - else - return GaussianFactor::shared_ptr(new JacobianFactor(var2, A2, var1, - A1, b, noiseModel::Unit::Create(b.size()))); + return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, + A2, b, noiseModel::Unit::Create(b.size()))); } /** @@ -370,10 +366,7 @@ namespace gtsam { */ virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { const Index var1 = ordering[key1_], var2 = ordering[key2_]; - if(var1 < var2) - return IndexFactor::shared_ptr(new IndexFactor(var1, var2)); - else - return IndexFactor::shared_ptr(new IndexFactor(var2, var1)); + return IndexFactor::shared_ptr(new IndexFactor(var1, var2)); } /** methods to retrieve both keys */ @@ -502,28 +495,8 @@ namespace gtsam { this->noiseModel_->WhitenInPlace(A2); this->noiseModel_->WhitenInPlace(A3); this->noiseModel_->whitenInPlace(b); - if(var1 < var2 && var2 < var3) - return GaussianFactor::shared_ptr( - new JacobianFactor(var1, A1, var2, A2, var3, A3, b, noiseModel::Unit::Create(b.size()))); - else if(var2 < var1 && var1 < var3) - return GaussianFactor::shared_ptr( - new JacobianFactor(var2, A2, var1, A1, var3, A3, b, noiseModel::Unit::Create(b.size()))); - else if(var1 < var3 && var3 < var2) - return GaussianFactor::shared_ptr( - new JacobianFactor(var1, A1, var3, A3, var2, A2, b, noiseModel::Unit::Create(b.size()))); - else if(var2 < var3 && var3 < var1) - return GaussianFactor::shared_ptr( - new JacobianFactor(var2, A2, var3, A3, var1, A1, b, noiseModel::Unit::Create(b.size()))); - else if(var3 < var1 && var1 < var2) - return GaussianFactor::shared_ptr( - new JacobianFactor(var3, A3, var1, A1, var2, A2, b, noiseModel::Unit::Create(b.size()))); - else if(var3 < var2 && var2 < var1) - return GaussianFactor::shared_ptr( - new JacobianFactor(var3, A3, var2, A2, var1, A1, b, noiseModel::Unit::Create(b.size()))); - else { - assert(false); - return GaussianFactor::shared_ptr(); - } + return GaussianFactor::shared_ptr( + new JacobianFactor(var1, A1, var2, A2, var3, A3, b, noiseModel::Unit::Create(b.size()))); } /** @@ -532,22 +505,7 @@ namespace gtsam { */ virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_]; - if(var1 < var2 && var2 < var3) - return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key2_], ordering[key3_])); - else if(var2 < var1 && var1 < var3) - return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key1_], ordering[key3_])); - else if(var1 < var3 && var3 < var2) - return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key3_], ordering[key2_])); - else if(var2 < var3 && var3 < var1) - return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key3_], ordering[key1_])); - else if(var3 < var1 && var1 < var2) - return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key1_], ordering[key2_])); - else if(var3 < var2 && var2 < var1) - return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key2_], ordering[key1_])); - else { - assert(false); - return GaussianFactor::shared_ptr(); - } + return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key2_], ordering[key3_])); } /** methods to retrieve keys */ diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 17b3342b3..c387ed469 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -144,22 +144,13 @@ namespace example { fg.add(ordering["x1"], 10*eye(2), -1.0*ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - if(ordering["x1"] < ordering["x2"]) - fg.add(ordering["x1"], -10*eye(2),ordering["x2"], 10*eye(2), Vector_(2, 2.0, -1.0), unit2); - else - fg.add(ordering["x2"], 10*eye(2),ordering["x1"], -10*eye(2), Vector_(2, 2.0, -1.0), unit2); + fg.add(ordering["x1"], -10*eye(2),ordering["x2"], 10*eye(2), Vector_(2, 2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - if(ordering["x1"] < ordering["l1"]) - fg.add(ordering["x1"], -5*eye(2), ordering["l1"], 5*eye(2), Vector_(2, 0.0, 1.0), unit2); - else - fg.add(ordering["l1"], 5*eye(2), ordering["x1"], -5*eye(2), Vector_(2, 0.0, 1.0), unit2); + fg.add(ordering["x1"], -5*eye(2), ordering["l1"], 5*eye(2), Vector_(2, 0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - if(ordering["x2"] < ordering["l1"]) - fg.add(ordering["x2"], -5*eye(2), ordering["l1"], 5*eye(2), Vector_(2, -1.0, 1.5), unit2); - else - fg.add(ordering["l1"], 5*eye(2), ordering["x2"], -5*eye(2), Vector_(2, -1.0, 1.5), unit2); + fg.add(ordering["x2"], -5*eye(2), ordering["l1"], 5*eye(2), Vector_(2, -1.0, 1.5), unit2); return *fg.dynamicCastFactors >(); } diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraph.cpp index eb951fc16..3c9a5667a 100644 --- a/tests/testSymbolicFactorGraph.cpp +++ b/tests/testSymbolicFactorGraph.cpp @@ -42,7 +42,7 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) expected.push_factor(o["x1"]); expected.push_factor(o["x1"],o["x2"]); expected.push_factor(o["x1"],o["l1"]); - expected.push_factor(o["l1"],o["x2"]); + expected.push_factor(o["x2"],o["l1"]); // construct it from the factor graph GaussianFactorGraph factorGraph = createGaussianFactorGraph(o);