30static constexpr Key kAnchorKey = 99999999;
37static NonlinearFactorGraph buildPoseGraph(
const NonlinearFactorGraph& graph) {
38 NonlinearFactorGraph poseGraph;
40 for (
const auto& factor : graph) {
43 boost::dynamic_pointer_cast<BetweenFactor<Pose> >(factor))
44 poseGraph.
add(between);
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());
59 NonlinearFactorGraph* posegraph,
60 bool singleIter =
true) {
61 const auto origin = Pose().translation();
65 for (
const auto& key_rot : initialRot.extract<
typename Pose::Rotation>()) {
66 const Key& key = key_rot.first;
67 const auto& rot = key_rot.second;
68 const Pose initializedPose(rot, origin);
69 initialPose.insert(key, initializedPose);
74 initialPose.insert(kAnchorKey, Pose());
75 posegraph->emplace_shared<PriorFactor<Pose> >(kAnchorKey, Pose(), priorModel);
78 GaussNewtonParams params;
80 params.maxIterations = 1;
82 params.setVerbosity(
"TERMINATION");
84 GaussNewtonOptimizer optimizer(*posegraph, initialPose, params);
85 const Values GNresult = optimizer.optimize();
89 for (
const auto& key_pose : GNresult.extract<Pose>()) {
90 const Key& key = key_pose.first;
91 if (key != kAnchorKey) {
92 const Pose& pose = key_pose.second;
93 estimate.insert(key, pose);
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:100
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:210
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition: NoiseModel.h:597
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...