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

Some comments, formatting

release/4.3a0
dellaert 2016-04-10 11:12:46 -07:00
parent 2c0c3d1955
commit 69c182d5a4
1 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
matlab/unstable_examples/FlightCameraTransformIMU.m
View File

@ -1,3 +1,7 @@
% Simulation for concurrent IMU, camera, IMU-camera transform estimation during flight with known landmarks
% author: Chris Beall
% date: July 2014
clear all; clear all;
clf; clf;
@ -24,7 +28,7 @@ if(write_video)
open(videoObj); open(videoObj);
end end
% IMU parameters %% IMU parameters
IMU_metadata.AccelerometerSigma = 1e-2; IMU_metadata.AccelerometerSigma = 1e-2;
IMU_metadata.GyroscopeSigma = 1e-2; IMU_metadata.GyroscopeSigma = 1e-2;
IMU_metadata.AccelerometerBiasSigma = 1e-6; IMU_metadata.AccelerometerBiasSigma = 1e-6;
@ -104,7 +108,9 @@ zlabel('z');
title('Estimated vs. actual IMU-cam transform'); title('Estimated vs. actual IMU-cam transform');
axis equal; axis equal;
%% Main loop
for i=1:size(trajectory)-1 for i=1:size(trajectory)-1
%% Preliminaries
xKey = symbol('x',i); xKey = symbol('x',i);
pose = trajectory.atPose3(xKey); % GT pose pose = trajectory.atPose3(xKey); % GT pose
pose_t = pose.translation(); % GT pose-translation pose_t = pose.translation(); % GT pose-translation
@ -139,6 +145,7 @@ for i=1:size(trajectory)-1
gps_pose = pose.retract([0; 0; 0; normrnd(0,gps_noise,3,1)]); gps_pose = pose.retract([0; 0; 0; normrnd(0,gps_noise,3,1)]);
fg.add(PoseTranslationPrior3D(xKey, gps_pose, GPS_trans_cov)); fg.add(PoseTranslationPrior3D(xKey, gps_pose, GPS_trans_cov));
%% First-time initialization
if i==1 if i==1
% camera transform % camera transform
if use_camera_transform_noise if use_camera_transform_noise
@ -158,12 +165,10 @@ for i=1:size(trajectory)-1
result = initial; result = initial;
end end
% priors on first two poses %% priors on first two poses
if i < 3 if i < 3
% fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); % fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
end end
%% the 'normal' case %% the 'normal' case
@ -202,14 +207,12 @@ for i=1:size(trajectory)-1
end end
end % end landmark observations end % end landmark observations
%% IMU %% IMU
deltaT = 1; deltaT = 1;
logmap = Pose3.Logmap(step); logmap = Pose3.Logmap(step);
omega = logmap(1:3); omega = logmap(1:3);
velocity = logmap(4:6); velocity = logmap(4:6);
% Simulate IMU measurements, considering Coriolis effect % Simulate IMU measurements, considering Coriolis effect
% (in this simple example we neglect gravity and there are no other forces acting on the body) % (in this simple example we neglect gravity and there are no other forces acting on the body)
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
@ -234,7 +237,7 @@ for i=1:size(trajectory)-1
fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
noiseModel.Diagonal.Sigmas(sqrt(10) * sigma_between_b))); noiseModel.Diagonal.Sigmas(sqrt(10) * sigma_between_b)));
% ISAM update %% ISAM update
isam.update(fg, initial); isam.update(fg, initial);
result = isam.calculateEstimate(); result = isam.calculateEstimate();
@ -300,10 +303,8 @@ for i=1:size(trajectory)-1
end end
% print out final camera transform %% print out final camera transform and write video
result.at(transformKey); result.at(transformKey);
if(write_video) if(write_video)
close(videoObj); close(videoObj);
end end