diff --git a/examples/matlab/VisualSLAMExample.m b/examples/matlab/VisualSLAMExample.m index 93ede149a..99d1a39a7 100644 --- a/examples/matlab/VisualSLAMExample.m +++ b/examples/matlab/VisualSLAMExample.m @@ -28,61 +28,40 @@ points = {gtsamPoint3([10 10 10]'),... gtsamPoint3([10 -10 -10]')}; % Camera poses on a circle around the cube, pointing at the world origin -nCameras = 4; +nCameras = 6; +height = 0; r = 30; poses = {}; for i=1:nCameras theta = (i-1)*2*pi/nCameras; - pose_i = gtsamPose3(... - gtsamRot3([-sin(theta) 0 -cos(theta); - cos(theta) 0 -sin(theta); - 0 -1 0]),... - gtsamPoint3([r*cos(theta), r*sin(theta), 0]')); - poses = [poses {pose_i}]; + t = gtsamPoint3([r*cos(theta), r*sin(theta), height]'); + camera = gtsamSimpleCamera_lookat(t, gtsamPoint3(), gtsamPoint3([0,0,1]'), gtsamCal3_S2()) + poses{i} = camera.pose(); end -% 2D visual measurements, simulated with Gaussian noise -z = {}; -measurementNoiseSigmas = [0.5,0.5]'; -measurementNoiseSampler = gtsamSharedDiagonal(measurementNoiseSigmas); -K = gtsamCal3_S2(50,50,0,50,50); -for i=1:size(poses,2) - zi = {}; - camera = gtsamSimpleCamera(K,poses{i}); - for j=1:size(points,2) - zi = [zi {camera.project(points{j}).compose(gtsamPoint2(measurementNoiseSampler.sample()))}]; - end - z = [z; zi]; -end - -pointNoiseSigmas = [0.1,0.1,0.1]'; -pointNoiseSampler = gtsamSharedDiagonal(pointNoiseSigmas); - +measurementNoiseSigma = 1.0; +pointNoiseSigma = 0.1; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; -poseNoiseSampler = gtsamSharedDiagonal(poseNoiseSigmas); %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) graph = visualSLAMGraph; %% Add factors for all measurements -measurementNoise = gtsamSharedNoiseModel_Sigmas(measurementNoiseSigmas); -for i=1:size(z,1) - for j=1:size(z,2) - graph.addMeasurement(z{i,j}, measurementNoise, symbol('x',i), symbol('l',j), K); +measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma); +K = gtsamCal3_S2(500,500,0,640/2,480/2); +for i=1:nCameras + camera = gtsamSimpleCamera(K,poses{i}); + for j=1:size(points,2) + zij = camera.project(points{j}); % you can add noise here if desired + graph.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K); end end %% Add Gaussian priors for a pose and a landmark to constrain the system -% posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas); -% graph.addPosePrior(symbol('x',1), poses{1}, posePriorNoise); -pointPriorNoise = gtsamSharedNoiseModel_Sigmas(pointNoiseSigmas); +posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas); +graph.addPosePrior(symbol('x',1), poses{1}, posePriorNoise); +pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,pointNoiseSigma); graph.addPointPrior(symbol('l',1), points{1}, pointPriorNoise); -pointPriorNoise = gtsamSharedNoiseModel_Sigmas(pointNoiseSigmas); -graph.addPointPrior(symbol('l',8), points{8}, pointPriorNoise); -pointPriorNoise = gtsamSharedNoiseModel_Sigmas(pointNoiseSigmas); -graph.addPointPrior(symbol('l',5), points{5}, pointPriorNoise); -pointPriorNoise = gtsamSharedNoiseModel_Sigmas(pointNoiseSigmas); -graph.addPointPrior(symbol('l',4), points{4}, pointPriorNoise); %% Print the graph graph.print(sprintf('\nFactor graph:\n')); @@ -90,10 +69,10 @@ graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy poses and points initialEstimate = visualSLAMValues; for i=1:size(poses,2) - initialEstimate.insertPose(symbol('x',i), poses{i}.compose(gtsamPose3_Expmap(poseNoiseSampler.sample()))); + initialEstimate.insertPose(symbol('x',i), poses{i}); end for j=1:size(points,2) - initialEstimate.insertPoint(symbol('l',j), points{j}.compose(gtsamPoint3(pointNoiseSampler.sample()))); + initialEstimate.insertPoint(symbol('l',j), points{j}); end initialEstimate.print(sprintf('\nInitial estimate:\n ')); @@ -101,10 +80,8 @@ initialEstimate.print(sprintf('\nInitial estimate:\n ')); result = graph.optimize(initialEstimate); result.print(sprintf('\nFinal result:\n ')); -%% Query the marginals -marginals = graph.marginals(result); - %% Plot results with covariance ellipses +marginals = graph.marginals(result); figure(1);clf hold on; for j=1:size(points,2) diff --git a/examples/matlab/VisualSLAMExample_triangle.m b/examples/matlab/VisualSLAMExample_triangle.m new file mode 100644 index 000000000..4fc1db320 --- /dev/null +++ b/examples/matlab/VisualSLAMExample_triangle.m @@ -0,0 +1,88 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, 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 A simple visual SLAM example for structure from motion +% @author Duy-Nguyen Ta +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% Create a triangle target, just 3 points on a plane +r = 10; +points = {}; +for j=1:3 + theta = (j-1)*2*pi/3; + points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]'); +end + +%% Create camera poses on a circle around the triangle +nCameras = 6; +height = 10; +r = 30; +poses = {}; +for i=1:nCameras + theta = (i-1)*2*pi/nCameras; + t = gtsamPoint3([r*cos(theta), r*sin(theta), height]'); + camera = gtsamSimpleCamera_lookat(t, gtsamPoint3(), gtsamPoint3([0,0,1]'), gtsamCal3_S2()) + poses{i} = camera.pose(); +end + +%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) +graph = visualSLAMGraph; + +%% Add factors for all measurements +K = gtsamCal3_S2(500,500,0,640/2,480/2); +measurementNoiseSigma=1; % in pixels +measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma); +for i=1:nCameras + camera = gtsamSimpleCamera(K,poses{i}); + for j=1:3 + zij = camera.project(points{j}); % you can add noise here if desired + graph.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K); + end +end + +%% Add Gaussian priors for 3 points to constrain the system +pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,0.1); +for j=1:3 + graph.addPointPrior(symbol('l',j), points{j}, pointPriorNoise); +end + +%% Print the graph +graph.print(sprintf('\nFactor graph:\n')); + +%% Initialize to noisy poses and points +initialEstimate = visualSLAMValues; +for i=1:size(poses,2) + initialEstimate.insertPose(symbol('x',i), poses{i}); +end +for j=1:size(points,2) + initialEstimate.insertPoint(symbol('l',j), points{j}); +end +initialEstimate.print(sprintf('\nInitial estimate:\n ')); + +%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +result = graph.optimize(initialEstimate); +result.print(sprintf('\nFinal result:\n ')); + +%% Plot results with covariance ellipses +marginals = graph.marginals(result); +figure(1);clf +hold on; +for j=1:size(points,2) + P = marginals.marginalCovariance(symbol('l',j)); + point_j = result.point(symbol('l',j)); + plot3(point_j.x, point_j.y, point_j.z,'marker','o'); + covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); +end + +for i=1:size(poses,2) + P = marginals.marginalCovariance(symbol('x',i)) + pose_i = result.pose(symbol('x',i)) + plotPose3(pose_i,P,10); +end +axis equal +