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;
VectorConfig poses;
// prior on x0
Vector x0 = zero(2);
string key0 = symbol('x', 0);
shared prior(new Point2Prior(x0, 1, key0));
// prior on x1
Vector x1 = zero(2);
string key1 = symbol('x', 1);
shared prior(new Point2Prior(x1, 1, key1));
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}
Vector odo = Vector_(2, 1.0, 0.0);
string key = symbol('x', t);

4
cpp/testLinearFactorGraph.cpp
View File

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

2
cpp/timeLinearFactorGraph.cpp
View File

@ -16,7 +16,7 @@ using namespace gtsam;
double timeKalmanSmoother(int T) {
LinearFactorGraph smoother = createSmoother(T);
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();
smoother.optimize(ordering);
clock_t end = clock ();