stereo VO Matlab example and data
Chris Beall
parent
7fdf3ef1bb
commit
1241bf383a
|
|
@ -0,0 +1 @@
|
|||
721.5377 721.5377 0.0 609.5593 172.854 0.537150588
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
1 1 0 0 0 0 1 0 0 -0 0 1 0 0 0 0 1
|
||||
2 0.999997 -0.00240146 0.00061075 0.00314304 0.0024019 0.999997 -0.00071977 0.00414596 -0.00060902 0.000721235 1 0.95998 0 0 0 1
|
||||
3 0.999993 -0.00371801 0.000595621 0.00280572 0.00371612 0.999988 0.00314729 0.00981461 -0.000607315 -0.00314506 0.999995 1.91967 0 0 0 1
|
||||
4 0.999981 -0.00619252 -0.000161076 0.00419847 0.00619374 0.999939 0.00918605 0.0120306 0.000104181 -0.00918687 0.999958 2.87292 0 0 0 1
|
||||
5 0.999948 -0.0100179 -0.00177824 0.00741887 0.010034 0.999907 0.00925915 0.0175144 0.00168532 -0.00927651 0.999956 3.8158 0 0 0 1
|
||||
6 0.999913 -0.0128105 -0.00298253 0.00649213 0.0128298 0.999896 0.00655406 0.0221085 0.00289826 -0.00659176 0.999974 4.75923 0 0 0 1
|
||||
7 0.999901 -0.0135442 -0.00393541 0.00202365 0.0135567 0.999903 0.00315259 0.0274932 0.00389233 -0.00320563 0.999987 5.69454 0 0 0 1
|
||||
8 0.999901 -0.0131232 -0.00504356 -0.00337588 0.0131365 0.99991 0.00261558 0.0312754 0.00500879 -0.00268157 0.999984 6.62905 0 0 0 1
|
||||
9 0.999898 -0.0127525 -0.00649016 -0.00834529 0.0127815 0.999908 0.00443934 0.0363095 0.00643296 -0.00452184 0.999969 7.56509 0 0 0 1
|
||||
10 0.999891 -0.0126057 -0.0076649 -0.0140367 0.012663 0.999892 0.00747804 0.0406566 0.00756981 -0.00757428 0.999943 8.50119 0 0 0 1
|
||||
11 0.99988 -0.012916 -0.00853732 -0.0314704 0.0129905 0.999878 0.00872879 0.0502508 0.00842353 -0.00883865 0.999925 9.44031 0 0 0 1
|
||||
12 0.999851 -0.0144077 -0.00947329 -0.0383227 0.0144958 0.999852 0.00930193 0.0627772 0.00933787 -0.00943787 0.999912 10.3736 0 0 0 1
|
||||
13 0.999838 -0.0146038 -0.0105024 -0.0473225 0.0147016 0.999849 0.00930217 0.0696137 0.010365 -0.00945507 0.999902 11.2945 0 0 0 1
|
||||
14 0.999827 -0.0148804 -0.0111878 -0.0611688 0.0149688 0.999857 0.00785918 0.0760175 0.0110692 -0.00802528 0.999907 12.2087 0 0 0 1
|
||||
15 0.999804 -0.015194 -0.0126986 -0.0750261 0.0152691 0.999866 0.0058357 0.0827557 0.0126082 -0.00602846 0.999902 13.1154 0 0 0 1
|
||||
16 0.999759 -0.0166787 -0.0142956 -0.0893963 0.0167446 0.99985 0.00450347 0.0886196 0.0142183 -0.00474176 0.999888 14.0218 0 0 0 1
|
||||
17 0.999738 -0.0164046 -0.0159548 -0.110797 0.0164665 0.999857 0.00375294 0.0939735 0.0158909 -0.00401468 0.999866 14.9288 0 0 0 1
|
||||
18 0.999731 -0.0152362 -0.0175058 -0.134602 0.0152899 0.999879 0.00293826 0.0949048 0.0174589 -0.00320513 0.999842 15.8347 0 0 0 1
|
||||
19 0.999721 -0.013717 -0.0192196 -0.158411 0.0137759 0.999901 0.00293494 0.0977547 0.0191774 -0.00319889 0.999811 16.7344 0 0 0 1
|
||||
20 0.999683 -0.0142193 -0.0207555 -0.182009 0.0142987 0.999891 0.00368405 0.101695 0.0207008 -0.00397966 0.999778 17.6352 0 0 0 1
|
||||
21 0.999659 -0.0137778 -0.0221853 -0.207494 0.01389 0.999891 0.00491306 0.105966 0.0221152 -0.00521954 0.999742 18.5239 0 0 0 1
|
||||
22 0.999637 -0.013439 -0.0233607 -0.237562 0.0135682 0.999893 0.0053802 0.112022 0.023286 -0.00569521 0.999713 19.408 0 0 0 1
|
||||
23 0.999585 -0.0146657 -0.0247774 -0.266515 0.0147976 0.999877 0.00515018 0.116865 0.0246988 -0.00551469 0.99968 20.2912 0 0 0 1
|
||||
24 0.999541 -0.0147341 -0.0264658 -0.293414 0.0148605 0.999879 0.00458559 0.12245 0.0263951 -0.00497678 0.999639 21.1689 0 0 0 1
|
||||
25 0.999508 -0.0146117 -0.0277547 -0.320586 0.0147561 0.999879 0.00500356 0.126076 0.0276782 -0.00541065 0.999602 22.0409 0 0 0 1
|
||||
26 0.999474 -0.0145772 -0.0289811 -0.347714 0.0147851 0.999866 0.00697181 0.131533 0.0288756 -0.00739663 0.999556 22.9037 0 0 0 1
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,77 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 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 Read Stereo Visual Odometry from file and optimize
|
||||
% @author Chris Beall
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%% Load calibration
|
||||
% format: fx fy skew cx cy baseline
|
||||
calib = dlmread('../Data/VO_calibration.txt');
|
||||
K = gtsamCal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
|
||||
stereo_model = gtsamSharedNoiseModel_Sigmas([1.0; 1.0; 1.0]);
|
||||
|
||||
%% create empty graph and values
|
||||
graph = visualSLAMGraph;
|
||||
initial = visualSLAMValues;
|
||||
|
||||
|
||||
%% load the initial poses from VO
|
||||
% row format: camera_id 4x4 pose (row, major)
|
||||
c = dlmread('../Data/VO_camera_poses.txt');
|
||||
|
||||
for i=1:size(c,1)
|
||||
pose = gtsamPose3(reshape(c(i,2:17),4,4)');
|
||||
initial.insertPose(symbol('x',c(i,1)),pose);
|
||||
end
|
||||
|
||||
%% load stereo measurements and initialize landmarks
|
||||
% camera_id landmark_id uL uR v X Y Z
|
||||
m = dlmread('../Data/VO_stereo_factors.txt');
|
||||
|
||||
for i=1:size(m,1)
|
||||
sf = m(i,:);
|
||||
graph.addStereoMeasurement(gtsamStereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ...
|
||||
symbol('x', sf(1)), symbol('l', sf(2)), K);
|
||||
|
||||
if ~initial.exists(symbol('l',sf(2)))
|
||||
% 3D landmarks are stored in camera coordinates: transform
|
||||
% to world coordinates using the respective initial pose
|
||||
pose = initial.pose(symbol('x', sf(1)));
|
||||
world_point = pose.transform_from(gtsamPoint3(sf(6),sf(7),sf(8)));
|
||||
initial.insertPoint(symbol('l',sf(2)), world_point);
|
||||
end
|
||||
end
|
||||
|
||||
%% add a constraint on the starting pose
|
||||
key = symbol('x',1);
|
||||
first_pose = initial.pose(key);
|
||||
graph.addPoseConstraint(symbol('x',1), first_pose);
|
||||
|
||||
%% optimize
|
||||
result = graph.optimize(initial);
|
||||
|
||||
%% visualize initial trajectory, final trajectory, and final points
|
||||
figure(1); clf;
|
||||
|
||||
% initial trajectory in red
|
||||
plot3(initial.xs(),initial.ys(),initial.zs(), '-*r','LineWidth',2);
|
||||
hold on;
|
||||
|
||||
% final trajectory in green
|
||||
plot3(result.xs(),result.ys(),result.zs(), '-*g','LineWidth',2);
|
||||
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
|
||||
|
||||
% switch to XZ view
|
||||
view([0 0]);
|
||||
|
||||
% optimized 3D points
|
||||
points = result.points();
|
||||
plot3(points(:,1),points(:,2),points(:,3),'.');
|
||||
|
||||
axis([-30 30 -30 30 0 60]);
|
||||
Loading…
Reference in New Issue