12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Pose3SLAM example works (MATLAB only)

release/4.3a0
Frank Dellaert 2012-06-04 05:53:51 +00:00
parent 198279f4ff
commit be7828b8cf
4 changed files with 92 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
examples/matlab/Pose3SLAMExample_graph.m
View File

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

36
examples/matlab/load3D.m
View File

@ -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

15
examples/matlab/plot3DTrajectory.m Normal file
View File

@ -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
View File

@ -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 {