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

Small fix of IMUKittiExampleAdvanced. Now the same priorFactor issue

release/4.3a0
lvzhaoyang 2014-12-08 10:41:52 -05:00
parent b881135f53
commit 6d97b8d3db
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
matlab/unstable_examples/IMUKittiExampleAdvanced.m
View File

@ -71,7 +71,7 @@ clear logposes relposes
%% Get initial conditions for the estimated trajectory %% Get initial conditions for the estimated trajectory
% % % currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame) % % % currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame)
currentPoseGlobal = Pose3; 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)); currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_x = noiseModel.Isotropic.Sigma(6, 0.01); sigma_init_x = noiseModel.Isotropic.Sigma(6, 0.01);
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0);
@ -120,7 +120,7 @@ for measurementIndex = 1:length(timestamps)
newValues.insert(currentVelKey, currentVelocityGlobal); newValues.insert(currentVelKey, currentVelocityGlobal);
newValues.insert(currentBiasKey, currentBias); newValues.insert(currentBiasKey, currentBias);
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); 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)); newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
else else
t_previous = timestamps(measurementIndex-1, 1); t_previous = timestamps(measurementIndex-1, 1);