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

Reinstated 'fix/matlab_examples_wrapper' changes

release/4.3a0
dellaert 2014-12-28 18:44:44 +01:00
parent 7c455efe50
commit 6b9c21b942
10 changed files with 52 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
matlab/unstable_examples/FlightCameraTransformIMU.m
View File

@ -57,7 +57,7 @@ isamParams.setFactorization('QR');
isam = ISAM2(isamParams);
%% Get initial conditions for the estimated trajectory
currentVelocityGlobal = LieVector([10;0;0]); % (This is slightly wrong!)
currentVelocityGlobal = [10;0;0]; % (This is slightly wrong!) Zhaoyang: Fixed
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
@ -101,7 +101,7 @@ axis equal;
for i=1:size(trajectory)-1
xKey = symbol('x',i);
pose = trajectory.at(xKey); % GT pose
pose = trajectory.atPose3(xKey); % GT pose
pose_t = pose.translation(); % GT pose-translation
if exist('h_cursor','var')
@ -165,13 +165,13 @@ for i=1:size(trajectory)-1
if i > 1
xKey_prev = symbol('x',i-1);
pose_prev = trajectory.at(xKey_prev);
pose_prev = trajectory.atPose3(xKey_prev);
step = pose_prev.between(pose);
% insert estimate for current pose with some normal noise on
% translation
initial.insert(xKey,result.at(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)])));
initial.insert(xKey,result.atPose3(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)])));
% visual measurements
if measurements.size > 0 && use_camera
@ -181,12 +181,12 @@ for i=1:size(trajectory)-1
zKey = measurementKeys.at(zz);
lKey = symbol('l',symbolIndex(zKey));
fg.add(TransformCalProjectionFactorCal3_S2(measurements.at(zKey), ...
fg.add(TransformCalProjectionFactorCal3_S2(measurements.atPoint2(zKey), ...
z_cov, xKey, transformKey, lKey, calibrationKey, false, true));
% only add landmark to values if doesn't exist yet
if ~result.exists(lKey)
noisy_landmark = landmarks.at(lKey).compose(Point3(normrnd(0,landmark_noise,3,1)));
noisy_landmark = landmarks.atPoint3(lKey).compose(Point3(normrnd(0,landmark_noise,3,1)));
initial.insert(lKey, noisy_landmark);
% and add a prior since its position is known

14
matlab/unstable_examples/IMUKittiExampleAdvanced.m
View File

@ -7,7 +7,7 @@ disp('Example of application of ISAM2 for visual-inertial navigation on the KITT
%% Read metadata and compute relative sensor pose transforms
% IMU metadata
disp('-- Reading sensor metadata')
IMU_metadata = importdata('KittiEquivBiasedImu_metadata.txt');
IMU_metadata = importdata(findExampleDataFile('KittiEquivBiasedImu_metadata.txt'));
IMU_metadata = cell2struct(num2cell(IMU_metadata.data), IMU_metadata.colheaders, 2);
IMUinBody = Pose3.Expmap([IMU_metadata.BodyPtx; IMU_metadata.BodyPty; IMU_metadata.BodyPtz;
IMU_metadata.BodyPrx; IMU_metadata.BodyPry; IMU_metadata.BodyPrz; ]);
@ -16,14 +16,14 @@ if ~IMUinBody.equals(Pose3, 1e-5)
end
% VO metadata
VO_metadata = importdata('KittiRelativePose_metadata.txt');
VO_metadata = importdata(findExampleDataFile('KittiRelativePose_metadata.txt'));
VO_metadata = cell2struct(num2cell(VO_metadata.data), VO_metadata.colheaders, 2);
VOinBody = Pose3.Expmap([VO_metadata.BodyPtx; VO_metadata.BodyPty; VO_metadata.BodyPtz;
VO_metadata.BodyPrx; VO_metadata.BodyPry; VO_metadata.BodyPrz; ]);
VOinIMU = IMUinBody.inverse().compose(VOinBody);
% GPS metadata
GPS_metadata = importdata('KittiGps_metadata.txt');
GPS_metadata = importdata(findExampleDataFile('KittiGps_metadata.txt'));
GPS_metadata = cell2struct(num2cell(GPS_metadata.data), GPS_metadata.colheaders, 2);
GPSinBody = Pose3.Expmap([GPS_metadata.BodyPtx; GPS_metadata.BodyPty; GPS_metadata.BodyPtz;
GPS_metadata.BodyPrx; GPS_metadata.BodyPry; GPS_metadata.BodyPrz; ]);
@ -32,7 +32,7 @@ GPSinIMU = IMUinBody.inverse().compose(GPSinBody);
%% Read data and change coordinate frame of GPS and VO measurements to IMU frame
disp('-- Reading sensor data from file')
% IMU data
IMU_data = importdata('KittiEquivBiasedImu.txt');
IMU_data = importdata(findExampleDataFile('KittiEquivBiasedImu.txt'));
IMU_data = cell2struct(num2cell(IMU_data.data), IMU_data.colheaders, 2);
imum = cellfun(@(x) x', num2cell([ [IMU_data.accelX]' [IMU_data.accelY]' [IMU_data.accelZ]' [IMU_data.omegaX]' [IMU_data.omegaY]' [IMU_data.omegaZ]' ], 2), 'UniformOutput', false);
[IMU_data.acc_omega] = deal(imum{:});
@ -40,7 +40,7 @@ imum = cellfun(@(x) x', num2cell([ [IMU_data.accelX]' [IMU_data.accelY]' [IMU_da
clear imum
% VO data
VO_data = importdata('KittiRelativePose.txt');
VO_data = importdata(findExampleDataFile('KittiRelativePose.txt'));
VO_data = cell2struct(num2cell(VO_data.data), VO_data.colheaders, 2);
% Merge relative pose fields and convert to Pose3
logposes = [ [VO_data.dtx]' [VO_data.dty]' [VO_data.dtz]' [VO_data.drx]' [VO_data.dry]' [VO_data.drz]' ];
@ -71,7 +71,7 @@ clear logposes relposes
%% Get initial conditions for the estimated trajectory
% % % currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame)
currentPoseGlobal = Pose3;
currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning
currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_x = noiseModel.Isotropic.Sigma(6, 0.01);
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0);
@ -120,7 +120,7 @@ for measurementIndex = 1:length(timestamps)
newValues.insert(currentVelKey, currentVelocityGlobal);
newValues.insert(currentBiasKey, currentBias);
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x));
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
else
t_previous = timestamps(measurementIndex-1, 1);

4
matlab/unstable_examples/SmartRangeFactorExample.m
View File

@ -82,8 +82,8 @@ for ind_pose = 2:7
key_prev = poseKey(ind_pose-1);
key_curr = poseKey(ind_pose);
prev_pose = smartValues.at(key_prev);
curr_pose = smartValues.at(key_curr);
prev_pose = smartValues.atPose2(key_prev);
curr_pose = smartValues.atPose2(key_curr);
GTDeltaPose = prev_pose.between(curr_pose);
noisePose = Pose2([t_noise*rand; t_noise*rand; r_noise*rand]);
NoisyDeltaPose = GTDeltaPose.compose(noisePose);

24
matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m
View File

@ -96,17 +96,17 @@ for i=1:20
if i > 1
if i < 11
initial.insert(i,result.at(i-1).compose(move_forward));
initial.insert(i,result.atPose3(i-1).compose(move_forward));
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
else
initial.insert(i,result.at(i-1).compose(move_circle));
initial.insert(i,result.atPose3(i-1).compose(move_circle));
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
end
end
% generate some camera measurements
cam_pose = initial.at(i).compose(actual_transform);
cam_pose = initial.atPose3(i).compose(actual_transform);
% gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
i
@ -137,10 +137,10 @@ for i=1:20
hold on;
%% plot results
result_camera_transform = result.at(transformKey);
result_camera_transform = result.atPose3(transformKey);
for j=1:i
gtsam.plotPose3(result.at(j),[],0.5);
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
gtsam.plotPose3(result.atPose3(j),[],0.5);
gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5);
end
xlabel('x (m)');
@ -153,12 +153,12 @@ for i=1:20
% axis equal
for l=101:100+nrPoints
plotPoint3(result.at(l),'g');
plotPoint3(result.atPoint3(l),'g');
end
ty = result.at(transformKey).translation().y();
fx = result.at(calibrationKey).fx();
fy = result.at(calibrationKey).fy();
ty = result.atPose3(transformKey).translation().y();
fx = result.atCal3_S2(calibrationKey).fx();
fy = result.atCal3_S2(calibrationKey).fy();
text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty));
text(1,5,3,sprintf('fx(900): %.0f',fx));
text(1,5,1,sprintf('fy(900): %.0f',fy));
@ -180,10 +180,10 @@ end
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
disp('Transform after optimization');
result.at(transformKey)
result.atPose3(transformKey)
disp('Calibration after optimization');
result.at(calibrationKey)
result.atCal3_S2(calibrationKey)

4
matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
View File

@ -89,7 +89,7 @@ isam = ISAM2(isamParams);
currentIMUPoseGlobal = Pose3();
%% Get initial conditions for the estimated trajectory
currentVelocityGlobal = LieVector([1;0;0]); % the vehicle is stationary at the beginning
currentVelocityGlobal = [1;0;0]; % the vehicle is stationary at the beginning
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
@ -127,7 +127,7 @@ for i=1:steps
fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov));
% velocity and bias evolution
fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
result = initial;

12
matlab/unstable_examples/TransformProjectionFactorExample.m
View File

@ -61,17 +61,17 @@ cheirality_exception_count = 0;
for i=1:20
if i > 1
if i < 11
initial.insert(i,initial.at(i-1).compose(move_forward));
initial.insert(i,initial.atPose3(i-1).compose(move_forward));
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
else
initial.insert(i,initial.at(i-1).compose(move_circle));
initial.insert(i,initial.atPose3(i-1).compose(move_circle));
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
end
end
% generate some camera measurements
cam_pose = initial.at(i).compose(actual_transform);
cam_pose = initial.atPose3(i).compose(actual_transform);
gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
i
@ -93,14 +93,14 @@ fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
%% camera plotting
for i=1:20
gtsam.plotPose3(initial.at(i).compose(camera_transform));
gtsam.plotPose3(initial.atPose3(i).compose(camera_transform));
end
xlabel('x (m)');
ylabel('y (m)');
disp('Transform before optimization');
initial.at(1000)
initial.atPose3(1000)
params = LevenbergMarquardtParams;
params.setAbsoluteErrorTol(1e-15);
@ -112,7 +112,7 @@ optimizer = LevenbergMarquardtOptimizer(fg, initial, params);
result = optimizer.optimizeSafely();
disp('Transform after optimization');
result.at(1000)
result.atPose3(1000) % results is empty here. optimizer doesn't generate result?
axis([0 25 0 25 0 10]);
axis equal

18
matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
View File

@ -86,17 +86,17 @@ for i=1:20
if i > 1
if i < 11
initial.insert(i,result.at(i-1).compose(move_forward));
initial.insert(i,result.atPose3(i-1).compose(move_forward));
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
else
initial.insert(i,result.at(i-1).compose(move_circle));
initial.insert(i,result.atPose3(i-1).compose(move_circle));
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
end
end
% generate some camera measurements
cam_pose = initial.at(i).compose(actual_transform);
cam_pose = initial.atPose3(i).compose(actual_transform);
% gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
i
@ -127,10 +127,10 @@ for i=1:20
hold on;
%% plot results
result_camera_transform = result.at(1000);
result_camera_transform = result.atPose3(1000);
for j=1:i
gtsam.plotPose3(result.at(j));
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
gtsam.plotPose3(result.atPose3(j));
gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5);
end
xlabel('x (m)');
@ -143,10 +143,10 @@ for i=1:20
% axis equal
for l=101:100+nrPoints
plotPoint3(result.at(l),'g');
plotPoint3(result.atPoint3(l),'g');
end
ty = result.at(1000).translation().y();
ty = result.atPose3(1000).translation().y();
text(5,5,5,sprintf('Y-Transform: %0.2g',ty));
if(write_video)
@ -168,7 +168,7 @@ fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
disp('Transform after optimization');
result.at(1000)
result.atPose3(1000)

2
matlab/unstable_examples/plot_projected_landmarks.m
View File

@ -23,7 +23,7 @@ z = zeros(1,nrMeasurements);
for i = 0:measurement_keys.size-1
key = measurement_keys.at(i);
key_index = gtsam.symbolIndex(key);
p = landmarks.at(gtsam.symbol('l',key_index));
p = landmarks.atPoint3(gtsam.symbol('l',key_index));
x(i+1) = p.x;
y(i+1) = p.y;

2
matlab/unstable_examples/project_landmarks.m
View File

@ -8,7 +8,7 @@ function [ measurements ] = project_landmarks( pose, landmarks, K )
measurements = Values;
for i=1:size(landmarks)-1
z = camera.project(landmarks.at(symbol('l',i)));
z = camera.project(landmarks.atPoint3(symbol('l',i)));
% check bounding box
if z.x < 0 || z.x > 1280

12
matlab/unstable_examples/testTSAMFactors.m
View File

@ -48,12 +48,12 @@ optimizer = LevenbergMarquardtOptimizer(graph, initial, params);
result = optimizer.optimize();
% Check result
CHECK('error',result.at(100).equals(b1,1e-5))
CHECK('error',result.at(10).equals(origin,1e-5))
CHECK('error',result.at(1).equals(Point2(0,1),1e-5))
CHECK('error',result.at(2).equals(Point2(0,1),1e-5))
CHECK('error',result.at(20).equals(origin,1e-5))
CHECK('error',result.at(200).equals(b2,1e-5))
CHECK('error',result.atPose2(100).equals(b1,1e-5))
CHECK('error',result.atPose2(10).equals(origin,1e-5))
CHECK('error',result.atPoint2(1).equals(Point2(0,1),1e-5))
CHECK('error',result.atPoint2(2).equals(Point2(0,1),1e-5))
CHECK('error',result.atPose2(20).equals(origin,1e-5))
CHECK('error',result.atPose2(200).equals(b2,1e-5))
% Check error
CHECK('error',abs(graph.error(result))<1e-9)