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

Adding noise does not help much

release/4.3a0
dellaert 2014-05-31 17:15:29 -04:00
parent 16c28b2e9c
commit 76297ea2cc
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/nonlinear/tests/timeLago.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/lago.h> #include <gtsam/nonlinear/lago.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h> #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <iostream> #include <iostream>
@ -32,11 +33,19 @@ int main(int argc, char *argv[]) {
size_t trials = 1; size_t trials = 1;
// read graph // read graph
Values::shared_ptr initial; Values::shared_ptr solution;
NonlinearFactorGraph::shared_ptr g; NonlinearFactorGraph::shared_ptr g;
string inputFile = findExampleDataFile("w10000"); string inputFile = findExampleDataFile("w10000");
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0)); SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
boost::tie(g, initial) = load2D(inputFile, model); boost::tie(g, solution) = load2D(inputFile, model);
// add noise to create initial estimate
Values initial;
Sampler sampler(42u);
Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>();
SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0));
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, poses)
initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise)));
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::shared_ptr priorModel = //
@ -60,7 +69,7 @@ int main(int argc, char *argv[]) {
{ {
gttic_(optimize); gttic_(optimize);
GaussNewtonOptimizer optimizer(*g, *initial); GaussNewtonOptimizer optimizer(*g, initial);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
} }