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

Fix plot_projected_landmarks.m project_landmarks.m 

Haven't finish FlightCameraTransformIMU.m, stuck at TransformCalProjectionFactorCal3_S2
release/4.3a0
lvzhaoyang 2014-12-07 00:23:17 -05:00
parent 615709dd49
commit 924a8d6670
1 changed files with 6 additions and 6 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); isam = ISAM2(isamParams);
%% Get initial conditions for the estimated trajectory %% 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)); currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
@ -101,7 +101,7 @@ axis equal;
for i=1:size(trajectory)-1 for i=1:size(trajectory)-1
xKey = symbol('x',i); xKey = symbol('x',i);
pose = trajectory.at(xKey); % GT pose pose = trajectory.atPose3(xKey); % GT pose
pose_t = pose.translation(); % GT pose-translation pose_t = pose.translation(); % GT pose-translation
if exist('h_cursor','var') if exist('h_cursor','var')
@ -165,13 +165,13 @@ for i=1:size(trajectory)-1
if i > 1 if i > 1
xKey_prev = symbol('x',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); step = pose_prev.between(pose);
% insert estimate for current pose with some normal noise on % insert estimate for current pose with some normal noise on
% translation % 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 % visual measurements
if measurements.size > 0 && use_camera if measurements.size > 0 && use_camera
@ -181,12 +181,12 @@ for i=1:size(trajectory)-1
zKey = measurementKeys.at(zz); zKey = measurementKeys.at(zz);
lKey = symbol('l',symbolIndex(zKey)); 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)); z_cov, xKey, transformKey, lKey, calibrationKey, false, true));
% only add landmark to values if doesn't exist yet % only add landmark to values if doesn't exist yet
if ~result.exists(lKey) 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); initial.insert(lKey, noisy_landmark);
% and add a prior since its position is known % and add a prior since its position is known