gtsam  4.1.0
gtsam
InitializePose.h
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 
19 #pragma once
20 
21 #include <gtsam/inference/Symbol.h>
26 
27 namespace gtsam {
28 namespace initialize {
29 
30 static constexpr Key kAnchorKey = 99999999;
31 
36 template <class Pose>
37 static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) {
38  NonlinearFactorGraph poseGraph;
39 
40  for (const auto& factor : graph) {
41  // recast to a between on Pose
42  if (auto between =
43  boost::dynamic_pointer_cast<BetweenFactor<Pose> >(factor))
44  poseGraph.add(between);
45 
46  // recast PriorFactor<Pose> to BetweenFactor<Pose>
47  if (auto prior = boost::dynamic_pointer_cast<PriorFactor<Pose> >(factor))
48  poseGraph.emplace_shared<BetweenFactor<Pose> >(
49  kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel());
50  }
51  return poseGraph;
52 }
53 
57 template <class Pose>
58 static Values computePoses(const Values& initialRot,
59  NonlinearFactorGraph* posegraph,
60  bool singleIter = true) {
61  const auto origin = Pose().translation();
62 
63  // Upgrade rotations to full poses
64  Values initialPose;
65  for (const auto& key_value : initialRot) {
66  Key key = key_value.key;
67  const auto& rot = initialRot.at<typename Pose::Rotation>(key);
68  Pose initializedPose = Pose(rot, origin);
69  initialPose.insert(key, initializedPose);
70  }
71 
72  // add prior on dummy node
73  auto priorModel = noiseModel::Unit::Create(Pose::dimension);
74  initialPose.insert(kAnchorKey, Pose());
75  posegraph->emplace_shared<PriorFactor<Pose> >(kAnchorKey, Pose(), priorModel);
76 
77  // Create optimizer
78  GaussNewtonParams params;
79  if (singleIter) {
80  params.maxIterations = 1;
81  } else {
82  params.setVerbosity("TERMINATION");
83  }
84  GaussNewtonOptimizer optimizer(*posegraph, initialPose, params);
85  Values GNresult = optimizer.optimize();
86 
87  // put into Values structure
88  Values estimate;
89  for (const auto& key_value : GNresult) {
90  Key key = key_value.key;
91  if (key != kAnchorKey) {
92  const Pose& pose = GNresult.at<Pose>(key);
93  estimate.insert(key, pose);
94  }
95  }
96  return estimate;
97 }
98 } // namespace initialize
99 } // namespace gtsam
PriorFactor.h
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition: NoiseModel.h:608
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
BetweenFactor.h
GaussNewtonOptimizer.h
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:186
Symbol.h