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

Fixed bug in smoother example

release/4.3a0
Frank Dellaert 2009-11-05 08:05:34 +00:00
parent e87c19ed7a
commit bd54c39a73
1 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
cpp/smallExample.cpp
View File

@ -247,14 +247,18 @@ ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() {
/* ************************************************************************* */ /* ************************************************************************* */
LinearFactorGraph createSmoother(int T) { LinearFactorGraph createSmoother(int T) {
// noise on measurements and odometry, respectively
double sigma1 = 1, sigma2 = 1;
// Create // Create
ExampleNonlinearFactorGraph nlfg; ExampleNonlinearFactorGraph nlfg;
VectorConfig poses; VectorConfig poses;
// prior on x1 // prior on x1
Vector x1 = zero(2); Vector x1 = Vector_(2,1.0,0.0);
string key1 = symbol('x', 1); string key1 = symbol('x', 1);
shared prior(new Point2Prior(x1, 1, key1)); shared prior(new Point2Prior(x1, sigma1, key1));
nlfg.push_back(prior); nlfg.push_back(prior);
poses.insert(key1, x1); poses.insert(key1, x1);
@ -262,15 +266,16 @@ LinearFactorGraph createSmoother(int T) {
// odometry between x_t and x_{t-1} // odometry between x_t and x_{t-1}
Vector odo = Vector_(2, 1.0, 0.0); Vector odo = Vector_(2, 1.0, 0.0);
string key = symbol('x', t); string key = symbol('x', t);
shared odometry(new Simulated2DOdometry(odo, 1, symbol('x', t - 1), key)); shared odometry(new Simulated2DOdometry(odo, sigma2, symbol('x', t - 1), key));
nlfg.push_back(odometry); nlfg.push_back(odometry);
// measurement on x_t // measurement on x_t is like perfect GPS
double sigma3 = 0.2; Vector xt = Vector_(2, (double)t, 0.0);
Vector z = Vector_(2, t, 0.0); shared measurement(new Point2Prior(xt, sigma1, key));
shared measurement(new Point2Prior(z, 1, key));
nlfg.push_back(measurement); nlfg.push_back(measurement);
poses.insert(key, z);
// initial estimate
poses.insert(key, xt);
} }
LinearFactorGraph lfg = nlfg.linearize(poses); LinearFactorGraph lfg = nlfg.linearize(poses);