Updated remaining matlab examples

matlab/examples/SFMExample.m

@@ -22,7 +22,7 @@ options.nrCameras = 10;
 options.showImages = false;
 
 %% Generate data
-[data,truth] = VisualISAMGenerateData(options);
+[data,truth] = support.VisualISAMGenerateData(options);
 
 measurementNoiseSigma = 1.0;
 pointNoiseSigma = 0.1;

matlab/examples/StereoVOExample.m

@@ -22,12 +22,13 @@ x1 = symbol('x',1); x2 = symbol('x',2);
 l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3);
 
 %% Create graph container and add factors to it
-graph = visualSLAM.Graph;
+import gtsam.*
+graph = NonlinearFactorGraph;
 
 %% add a constraint on the starting pose
 import gtsam.*
 first_pose = Pose3();
-graph.addPoseConstraint(x1, first_pose);
+graph.add(NonlinearEqualityPose3(x1, first_pose));
 
 %% Create realistic calibration and measurement noise model
 % format: fx fy skew cx cy baseline
@@ -38,48 +39,41 @@ stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
 %% Add measurements
 import gtsam.*
 % pose 1
-graph.addStereoMeasurement(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K);
-graph.addStereoMeasurement(StereoPoint2(120,  80, 440), stereo_model, x1, l2, K);
-graph.addStereoMeasurement(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K);
+graph.add(GenericStereoFactor3D(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K));
+graph.add(GenericStereoFactor3D(StereoPoint2(120,  80, 440), stereo_model, x1, l2, K));
+graph.add(GenericStereoFactor3D(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K));
 
 %pose 2
-graph.addStereoMeasurement(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K);
-graph.addStereoMeasurement(StereoPoint2( 70,  20, 490), stereo_model, x2, l2, K);
-graph.addStereoMeasurement(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K);
+graph.add(GenericStereoFactor3D(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K));
+graph.add(GenericStereoFactor3D(StereoPoint2( 70,  20, 490), stereo_model, x2, l2, K));
+graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K));
 
 
 %% Create initial estimate for camera poses and landmarks
 import gtsam.*
-initialEstimate = visualSLAM.Values;
-initialEstimate.insertPose(x1, first_pose);
+initialEstimate = Values;
+initialEstimate.insert(x1, first_pose);
 % noisy estimate for pose 2
-initialEstimate.insertPose(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)));
-initialEstimate.insertPoint(l1, Point3( 1,  1, 5));
-initialEstimate.insertPoint(l2, Point3(-1,  1, 5));
-initialEstimate.insertPoint(l3, Point3( 0,-.5, 5));
+initialEstimate.insert(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)));
+initialEstimate.insert(l1, Point3( 1,  1, 5));
+initialEstimate.insert(l2, Point3(-1,  1, 5));
+initialEstimate.insert(l3, Point3( 0,-.5, 5));
 
 %% optimize
 fprintf(1,'Optimizing\n'); tic
-result = graph.optimize(initialEstimate,1);
+import gtsam.*
+optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
+result = optimizer.optimizeSafely();
 toc
 
 %% visualize initial trajectory, final trajectory, and final points
 cla; hold on;
 axis normal
-axis([-1 6 -2 2 -1.5 1.5]);
+axis([-1.5 1.5 -2 2 -1 6]);
 axis equal
 view(-38,12)
+camup([0;1;0]);
 
-% initial trajectory in red (rotated so Z is up)
-T=initialEstimate.translations;
-plot3(T(:,3),-T(:,1),-T(:,2), '-*r','LineWidth',2);
-
-% final trajectory in green (rotated so Z is up)
-T=result.translations;
-plot3(T(:,3),-T(:,1),-T(:,2), '-*g','LineWidth',2);
-xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
-
-% optimized 3D points (rotated so Z is up)
-P = result.points();
-plot3(P(:,3),-P(:,1),-P(:,2),'*');
-
+plot3DTrajectory(initialEstimate, 'r', 1, 0.3);
+plot3DTrajectory(result, 'g', 1, 0.3);
+plot3DPoints(result);

matlab/examples/StereoVOExample_large.m

@@ -13,73 +13,65 @@
 %% Load calibration
 import gtsam.*
 % format: fx fy skew cx cy baseline
-calib = dlmread('../../examples/Data/VO_calibration.txt');
+calib = dlmread(support.findExampleDataFile('VO_calibration.txt'));
 K = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
 stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
 
 %% create empty graph and values
-graph = visualSLAM.Graph;
-initial = visualSLAM.Values;
+graph = NonlinearFactorGraph;
+initial = Values;
 
 
 %% load the initial poses from VO
 % row format: camera_id 4x4 pose (row, major)
 import gtsam.*
 fprintf(1,'Reading data\n');
-cameras = dlmread('../../examples/Data/VO_camera_poses_large.txt');
+cameras = dlmread(support.findExampleDataFile('VO_camera_poses_large.txt'));
 for i=1:size(cameras,1)
     pose = Pose3(reshape(cameras(i,2:17),4,4)');
-    initial.insertPose(symbol('x',cameras(i,1)),pose);
+    initial.insert(symbol('x',cameras(i,1)),pose);
 end
 
 %% load stereo measurements and initialize landmarks
 % camera_id landmark_id uL uR v X Y Z
 import gtsam.*
-measurements = dlmread('../../examples/Data/VO_stereo_factors_large.txt');
+measurements = dlmread(support.findExampleDataFile('VO_stereo_factors_large.txt'));
 
 fprintf(1,'Creating Graph\n'); tic
 for i=1:size(measurements,1)
     sf = measurements(i,:);
-    graph.addStereoMeasurement(StereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ...
-        symbol('x', sf(1)), symbol('l', sf(2)), K);
+    graph.add(GenericStereoFactor3D(StereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ...
+        symbol('x', sf(1)), symbol('l', sf(2)), K));
     
     if ~initial.exists(symbol('l',sf(2)))
         % 3D landmarks are stored in camera coordinates: transform
         % to world coordinates using the respective initial pose
-        pose = initial.pose(symbol('x', sf(1)));
+        pose = initial.at(symbol('x', sf(1)));
         world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8)));
-        initial.insertPoint(symbol('l',sf(2)), world_point);
+        initial.insert(symbol('l',sf(2)), world_point);
     end
 end
 toc
 
 %% add a constraint on the starting pose
 key = symbol('x',1);
-first_pose = initial.pose(key);
-graph.addPoseConstraint(key, first_pose);
+first_pose = initial.at(key);
+graph.add(NonlinearEqualityPose3(key, first_pose));
 
 %% optimize
 fprintf(1,'Optimizing\n'); tic
-result = graph.optimize(initial,1);
+optimizer = LevenbergMarquardtOptimizer(graph, initial);
+result = optimizer.optimizeSafely();
 toc
 
 %% visualize initial trajectory, final trajectory, and final points
-figure(1); clf; hold on;
+cla; hold on;
 
-% initial trajectory in red (rotated so Z is up)
-plot3(initial.zs(),-initial.xs(),-initial.ys(), '-*r','LineWidth',2);
+plot3DTrajectory(initial, 'r', 1, 0.5);
+plot3DTrajectory(result, 'g', 1, 0.5);
+plot3DPoints(result);
 
-% final trajectory in green (rotated so Z is up)
-plot3(result.zs(),-result.xs(),-result.ys(), '-*g','LineWidth',2);
-xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
-
-% switch to XZ view
-view([0 0]);
-
-% optimized 3D points (rotated so Z is up)
-points = result.points();
-plot3(points(:,3),-points(:,1),-points(:,2),'.');
-
-axis([0 100 -20 20 -5 20]);
+axis([-5 20 -20 20 0 100]);
 axis equal
 view(3)
+camup([0;1;0]);

matlab/examples/gtsamExamples.m

@@ -22,7 +22,7 @@ function varargout = gtsamExamples(varargin)
 
 % Edit the above text to modify the response to help gtsamExamples
 
-% Last Modified by GUIDE v2.5 23-Jul-2012 13:12:19
+% Last Modified by GUIDE v2.5 24-Jul-2012 10:18:05
 
 % Begin initialization code - DO NOT EDIT
 gui_Singleton = 1;
@@ -114,9 +114,7 @@ echo off
 % --- Executes on button press in Pose2SLAMManhattan.
 function Pose2SLAMManhattan_Callback(hObject, eventdata, handles)
 axes(handles.axes3);
-echo on
 Pose2SLAMExample_graph
-echo off
 
 % --- Executes on button press in Pose3SLAM.
 function Pose3SLAM_Callback(hObject, eventdata, handles)
@@ -142,9 +140,7 @@ echo off
 % --- Executes on button press in PlanarSLAMSampling.
 function PlanarSLAMSampling_Callback(hObject, eventdata, handles)
 axes(handles.axes3);
-echo on
 PlanarSLAMExample_sampling
-echo off
 
 % --- Executes on button press in SFM.
 function SFM_Callback(hObject, eventdata, handles)
@@ -166,3 +162,8 @@ axes(handles.axes3);
 echo on
 StereoVOExample
 echo off
+
+% --- Executes on button press in StereoVOLarge.
+function StereoVOLarge_Callback(hObject, eventdata, handles)
+axes(handles.axes3);
+StereoVOExample_large