本文整理汇总了C++中NonlinearFactorGraph::saveGraph方法的典型用法代码示例。如果您正苦于以下问题：C++ NonlinearFactorGraph::saveGraph方法的具体用法？C++ NonlinearFactorGraph::saveGraph怎么用？C++ NonlinearFactorGraph::saveGraph使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类NonlinearFactorGraph的用法示例。
在下文中一共展示了NonlinearFactorGraph::saveGraph方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: main
int main (int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
// 2c. Add the loop closure constraint
graph.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
// 3. Create the data structure to hold the initial estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2 ));
initial.insert(2, Pose2(2.3, 0.1, -0.2 ));
initial.insert(3, Pose2(4.1, 0.1, M_PI_2));
initial.insert(4, Pose2(4.0, 2.0, M_PI ));
initial.insert(5, Pose2(2.1, 2.1, -M_PI_2));
// Single Step Optimization using Levenberg-Marquardt
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
// save factor graph as graphviz dot file
// Render to PDF using "fdp Pose2SLAMExample.dot -Tpdf > graph.pdf"
ofstream os("Pose2SLAMExample.dot");
graph.saveGraph(os, result);
// Also print out to console
graph.saveGraph(cout, result);
return 0;
}