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

Removed key sorting from NonlinearFactor

release/4.3a0
Richard Roberts 2011-06-04 14:54:27 +00:00
parent 5ea18829b5
commit 265b057580
3 changed files with 10 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
gtsam/nonlinear/NonlinearFactor.h
View File

@ -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 */

15
gtsam/slam/smallExample.cpp
View File

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

2
tests/testSymbolicFactorGraph.cpp
View File

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