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

Rotated display so Z is up, made axis square

release/4.3a0
Frank Dellaert 2012-06-07 03:54:48 +00:00
parent 2414bfc3c9
commit 903580abb4
1 changed files with 26 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
examples/matlab/VO_Example.m
View File

@ -23,55 +23,60 @@ initial = visualSLAMValues;
%% load the initial poses from VO %% load the initial poses from VO
% row format: camera_id 4x4 pose (row, major) % row format: camera_id 4x4 pose (row, major)
c = dlmread('../Data/VO_camera_poses.txt'); fprintf(1,'Reading data\n');
cameras = dlmread('../Data/VO_camera_poses.txt');
for i=1:size(c,1) for i=1:size(cameras,1)
pose = gtsamPose3(reshape(c(i,2:17),4,4)'); pose = gtsamPose3(reshape(cameras(i,2:17),4,4)');
initial.insertPose(symbol('x',c(i,1)),pose); initial.insertPose(symbol('x',cameras(i,1)),pose);
end end
%% load stereo measurements and initialize landmarks %% load stereo measurements and initialize landmarks
% camera_id landmark_id uL uR v X Y Z % camera_id landmark_id uL uR v X Y Z
m = dlmread('../Data/VO_stereo_factors.txt'); measurements = dlmread('../Data/VO_stereo_factors.txt');
for i=1:size(m,1) fprintf(1,'Creating Graph\n'); tic
sf = m(i,:); for i=1:size(measurements,1)
sf = measurements(i,:);
graph.addStereoMeasurement(gtsamStereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ... graph.addStereoMeasurement(gtsamStereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ...
symbol('x', sf(1)), symbol('l', sf(2)), K); symbol('x', sf(1)), symbol('l', sf(2)), K);
if ~initial.exists(symbol('l',sf(2))) if ~initial.exists(symbol('l',sf(2)))
% 3D landmarks are stored in camera coordinates: transform % 3D landmarks are stored in camera coordinates: transform
% to world coordinates using the respective initial pose % to world coordinates using the respective initial pose
pose = initial.pose(symbol('x', sf(1))); pose = initial.pose(symbol('x', sf(1)));
world_point = pose.transform_from(gtsamPoint3(sf(6),sf(7),sf(8))); world_point = pose.transform_from(gtsamPoint3(sf(6),sf(7),sf(8)));
initial.insertPoint(symbol('l',sf(2)), world_point); initial.insertPoint(symbol('l',sf(2)), world_point);
end end
end end
toc
%% add a constraint on the starting pose %% add a constraint on the starting pose
key = symbol('x',1); key = symbol('x',1);
first_pose = initial.pose(key); first_pose = initial.pose(key);
graph.addPoseConstraint(symbol('x',1), first_pose); graph.addPoseConstraint(key, first_pose);
%% optimize %% optimize
fprintf(1,'Optimizing\n'); tic
result = graph.optimize(initial); result = graph.optimize(initial);
toc
%% visualize initial trajectory, final trajectory, and final points %% visualize initial trajectory, final trajectory, and final points
figure(1); clf; figure(1); clf; hold on;
% initial trajectory in red % initial trajectory in red (rotated so Z is up)
plot3(initial.xs(),initial.ys(),initial.zs(), '-*r','LineWidth',2); plot3(initial.zs(),-initial.xs(),-initial.ys(), '-*r','LineWidth',2);
hold on;
% final trajectory in green % final trajectory in green (rotated so Z is up)
plot3(result.xs(),result.ys(),result.zs(), '-*g','LineWidth',2); plot3(result.zs(),-result.xs(),-result.ys(), '-*g','LineWidth',2);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
% switch to XZ view % switch to XZ view
view([0 0]); view([0 0]);
% optimized 3D points % optimized 3D points (rotated so Z is up)
points = result.points(); points = result.points();
plot3(points(:,1),points(:,2),points(:,3),'.'); plot3(points(:,3),-points(:,1),-points(:,2),'.');
axis([-30 30 -30 30 0 60]); axis([0 100 -20 20 -5 20]);
axis equal
view(3)