12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Odometry example was deliberately not using Symbols... Fixed...

release/4.3a0
Frank Dellaert 2012-08-04 20:18:42 +00:00
parent 821c08844c
commit 297fbfa1a5
1 changed files with 12 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
examples/OdometryExample.cpp
View File

@ -27,11 +27,6 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
// Each variable in the system (poses) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use symbols
#include <gtsam/nonlinear/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Between factors for the relative motion described by odometry measurements.
@ -60,35 +55,35 @@
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Create a factor graph container
// Create an empty nonlinear factor graph
NonlinearFactorGraph graph;
// Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(Symbol('x', 1), prior, priorNoise));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(Symbol('x', 1), Symbol('x', 2), Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(Symbol('x', 2), Symbol('x', 3), Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.print("\nFactor Graph:\n"); // print
// Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(Symbol('x', 1), Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(Symbol('x', 2), Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(Symbol('x', 3), Pose2(4.1, 0.1, 0.1));
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.print("\nInitial Estimate:\n"); // print
// optimize using Levenberg-Marquardt optimization
@ -97,9 +92,9 @@ int main(int argc, char** argv) {
// Calculate and print marginal covariances for all variables
Marginals marginals(graph, result);
cout << "Pose 1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl;
cout << "Pose 2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl;
cout << "Pose 3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl;
cout << "Pose 1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "Pose 2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "Pose 3 covariance:\n" << marginals.marginalCovariance(3) << endl;
return 0;
}