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

Smoother now creates x1...xT, not x0 anymore

release/4.3a0
Frank Dellaert 2009-10-31 16:54:38 +00:00
parent fd5f43092f
commit 68d2f81f0a
3 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cpp/smallExample.cpp
View File

@ -245,14 +245,14 @@ LinearFactorGraph createSmoother(int T) {
ExampleNonlinearFactorGraph nlfg; ExampleNonlinearFactorGraph nlfg;
VectorConfig poses; VectorConfig poses;
// prior on x0 // prior on x1
Vector x0 = zero(2); Vector x1 = zero(2);
string key0 = symbol('x', 0); string key1 = symbol('x', 1);
shared prior(new Point2Prior(x0, 1, key0)); shared prior(new Point2Prior(x1, 1, key1));
nlfg.push_back(prior); nlfg.push_back(prior);
poses.insert(key0, x0); poses.insert(key1, x1);
for (int t = 1; t <= T; t++) { for (int t = 2; t <= T; 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);

4
cpp/testLinearFactorGraph.cpp
View File

@ -510,9 +510,9 @@ TEST( LinearFactorGraph, findAndRemoveFactors_twice )
/* ************************************************************************* */ /* ************************************************************************* */
TEST(timeLinearFactorGraph, createSmoother) TEST(timeLinearFactorGraph, createSmoother)
{ {
LinearFactorGraph fg1 = createSmoother(1); LinearFactorGraph fg1 = createSmoother(2);
LONGS_EQUAL(3,fg1.size()); LONGS_EQUAL(3,fg1.size());
LinearFactorGraph fg2 = createSmoother(2); LinearFactorGraph fg2 = createSmoother(3);
LONGS_EQUAL(5,fg2.size()); LONGS_EQUAL(5,fg2.size());
} }

2
cpp/timeLinearFactorGraph.cpp
View File

@ -16,7 +16,7 @@ using namespace gtsam;
double timeKalmanSmoother(int T) { double timeKalmanSmoother(int T) {
LinearFactorGraph smoother = createSmoother(T); LinearFactorGraph smoother = createSmoother(T);
Ordering ordering; Ordering ordering;
for (int t = 0; t <= T; t++) ordering.push_back(symbol('x',t)); for (int t = 1; t <= T; t++) ordering.push_back(symbol('x',t));
clock_t start = clock(); clock_t start = clock();
smoother.optimize(ordering); smoother.optimize(ordering);
clock_t end = clock (); clock_t end = clock ();