Pose3SLAM example works (MATLAB only)
							parent
							
								
									198279f4ff
								
							
						
					
					
						commit
						be7828b8cf
					
				|  | @ -10,19 +10,24 @@ | |||
| % @author Frank Dellaert | ||||
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | ||||
| 
 | ||||
| N = 2500; | ||||
| % filename = '../Data/sphere_smallnoise.graph'; | ||||
| % filename = '../Data/sphere2500_groundtruth.txt'; | ||||
| filename = '../Data/sphere2500.txt'; | ||||
| 
 | ||||
| %% Initialize graph, initial estimate, and odometry noise | ||||
| model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); | ||||
| [graph,initial]=load3D('../Data/sphere_smallnoise.graph',model,100); | ||||
| 
 | ||||
| %% Fix first pose | ||||
| [graph,initial]=load3D(filename,model,true,N); | ||||
| first = initial.pose(0); | ||||
| graph.addHardConstraint(0, first); % add directly to graph | ||||
| graph.addHardConstraint(0, first); | ||||
| 
 | ||||
| %% Plot Initial Estimate | ||||
| figure(1);clf | ||||
| plot3(initial.xs(),initial.ys(),initial.zs(),'g-'); hold on | ||||
| plot3(first.x(),first.y(),first.z(),'r*');  | ||||
| plot3(first.x(),first.y(),first.z(),'r*'); hold on | ||||
| plot3DTrajectory(initial,'g-',false); | ||||
| 
 | ||||
| %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd | ||||
| result = graph.optimize(initial); | ||||
| hold on; plot3(result.xs(),result.ys(),result.zs(),'b-');axis equal;  | ||||
| %% Read again, now all constraints | ||||
| [graph,discard]=load3D(filename,model,false,N); | ||||
| graph.addHardConstraint(0, first); | ||||
| result = graph.optimize(initial); % start from old result | ||||
| plot3DTrajectory(result,'r-',false); axis equal; | ||||
|  |  | |||
|  | @ -1,6 +1,9 @@ | |||
| function [graph,initial] = load3D(filename,model,N) | ||||
| function [graph,initial] = load3D(filename,model,successive,N) | ||||
| % load3D: read TORO 3D pose graph | ||||
| % cannot read noise model from file yet, uses specified model | ||||
| % if [successive] is tru, constructs initial estimate from odometry | ||||
| 
 | ||||
| if nargin<3, successive=false; end | ||||
| fid = fopen(filename); | ||||
| if fid < 0 | ||||
|     error(['load2D: Cannot open file ' filename]); | ||||
|  | @ -14,24 +17,39 @@ lines=columns{1}; | |||
| % loop over lines and add vertices | ||||
| graph = pose3SLAMGraph; | ||||
| initial = pose3SLAMValues; | ||||
| origin=gtsamPose3; | ||||
| initial.insertPose(0,origin); | ||||
| n=size(lines,1); | ||||
| if nargin<3, N=n;end | ||||
| if nargin<4, N=n;end | ||||
| 
 | ||||
| for i=1:n | ||||
|     line_i=lines{i}; | ||||
|     if strcmp('VERTEX3',line_i(1:7)) | ||||
|         v = textscan(line_i,'%s %d %f %f %f %f %f %f',1); | ||||
|         if v{2}<N | ||||
|         i=v{2}; | ||||
|         if (~successive & i<N | successive & i==0) | ||||
|             t = gtsamPoint3(v{3}, v{4}, v{5}); | ||||
|             R = gtsamRot3_ypr(v{6}, v{7}, v{8}); | ||||
|             initial.insertPose(v{2}, gtsamPose3(R,t)); | ||||
|             R = gtsamRot3_ypr(v{8}, -v{7}, v{6}); | ||||
|             initial.insertPose(i, gtsamPose3(R,t)); | ||||
|         end | ||||
|     elseif strcmp('EDGE3',line_i(1:5)) | ||||
|         e = textscan(line_i,'%s %d %d %f %f %f %f %f %f',1); | ||||
|         if e{2}<N & e{3}<N | ||||
|             t = gtsamPoint3(e{4}, e{5}, e{6}); | ||||
|             R = gtsamRot3_ypr(e{7}, e{8}, e{9}); | ||||
|             graph.addConstraint(e{2}, e{3}, gtsamPose3(R,t), model); | ||||
|         i1=e{2}; | ||||
|         i2=e{3}; | ||||
|         if i1<N & i2<N | ||||
|             if ~successive | abs(i2-i1)==1 | ||||
|                 t = gtsamPoint3(e{4}, e{5}, e{6}); | ||||
|                 R = gtsamRot3_ypr(e{9}, e{8}, e{7}); | ||||
|                 dpose = gtsamPose3(R,t); | ||||
|                 graph.addConstraint(i1, i2, dpose, model); | ||||
|                 if successive | ||||
|                     if i2>i1 | ||||
|                         initial.insertPose(i2,initial.pose(i1).compose(dpose)); | ||||
|                     else | ||||
|                         initial.insertPose(i1,initial.pose(i2).compose(dpose.inverse)); | ||||
|                     end | ||||
|                 end | ||||
|             end | ||||
|         end | ||||
|     end | ||||
| end | ||||
|  |  | |||
|  | @ -0,0 +1,15 @@ | |||
| function plot3DTrajectory(values,style,frames) | ||||
| % plot3DTrajectory | ||||
| if nargin<3,frames=false;end | ||||
| 
 | ||||
| plot3(values.xs(),values.ys(),values.zs(),style); hold on | ||||
| if frames | ||||
|     for i=0:N-1 | ||||
|         pose = values.pose(i); | ||||
|         t = pose.translation; | ||||
|         R = pose.rotation.matrix; | ||||
|         quiver3(t.x,t.y,t.z,R(1,1),R(2,1),R(3,1),'r'); | ||||
|         quiver3(t.x,t.y,t.z,R(1,2),R(2,2),R(3,2),'g'); | ||||
|         quiver3(t.x,t.y,t.z,R(1,3),R(2,3),R(3,3),'b'); | ||||
|     end | ||||
| end | ||||
							
								
								
									
										47
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										47
									
								
								gtsam.h
								
								
								
								
							|  | @ -197,26 +197,51 @@ class Pose2 { | |||
| }; | ||||
| 
 | ||||
| class Pose3 { | ||||
| 	// Standard Constructors
 | ||||
| 	Pose3(); | ||||
| 	Pose3(const gtsam::Pose3& pose); | ||||
| 	Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); | ||||
| 	Pose3(Matrix t); | ||||
| 	Pose3(const gtsam::Pose2& pose2); | ||||
| 	static gtsam::Pose3 Expmap(Vector v); | ||||
| 	static Vector Logmap(const gtsam::Pose3& p); | ||||
| 	Pose3(Matrix t); | ||||
| 
 | ||||
| 	// Testable
 | ||||
| 	void print(string s) const; | ||||
| 	bool equals(const gtsam::Pose3& pose, double tol) const; | ||||
| 
 | ||||
| 	// Group
 | ||||
| 	static gtsam::Pose3 identity(); | ||||
| 	gtsam::Pose3 inverse(); | ||||
| 	gtsam::Pose3 compose(const gtsam::Pose3& p2); | ||||
| 	gtsam::Pose3 between(const gtsam::Pose3& p2); | ||||
| 
 | ||||
| 	// Manifold
 | ||||
| 	static size_t Dim(); | ||||
| 	size_t dim() const; | ||||
| 	gtsam::Pose3 retract(Vector v); | ||||
| 	gtsam::Pose3 retractFirstOrder(Vector v); | ||||
| 	Vector localCoordinates(const gtsam::Pose3& T2) const; | ||||
| 
 | ||||
| 	// Lie Group
 | ||||
| 	static gtsam::Pose3 Expmap(Vector v); | ||||
| 	static Vector Logmap(const gtsam::Pose3& p); | ||||
| 	Matrix adjointMap() const; | ||||
| 	Vector adjoint(const Vector& xi) const; | ||||
| 	static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); | ||||
| 
 | ||||
| 	// Group Action on Point3
 | ||||
| 	gtsam::Point3 transform_from(const gtsam::Point3& p) const; | ||||
| 	gtsam::Point3 transform_to(const gtsam::Point3& p) const; | ||||
| 
 | ||||
| 	// Standard Interface
 | ||||
| 	gtsam::Rot3 rotation() const; | ||||
| 	gtsam::Point3 translation() const; | ||||
| 	double x() const; | ||||
| 	double y() const; | ||||
| 	double z() const; | ||||
| 	Matrix matrix() const; | ||||
| 	Matrix adjointMap() const; | ||||
| 	gtsam::Pose3 compose(const gtsam::Pose3& p2); | ||||
| 	gtsam::Pose3 between(const gtsam::Pose3& p2); | ||||
| 	gtsam::Pose3 retract(Vector v); | ||||
| 	gtsam::Pose3 retractFirstOrder(Vector v); | ||||
| 	Vector localCoordinates(const gtsam::Pose3& T2) const; | ||||
| 	gtsam::Point3 translation() const; | ||||
| 	gtsam::Rot3 rotation() const; | ||||
| 	gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; | ||||
| 	double range(const gtsam::Point3& point); | ||||
| 	// double range(const gtsam::Pose3& pose);
 | ||||
| }; | ||||
| 
 | ||||
| class Cal3_S2 { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue