Pose3Localization.cpp
Go to the documentation of this file.
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 
20 #include <gtsam/slam/dataset.h>
24 #include <fstream>
25 
26 using namespace std;
27 using namespace gtsam;
28 
29 int main(const int argc, const char* argv[]) {
30  // Read graph from file
31  string g2oFile;
32  if (argc < 2)
33  g2oFile = findExampleDataFile("pose3Localizationexample.txt");
34  else
35  g2oFile = argv[1];
36 
38  Values::shared_ptr initial;
39  bool is3D = true;
40  std::tie(graph, initial) = readG2o(g2oFile, is3D);
41 
42  // Add prior on the first key
43  auto priorModel = noiseModel::Diagonal::Variances(
44  (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
45  Key firstKey = 0;
46  for (const auto key : initial->keys()) {
47  std::cout << "Adding prior to g2o file " << std::endl;
48  firstKey = key;
49  graph->addPrior(firstKey, Pose3(), priorModel);
50  break;
51  }
52 
53  std::cout << "Optimizing the factor graph" << std::endl;
55  params.setVerbosity("TERMINATION"); // show info about stopping conditions
57  Values result = optimizer.optimize();
58  std::cout << "Optimization complete" << std::endl;
59 
60  std::cout << "initial error=" << graph->error(*initial) << std::endl;
61  std::cout << "final error=" << graph->error(result) << std::endl;
62 
63  if (argc < 3) {
64  result.print("result");
65  } else {
66  const string outputFile = argv[2];
67  std::cout << "Writing results to file: " << outputFile << std::endl;
69  Values::shared_ptr initial2;
70  std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
71  writeG2o(*graphNoKernel, result, outputFile);
72  std::cout << "done! " << std::endl;
73  }
74 
75  // Calculate and print marginal covariances for all variables
77  for (const auto& [key, pose] : result.extract<Pose3>()) {
78  std::cout << marginals.marginalCovariance(key) << endl;
79  }
80  return 0;
81 }
main
int main(const int argc, const char *argv[])
Definition: Pose3Localization.cpp:29
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::readG2o
GraphAndValues readG2o(const std::string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition: dataset.cpp:621
gtsam::Marginals
Definition: Marginals.h:32
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::Marginals::marginalCovariance
Matrix marginalCovariance(Key variable) const
Definition: Marginals.cpp:133
GaussNewtonOptimizer.h
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
dataset.h
utility functions for loading datasets
gtsam::Values::extract
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Definition: Values-inl.h:105
initial2
Definition: testScenarioRunner.cpp:209
BetweenFactor.h
gtsam::Pose3
Definition: Pose3.h:37
gtsam::GaussNewtonParams
Definition: GaussNewtonOptimizer.h:30
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
key
const gtsam::Symbol key('X', 0)
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
Marginals.h
A class for computing marginals in a NonlinearFactorGraph.
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
outputFile
string outputFile
Definition: SolverComparer.cpp:88
initial
Definition: testScenarioRunner.cpp:148
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
marginals
Marginals marginals(graph, result)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::NonlinearFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactorGraph.h:61
gtsam::writeG2o
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure.
Definition: dataset.cpp:636


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:03:36