1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file timeLago.cpp
14 * @brief Time the LAGO initialization method
15 * @author Richard Roberts
16 * @date Dec 3, 2010
17 */
18
19#include <gtsam/slam/dataset.h>
20#include <gtsam/slam/lago.h>
21#include <gtsam/geometry/Pose2.h>
22#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
23#include <gtsam/linear/Sampler.h>
24#include <gtsam/base/timing.h>
25
26#include <iostream>
27
28using namespace std;
29using namespace gtsam;
30
31int main(int argc, char *argv[]) {
32
33 size_t trials = 1;
34
35 // read graph
36 string inputFile = findExampleDataFile(name: "w10000");
37 auto model = noiseModel::Diagonal::Sigmas(sigmas: (Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
38 const auto [g, solution] = load2D(filename: inputFile, model);
39
40 // add noise to create initial estimate
41 Values initial;
42 auto noise = noiseModel::Diagonal::Sigmas(sigmas: (Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished());
43 Sampler sampler(noise);
44 for(const auto& [key,pose]: solution->extract<Pose2>())
45 initial.insert(j: key, val: pose.retract(v: sampler.sample()));
46
47 // Add prior on the pose having index (key) = 0
48 noiseModel::Diagonal::shared_ptr priorModel = //
49 noiseModel::Diagonal::Sigmas(sigmas: Vector3(1e-6, 1e-6, 1e-8));
50 g->addPrior(key: 0, prior: Pose2(), model: priorModel);
51
52 // LAGO
53 for (size_t i = 0; i < trials; i++) {
54 {
55 gttic_(lago);
56
57 gttic_(init);
58 Values lagoInitial = lago::initialize(graph: *g);
59 gttoc_(init);
60
61 gttic_(refine);
62 GaussNewtonOptimizer optimizer(*g, lagoInitial);
63 Values result = optimizer.optimize();
64 gttoc_(refine);
65 }
66
67 {
68 gttic_(optimize);
69 GaussNewtonOptimizer optimizer(*g, initial);
70 Values result = optimizer.optimize();
71 }
72
73 tictoc_finishedIteration_();
74 }
75
76 tictoc_print_();
77
78 return 0;
79}
80