From f01ee3b5d9d6b88361fc5e1c8bab5bee5c93499d Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 25 Jan 2014 10:18:58 -0500
Subject: [PATCH] MATLAB changes from develop

---
 matlab/+gtsam/EXPECT.m                        | 10 ++++
 matlab/+gtsam/covarianceEllipse.m             |  7 ++-
 matlab/gtsam_examples/MonocularVOExample.m    | 56 +++++++++++++++++++
 .../gtsam_examples/Pose2SLAMExample_graph.m   |  2 +-
 matlab/gtsam_examples/StereoVOExample.m       |  2 +-
 5 files changed, 72 insertions(+), 5 deletions(-)
 create mode 100644 matlab/+gtsam/EXPECT.m
 create mode 100644 matlab/gtsam_examples/MonocularVOExample.m

diff --git a/matlab/+gtsam/EXPECT.m b/matlab/+gtsam/EXPECT.m
new file mode 100644
index 000000000..382c200cc
--- /dev/null
+++ b/matlab/+gtsam/EXPECT.m
@@ -0,0 +1,10 @@
+function EXPECT(name,assertion)
+% EXPECT throw a warning if an assertion fails
+%
+% EXPECT(name,assertion)
+%  - name of test
+%  - assertion
+
+if (assertion~=1)
+    warning(['EXPECT ' name ' fails']);
+end
diff --git a/matlab/+gtsam/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m
index 7728fc866..387d7cd12 100644
--- a/matlab/+gtsam/covarianceEllipse.m
+++ b/matlab/+gtsam/covarianceEllipse.m
@@ -1,16 +1,17 @@
-function covarianceEllipse(x,P,color)
+function covarianceEllipse(x,P,color, k)
 % covarianceEllipse plots a Gaussian as an uncertainty ellipse
 % Based on Maybeck Vol 1, page 366
 % k=2.296 corresponds to 1 std, 68.26% of all probability
 % k=11.82 corresponds to 3 std, 99.74% of all probability
 %
-% covarianceEllipse(x,P,color)
+% covarianceEllipse(x,P,color,k)
 % it is assumed x and y are the first two components of state x
+% k is scaling for std deviations, defaults to 1 std
 
 [e,s] = eig(P(1:2,1:2));
 s1 = s(1,1);
 s2 = s(2,2);
-k = 2.296;
+if nargin<4, k = 2.296; end;
 [ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) );
 line(ex,ey,'color',color);
 
diff --git a/matlab/gtsam_examples/MonocularVOExample.m b/matlab/gtsam_examples/MonocularVOExample.m
new file mode 100644
index 000000000..9fd2df463
--- /dev/null
+++ b/matlab/gtsam_examples/MonocularVOExample.m
@@ -0,0 +1,56 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% GTSAM Copyright 2010-2013, Georgia Tech Research Corporation,
+% Atlanta, Georgia 30332-0415
+% All Rights Reserved
+% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+%
+% See LICENSE for the license information
+%
+% @brief Monocular VO Example with 5 landmarks and two cameras
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% import
+import gtsam.*
+
+%% Create two cameras and corresponding essential matrix E
+aRb = Rot3.Yaw(pi/2);
+aTb = Point3 (0.1, 0, 0);
+identity=Pose3;
+aPb = Pose3(aRb, aTb);
+cameraA = CalibratedCamera(identity);
+cameraB = CalibratedCamera(aPb);
+
+%% Create 5 points
+P = { Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5) };
+
+%% Project points in both cameras
+for i=1:5
+  pA{i}= cameraA.project(P{i});
+  pB{i}= cameraB.project(P{i});
+end
+
+%% We start with a factor graph and add epipolar constraints to it
+% Noise sigma is 1cm, assuming metric measurements
+graph = NonlinearFactorGraph;
+model = noiseModel.Isotropic.Sigma(1,0.01);
+for i=1:5
+  graph.add(EssentialMatrixFactor(1, pA{i}, pB{i}, model));
+end
+
+%% Create initial estimate
+initial = Values;
+trueE = EssentialMatrix(aRb,Sphere2(aTb));
+initialE = trueE.retract([0.1, -0.1, 0.1, 0.1, -0.1]');
+initial.insert(1, initialE);
+
+%% Optimize
+parameters = LevenbergMarquardtParams;
+% parameters.setVerbosity('ERROR');
+optimizer = LevenbergMarquardtOptimizer(graph, initial, parameters);
+result = optimizer.optimize();
+
+%% Print result (as essentialMatrix) and error
+error = graph.error(result)
+E = result.at(1)
+
diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m
index eb1b03950..b4957cce3 100644
--- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m
+++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m
@@ -13,7 +13,7 @@
 import gtsam.*
 
 %% Find data file
-datafile = findExampleDataFile('w100-odom.graph');
+datafile = findExampleDataFile('w100.graph');
 
 %% Initialize graph, initial estimate, and odometry noise
 model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
diff --git a/matlab/gtsam_examples/StereoVOExample.m b/matlab/gtsam_examples/StereoVOExample.m
index 8dbaa1a76..b437ca80c 100644
--- a/matlab/gtsam_examples/StereoVOExample.m
+++ b/matlab/gtsam_examples/StereoVOExample.m
@@ -33,7 +33,7 @@ graph.add(NonlinearEqualityPose3(x1, first_pose));
 %% Create realistic calibration and measurement noise model
 % format: fx fy skew cx cy baseline
 K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
-stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
+stereo_model = noiseModel.Isotropic.Sigma(3,1);
 
 %% Add measurements
 % pose 1