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

Properly read w10000

release/4.3a0
dellaert 2014-05-31 16:24:50 -04:00
parent 7e6a333b1a
commit 42edec1066
1 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -29,18 +29,19 @@ using namespace gtsam;
int main(int argc, char *argv[]) { int main(int argc, char *argv[]) {
size_t trials = 1000; size_t trials = 1;
// read graph // read graph
NonlinearFactorGraph g; Values::shared_ptr initial;
Values initial; NonlinearFactorGraph::shared_ptr g;
string inputFile = findExampleDataFile("noisyToyGraph"); string inputFile = findExampleDataFile("w10000");
readG2o(inputFile, g, initial); SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
boost::tie(g, initial) = load2D(inputFile, model);
// 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 = //
noiseModel::Diagonal::Sigmas(Vector3(0, 0, 0)); noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8));
g.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); g->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
// LAGO // LAGO
for (size_t i = 0; i < trials; i++) { for (size_t i = 0; i < trials; i++) {
@ -48,18 +49,18 @@ int main(int argc, char *argv[]) {
gttic_(lago); gttic_(lago);
gttic_(init); gttic_(init);
Values lagoInitial = lago::initialize(g); Values lagoInitial = lago::initialize(*g);
gttoc_(init); gttoc_(init);
gttic_(refine); gttic_(refine);
GaussNewtonOptimizer optimizer(g, lagoInitial); GaussNewtonOptimizer optimizer(*g, lagoInitial);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
gttoc_(refine); gttoc_(refine);
} }
{ {
gttic_(optimize); gttic_(optimize);
GaussNewtonOptimizer optimizer(g, initial); GaussNewtonOptimizer optimizer(*g, *initial);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
} }