MATLAB changes from develop

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

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

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

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

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