Loading...
Searching...
No Matches
InitializePose.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
namespace gtsam {
namespace initialize {
static constexpr Key kAnchorKey = 99999999;
template <class Pose>
static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) {
NonlinearFactorGraph poseGraph;
for (const auto& factor : graph) {
// recast to a between on Pose
if (auto between =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(factor))
poseGraph.add(between);
// recast PriorFactor<Pose> to BetweenFactor<Pose>
if (auto prior = boost::dynamic_pointer_cast<PriorFactor<Pose> >(factor))
poseGraph.emplace_shared<BetweenFactor<Pose> >(
kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel());
}
return poseGraph;
}
template <class Pose>
static Values computePoses(const Values& initialRot,
NonlinearFactorGraph* posegraph,
bool singleIter = true) {
const auto origin = Pose().translation();
// Upgrade rotations to full poses
Values initialPose;
for (const auto& key_rot : initialRot.extract<typename Pose::Rotation>()) {
const Key& key = key_rot.first;
const auto& rot = key_rot.second;
const Pose initializedPose(rot, origin);
initialPose.insert(key, initializedPose);
}
// add prior on dummy node
auto priorModel = noiseModel::Unit::Create(Pose::dimension);
initialPose.insert(kAnchorKey, Pose());
posegraph->emplace_shared<PriorFactor<Pose> >(kAnchorKey, Pose(), priorModel);
// Create optimizer
GaussNewtonParams params;
if (singleIter) {
params.maxIterations = 1;
} else {
params.setVerbosity("TERMINATION");
}
GaussNewtonOptimizer optimizer(*posegraph, initialPose, params);
const Values GNresult = optimizer.optimize();
// put into Values structure
Values estimate;
for (const auto& key_pose : GNresult.extract<Pose>()) {
const Key& key = key_pose.first;
if (key != kAnchorKey) {
const Pose& pose = key_pose.second;
estimate.insert(key, pose);
}
}
return estimate;
}
} // namespace initialize
} // namespace gtsam
GaussNewtonOptimizer.h
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
BetweenFactor.h
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
Symbol.h
PriorFactor.h