Added several code files and gtsam.pdf.

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<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
+graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
+graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
+

doc/Code/LocalizationFactor.cpp

@@ -0,0 +1,14 @@
+class UnaryFactor: public NoiseModelFactor1<Pose2> {
+  double mx_, my_; ///< X and Y measurements
+
+public:
+  UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
+    NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
+
+  Vector evaluateError(const Pose2& q, 
+                       boost::optional<Matrix&> 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_);
+  }
+};

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];

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

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<Pose2>(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<Pose2>(1, 2, odometry, odometryNoise));
+graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

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;

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();
+

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];

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)

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

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));

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)

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);
+

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

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<Pose2>(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<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
+graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
+graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
+graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
+
+// Add pose constraint
+graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
+

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));
+

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;

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

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<Pose3, Point3, Cal3_S2>
+        (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);
+ }

doc/Code/calls.txt

@@ -0,0 +1,7 @@
+>> graph.error(initialEstimate)
+ans =
+   20.1086
+
+>> graph.error(result)
+ans =
+   8.2631e-18

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];

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              

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