gtsam 4.1.1
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
26
27namespace gtsam {
28namespace initialize {
29
30static constexpr Key kAnchorKey = 99999999;
31
36template <class Pose>
37static 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
57template <class Pose>
58static 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
Factor Graph consisting of non-linear factors.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:189
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition: NoiseModel.h:611