VisualSLAM matlab example
							parent
							
								
									22e71e4374
								
							
						
					
					
						commit
						715d663e4f
					
				|  | @ -0,0 +1,119 @@ | |||
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | ||||
| % 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 | ||||
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | ||||
| 
 | ||||
| %% Assumptions | ||||
| %  - Landmarks as 8 vertices of a cube: (10,10,10) (-10,10,10) etc... | ||||
| %  - Cameras are on a circle around the cube, pointing at the world origin | ||||
| %  - Each camera sees all landmarks.  | ||||
| %  - Visual measurements as 2D points are given, corrupted by Gaussian noise. | ||||
| 
 | ||||
| %% Generate simulated data | ||||
| % 3D landmarks as vertices of a cube | ||||
| points = {gtsamPoint3([10 10 10]'),... | ||||
|     gtsamPoint3([-10 10 10]'),... | ||||
|     gtsamPoint3([-10 -10 10]'),... | ||||
|     gtsamPoint3([10 -10 10]'),... | ||||
|     gtsamPoint3([10 10 -10]'),... | ||||
|     gtsamPoint3([-10 10 -10]'),... | ||||
|     gtsamPoint3([-10 -10 -10]'),... | ||||
|     gtsamPoint3([10 -10 -10]')}; | ||||
| 
 | ||||
| % Camera poses on a circle around the cube, pointing at the world origin | ||||
| nCameras = 8; | ||||
| r = 30; | ||||
| poses = {}; | ||||
| for i=1:nCameras | ||||
|     theta = i*2*pi/nCameras; | ||||
|     posei = 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 {posei}]; | ||||
| 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); | ||||
| 
 | ||||
| poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; | ||||
| poseNoiseSampler = gtsamSharedDiagonal(poseNoiseSigmas); | ||||
| 
 | ||||
| hold off; | ||||
| 
 | ||||
| %% 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); | ||||
|     end | ||||
| end | ||||
| 
 | ||||
| %% Add Gaussian priors for a pose and a landmark to constraint the system | ||||
| posePriorNoise  = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas); | ||||
| graph.addPosePrior(symbol('x',1), poses{1}, posePriorNoise); | ||||
| pointPriorNoise  = gtsamSharedNoiseModel_Sigmas(pointNoiseSigmas); | ||||
| graph.addPointPrior(symbol('l',1), points{1}, pointPriorNoise); | ||||
| 
 | ||||
| %% 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}.compose(gtsamPose3_Expmap(poseNoiseSampler.sample()))); | ||||
| end | ||||
| for j=1:size(points,2) | ||||
|     initialEstimate.insertPoint(symbol('l',j), points{j}.compose(gtsamPoint3(pointNoiseSampler.sample()))); | ||||
| 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  ')); | ||||
| 
 | ||||
| %% Query the marginals | ||||
| marginals = graph.marginals(result); | ||||
| 
 | ||||
| %% Plot results with covariance ellipses | ||||
| 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)); | ||||
|     posei = result.pose(symbol('x',i)) | ||||
|     plotCamera(posei,10); | ||||
|     posei_t = posei.translation() | ||||
|     covarianceEllipse3D([posei_t.x;posei_t.y;posei_t.z],P(4:6,4:6)); | ||||
| end | ||||
| 
 | ||||
|  | @ -0,0 +1,25 @@ | |||
| function covarianceEllipse3D(c,P) | ||||
| % covarianceEllipse3D: plot 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 | ||||
| % | ||||
| % Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966 | ||||
| 
 | ||||
| [e,s] = eig(P); | ||||
| k = 11.82;  | ||||
| radii = k*sqrt(diag(s)); | ||||
| 
 | ||||
| % generate data for "unrotated" ellipsoid | ||||
| [xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3)); | ||||
| 
 | ||||
| % rotate data with orientation matrix U and center M | ||||
| data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc); | ||||
| n = size(data,2); | ||||
| x = data(1:n,:)+c(1); y = data(n+1:2*n,:)+c(2); z = data(2*n+1:end,:)+c(3); | ||||
| 
 | ||||
| % now plot the rotated ellipse | ||||
| sc = mesh(x,y,z); | ||||
| shading interp | ||||
| alpha(0.5) | ||||
| axis equal | ||||
|  | @ -0,0 +1,18 @@ | |||
| function plotCamera(pose, axisLength) | ||||
|     C = pose.translation().vector(); | ||||
|     R = pose.rotation().matrix(); | ||||
|      | ||||
|     xAxis = C+R(:,1)*axisLength; | ||||
|     L = [C xAxis]'; | ||||
|     line(L(:,1),L(:,2),L(:,3),'Color','r'); | ||||
|      | ||||
|     yAxis = C+R(:,2)*axisLength; | ||||
|     L = [C yAxis]'; | ||||
|     line(L(:,1),L(:,2),L(:,3),'Color','g'); | ||||
|      | ||||
|     zAxis = C+R(:,3)*axisLength; | ||||
|     L = [C zAxis]'; | ||||
|     line(L(:,1),L(:,2),L(:,3),'Color','b'); | ||||
|      | ||||
|     axis equal | ||||
| end | ||||
		Loading…
	
		Reference in New Issue