diff --git a/doc/Code/LocalizationExample2.cpp b/doc/Code/LocalizationExample2.cpp new file mode 100644 index 000000000..42f19de9e --- /dev/null +++ b/doc/Code/LocalizationExample2.cpp @@ -0,0 +1,7 @@ +// add unary measurement factors, like GPS, on all three poses +noiseModel::Diagonal::shared_ptr unaryNoise = + unaryNoise::Diagonal::Sigmas((Vector(2)<< 0.1, 0.1)); // 10cm std on x,y +graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); +graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); +graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); + diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp new file mode 100644 index 000000000..9be7dce99 --- /dev/null +++ b/doc/Code/LocalizationFactor.cpp @@ -0,0 +1,14 @@ +class UnaryFactor: public NoiseModelFactor1 { + double mx_, my_; ///< X and Y measurements + +public: + UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): + NoiseModelFactor1(model, j), mx_(x), my_(y) {} + + Vector evaluateError(const Pose2& q, + boost::optional H = boost::none) const + { + if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0); + return (Vector(2) << q.x() - mx_, q.y() - my_); + } +}; diff --git a/doc/Code/LocalizationOutput4.txt b/doc/Code/LocalizationOutput4.txt new file mode 100644 index 000000000..902b79f79 --- /dev/null +++ b/doc/Code/LocalizationOutput4.txt @@ -0,0 +1,14 @@ +Factor graph: +size: 5 +factor 0: BetweenFactor(1,2) + measured(2, 0, 0) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 1: BetweenFactor(2,3) + measured(2, 0, 0) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 2: keys = { 1 } + noise model: diagonal sigmas [0.1; 0.1]; +factor 3: keys = { 2 } + noise model: diagonal sigmas [0.1; 0.1]; +factor 4: keys = { 3 } + noise model: diagonal sigmas [0.1; 0.1]; diff --git a/doc/Code/LocalizationOutput5.txt b/doc/Code/LocalizationOutput5.txt new file mode 100644 index 000000000..d04162f20 --- /dev/null +++ b/doc/Code/LocalizationOutput5.txt @@ -0,0 +1,18 @@ +Final Result: +Values with 3 values: +Value 1: (-1.5e-14, 1.3e-15, -1.4e-16) +Value 2: (2, 3.1e-16, -8.5e-17) +Value 3: (4, -6e-16, -8.2e-17) + +x1 covariance: + 0.0083 4.3e-19 -1.1e-18 + 4.3e-19 0.0094 -0.0031 + -1.1e-18 -0.0031 0.0082 +x2 covariance: + 0.0071 2.5e-19 -3.4e-19 + 2.5e-19 0.0078 -0.0011 + -3.4e-19 -0.0011 0.0082 +x3 covariance: + 0.0083 4.4e-19 1.2e-18 + 4.4e-19 0.0094 0.0031 + 1.2e-18 0.0031 0.018 diff --git a/doc/Code/OdometryExample.cpp b/doc/Code/OdometryExample.cpp new file mode 100644 index 000000000..c2b6558a4 --- /dev/null +++ b/doc/Code/OdometryExample.cpp @@ -0,0 +1,15 @@ +// Create an empty nonlinear factor graph +NonlinearFactorGraph graph; + +// Add a Gaussian prior on pose x_1 +Pose2 priorMean(0.0, 0.0, 0.0); +noiseModel::Diagonal::shared_ptr priorNoise = + noiseModel::Diagonal::Sigmas((Vector(3)<< 0.3, 0.3, 0.1)); +graph.add(PriorFactor(1, priorMean, priorNoise)); + +// Add two odometry factors +Pose2 odometry(2.0, 0.0, 0.0); +noiseModel::Diagonal::shared_ptr odometryNoise = + noiseModel::Diagonal::Sigmas((Vector(3)<< 0.2, 0.2, 0.1)); +graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); diff --git a/doc/Code/OdometryMarginals.cpp b/doc/Code/OdometryMarginals.cpp new file mode 100644 index 000000000..e1b4ed411 --- /dev/null +++ b/doc/Code/OdometryMarginals.cpp @@ -0,0 +1,6 @@ +// Query the marginals +cout.precision(2); +Marginals marginals(graph, result); +cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl; +cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl; +cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl; diff --git a/doc/Code/OdometryOptimize.cpp b/doc/Code/OdometryOptimize.cpp new file mode 100644 index 000000000..ee3c918d4 --- /dev/null +++ b/doc/Code/OdometryOptimize.cpp @@ -0,0 +1,9 @@ +// create (deliberatly inaccurate) initial estimate +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, 0.1)); + +// optimize using Levenberg-Marquardt optimization +Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + diff --git a/doc/Code/OdometryOutput1.txt b/doc/Code/OdometryOutput1.txt new file mode 100644 index 000000000..cc34e8ef2 --- /dev/null +++ b/doc/Code/OdometryOutput1.txt @@ -0,0 +1,11 @@ +Factor Graph: +size: 3 +factor 0: PriorFactor on 1 + prior mean: (0, 0, 0) + noise model: diagonal sigmas [0.3; 0.3; 0.1]; +factor 1: BetweenFactor(1,2) + measured: (2, 0, 0) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 2: BetweenFactor(2,3) + measured: (2, 0, 0) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; diff --git a/doc/Code/OdometryOutput2.txt b/doc/Code/OdometryOutput2.txt new file mode 100644 index 000000000..acfa0b95d --- /dev/null +++ b/doc/Code/OdometryOutput2.txt @@ -0,0 +1,11 @@ +Initial Estimate: +Values with 3 values: +Value 1: (0.5, 0, 0.2) +Value 2: (2.3, 0.1, -0.2) +Value 3: (4.1, 0.1, 0.1) + +Final Result: +Values with 3 values: +Value 1: (-1.8e-16, 8.7e-18, -9.1e-19) +Value 2: (2, 7.4e-18, -2.5e-18) +Value 3: (4, -1.8e-18, -3.1e-18) diff --git a/doc/Code/OdometryOutput3.txt b/doc/Code/OdometryOutput3.txt new file mode 100644 index 000000000..e346ccb4d --- /dev/null +++ b/doc/Code/OdometryOutput3.txt @@ -0,0 +1,12 @@ +x1 covariance: + 0.09 1.1e-47 5.7e-33 + 1.1e-47 0.09 1.9e-17 + 5.7e-33 1.9e-17 0.01 +x2 covariance: + 0.13 4.7e-18 2.4e-18 + 4.7e-18 0.17 0.02 + 2.4e-18 0.02 0.02 +x3 covariance: + 0.17 2.7e-17 8.4e-18 + 2.7e-17 0.37 0.06 + 8.4e-18 0.06 0.03 diff --git a/doc/Code/PlanarSLAMExample.m b/doc/Code/PlanarSLAMExample.m new file mode 100644 index 000000000..0231d52d0 --- /dev/null +++ b/doc/Code/PlanarSLAMExample.m @@ -0,0 +1,25 @@ +% Create graph container and add factors to it +graph = NonlinearFactorGraph; + +% Create keys for variables +i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); +j1 = symbol('l',1); j2 = symbol('l',2); + +% Add prior +priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); +% add directly to graph +graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); + +% Add odometry +odometry = Pose2(2.0, 0.0, 0.0); +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); + +% Add bearing/range measurement factors +degrees = pi/180; +noiseModel = noiseModel.Diagonal.Sigmas([0.1; 0.2]); +graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), noiseModel)); +graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, noiseModel)); +graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, noiseModel)); diff --git a/doc/Code/PlanarSLAMExample.txt b/doc/Code/PlanarSLAMExample.txt new file mode 100644 index 000000000..507a6b5ec --- /dev/null +++ b/doc/Code/PlanarSLAMExample.txt @@ -0,0 +1,7 @@ +>> result +Values with 5 values: + l1: (2, 2) + l2: (4, 2) + x1: (-1.8e-16, 5.1e-17, -1.5e-17) + x2: (2, -5.8e-16, -4.6e-16) + x3: (4, -3.1e-15, -4.6e-16) diff --git a/doc/Code/Pose2SLAMExample-graph.m b/doc/Code/Pose2SLAMExample-graph.m new file mode 100644 index 000000000..0e384359c --- /dev/null +++ b/doc/Code/Pose2SLAMExample-graph.m @@ -0,0 +1,15 @@ +%% Initialize graph, initial estimate, and odometry noise +datafile = findExampleDataFile('w100.graph'); +model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); +[graph,initial] = load2D(datafile, model); + +%% Add a Gaussian prior on pose x_0 +priorMean = Pose2(0, 0, 0); +priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]); +graph.add(PriorFactorPose2(0, priorMean, priorNoise)); + +%% Optimize using Levenberg-Marquardt optimization and get marginals +optimizer = LevenbergMarquardtOptimizer(graph, initial); +result = optimizer.optimizeSafely; +marginals = Marginals(graph, result); + diff --git a/doc/Code/Pose2SLAMExample-graph2.m b/doc/Code/Pose2SLAMExample-graph2.m new file mode 100644 index 000000000..1cbd020d0 --- /dev/null +++ b/doc/Code/Pose2SLAMExample-graph2.m @@ -0,0 +1,8 @@ +%% Plot result, including covariance ellipses +figure(1);clf; plot(initial.xs(),initial.ys(),'g-*'); axis equal +hold on; plot(result.xs(),result.ys(),'b-*') +for i=1:result.size()-1 + pose_i = result.pose(i); + P{i}=marginals.marginalCovariance(i); + covarianceEllipse([pose_i.x;pose_i.y],P{i},'b') +end diff --git a/doc/Code/Pose2SLAMExample.cpp b/doc/Code/Pose2SLAMExample.cpp new file mode 100644 index 000000000..470f13397 --- /dev/null +++ b/doc/Code/Pose2SLAMExample.cpp @@ -0,0 +1,16 @@ +NonlinearFactorGraph graph; +noiseModel::Diagonal::shared_ptr priorNoise = + noiseModel::Diagonal::Sigmas((Vector(3)<< 0.3, 0.3, 0.1)); +graph.add(PriorFactor(1, Pose2(0,0,0), priorNoise)); + +// Add odometry factors +noiseModel::Diagonal::shared_ptr model = + noiseModel::Diagonal::Sigmas((Vector(3)<< 0.2, 0.2, 0.1)); +graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); +graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); +graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); +graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + +// Add pose constraint +graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + diff --git a/doc/Code/Pose2SLAMExample.m b/doc/Code/Pose2SLAMExample.m new file mode 100644 index 000000000..561a07b1e --- /dev/null +++ b/doc/Code/Pose2SLAMExample.m @@ -0,0 +1,14 @@ +graph = NonlinearFactorGraph; +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); +graph.add(PriorFactorPose2(1, Pose2(0, 0, 0), priorNoise)); + +%% Add odometry factors +model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +graph.add(BetweenFactorPose2(1, 2, Pose2(2, 0, 0 ), model)); +graph.add(BetweenFactorPose2(2, 3, Pose2(2, 0, pi/2), model)); +graph.add(BetweenFactorPose2(3, 4, Pose2(2, 0, pi/2), model)); +graph.add(BetweenFactorPose2(4, 5, Pose2(2, 0, pi/2), model)); + +%% Add pose constraint +graph.add(BetweenFactorPose2(5, 2, Pose2(2, 0, pi/2), model)); + diff --git a/doc/Code/Pose3SLAMExample-graph.m b/doc/Code/Pose3SLAMExample-graph.m new file mode 100644 index 000000000..dde5ba85e --- /dev/null +++ b/doc/Code/Pose3SLAMExample-graph.m @@ -0,0 +1,12 @@ +%% Initialize graph, initial estimate, and odometry noise +datafile = findExampleDataFile('sphere2500.txt'); +model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.1; 0.1; 0.1]); +[graph,initial] = load3D(datafile, model, true, 2500); +plot3DTrajectory(initial,'g-',false); % Plot Initial Estimate + +%% Read again, now with all constraints, and optimize +graph = load3D(datafile, model); +graph.add(NonlinearEqualityPose3(0, initial.at(0))); +optimizer = LevenbergMarquardtOptimizer(graph, initial); +result = optimizer.optimizeSafely(); +plot3DTrajectory(result, 'r-', false); axis equal; diff --git a/doc/Code/SFMExample.m b/doc/Code/SFMExample.m new file mode 100644 index 000000000..cb6bdf00b --- /dev/null +++ b/doc/Code/SFMExample.m @@ -0,0 +1,9 @@ +%% Add factors for all measurements +noise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); +for i=1:length(Z), + for k=1:length(Z{i}) + j = J{i}{k}; + G.add(GenericProjectionFactorCal3_S2( + Z{i}{k}, noise, symbol('x',i), symbol('p',j), K)); + end +end diff --git a/doc/Code/VisualISAMExample.cpp b/doc/Code/VisualISAMExample.cpp new file mode 100644 index 000000000..345913cd9 --- /dev/null +++ b/doc/Code/VisualISAMExample.cpp @@ -0,0 +1,24 @@ +int relinearizeInterval = 3; +NonlinearISAM isam(relinearizeInterval); + +// ... first frame initialization omitted ... + +// Loop over the different poses, adding the observations to iSAM +for (size_t i = 1; i < poses.size(); ++i) { + + // Add factors for each landmark observation + NonlinearFactorGraph graph; + for (size_t j = 0; j < points.size(); ++j) { + graph.add( + GenericProjectionFactor + (z[i][j], noise,Symbol('x', i), Symbol('l', j), K) + ); + } + + // Add an initial guess for the current pose + Values initialEstimate; + initialEstimate.insert(Symbol('x', i), initial_x[i]); + + // Update iSAM with the new factors + isam.update(graph, initialEstimate); + } diff --git a/doc/Code/calls.txt b/doc/Code/calls.txt new file mode 100644 index 000000000..2afd2fd12 --- /dev/null +++ b/doc/Code/calls.txt @@ -0,0 +1,7 @@ +>> graph.error(initialEstimate) +ans = + 20.1086 + +>> graph.error(result) +ans = + 8.2631e-18 diff --git a/doc/Code/print.txt b/doc/Code/print.txt new file mode 100644 index 000000000..da8fb3b2a --- /dev/null +++ b/doc/Code/print.txt @@ -0,0 +1,23 @@ +>> priorNoise +diagonal sigmas [0.3; 0.3; 0.1]; + +>> graph +size: 6 +factor 0: PriorFactor on 1 + prior mean: (0, 0, 0) + noise model: diagonal sigmas [0.3; 0.3; 0.1]; +factor 1: BetweenFactor(1,2) + measured: (2, 0, 0) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 2: BetweenFactor(2,3) + measured: (2, 0, 1.6) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 3: BetweenFactor(3,4) + measured: (2, 0, 1.6) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 4: BetweenFactor(4,5) + measured: (2, 0, 1.6) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 5: BetweenFactor(5,2) + measured: (2, 0, 1.6) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; diff --git a/doc/Code/whos.txt b/doc/Code/whos.txt new file mode 100644 index 000000000..592fedca9 --- /dev/null +++ b/doc/Code/whos.txt @@ -0,0 +1,7 @@ +>> whos + Name Size Bytes Class + graph 1x1 112 gtsam.NonlinearFactorGraph + priorNoise 1x1 112 gtsam.noiseModel.Diagonal + model 1x1 112 gtsam.noiseModel.Diagonal + initialEstimate 1x1 112 gtsam.Values + optimizer 1x1 112 gtsam.LevenbergMarquardtOptimizer diff --git a/doc/gtsam.lyx b/doc/gtsam.lyx index 38b601d23..9d1e6831a 100644 --- a/doc/gtsam.lyx +++ b/doc/gtsam.lyx @@ -139,7 +139,7 @@ September 2014 \begin_layout Standard \begin_inset CommandInset include LatexCommand input -filename "../common_macros.tex" +filename "common_macros.tex" \end_inset @@ -2129,12 +2129,12 @@ loop closure placement h wide false sideways false -status collapsed +status open \begin_layout Plain Layout \align center \begin_inset Graphics - filename figures/PoseSLAM/example.pdf + filename images/example1.pdf width 80text% BoundingBox 30bp 170bp 610bp 630bp clip @@ -2662,12 +2662,12 @@ reference "fig:SLAM" placement h wide false sideways false -status collapsed +status open \begin_layout Plain Layout \align center \begin_inset Graphics - filename figures/PlanarSLAM/example.pdf + filename images/example2.pdf scale 47 BoundingBox 90bp 220bp 520bp 555bp clip @@ -3014,12 +3014,12 @@ A Real-World Example placement h wide false sideways false -status collapsed +status open \begin_layout Plain Layout \align center \begin_inset Graphics - filename figures/PlanarSLAM/Victoria.pdf + filename images/Victoria.pdf width 90text% BoundingBox 0bp 0bp 420bp 180bp clip diff --git a/doc/gtsam.pdf b/doc/gtsam.pdf new file mode 100644 index 000000000..a5b92ec27 Binary files /dev/null and b/doc/gtsam.pdf differ diff --git a/doc/images/Victoria.pdf b/doc/images/Victoria.pdf new file mode 100644 index 000000000..0fa9b9220 Binary files /dev/null and b/doc/images/Victoria.pdf differ diff --git a/doc/images/example1.pdf b/doc/images/example1.pdf new file mode 100644 index 000000000..e9446f0fe Binary files /dev/null and b/doc/images/example1.pdf differ diff --git a/doc/images/example2.pdf b/doc/images/example2.pdf new file mode 100644 index 000000000..8fa600eda Binary files /dev/null and b/doc/images/example2.pdf differ