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

Removed noise from pose prior for non-BetweenFactors. Added saving figures as .png

release/4.3a0
djensen3 2014-04-24 10:24:44 -04:00
parent c1ab0053eb
commit 20eeb90682
2 changed files with 17 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
View File

@ -11,15 +11,15 @@ clc
clear all clear all
close all close all
saveResults = 1; saveResults = 0;
%% Configuration %% Configuration
options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj
options.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses
options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities)
options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1)
options.imuNonzeroBias = 1; % if true, a nonzero bias is applied to IMU measurements options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements
options.includeCameraFactors = 0; % not fully implementemeasurements, values, noiseModels, measurementNoise, options, metadatad yet options.includeCameraFactors = 0; % not fully implementemeasurements, values, noiseModels, measurementNoise, options, metadatad yet
numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors
@ -30,7 +30,7 @@ options.gpsStartPose = 100; % Pose number to start including GPS factors
options.trajectoryLength = 209; % length of the ground truth trajectory options.trajectoryLength = 209; % length of the ground truth trajectory
options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories)
numMonteCarloRuns = 100; % number of Monte Carlo runs to perform numMonteCarloRuns = 2; % number of Monte Carlo runs to perform
%% Camera metadata %% Camera metadata
K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration
@ -54,8 +54,8 @@ metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
metadata.imu.AccelerometerSigma = 1e-3; metadata.imu.AccelerometerSigma = 1e-3;
metadata.imu.GyroscopeSigma = 1e-5; metadata.imu.GyroscopeSigma = 1e-5;
metadata.imu.IntegrationSigma = 1e-5; metadata.imu.IntegrationSigma = 1e-5;
metadata.imu.BiasAccelerometerSigma = 1e-4; %metadata.imu.epsBias; metadata.imu.BiasAccelerometerSigma = 1e-10; %metadata.imu.epsBias;
metadata.imu.BiasGyroscopeSigma = 1e-4; %metadata.imu.epsBias; metadata.imu.BiasGyroscopeSigma = 1e-10; %metadata.imu.epsBias;
metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias; metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias;
metadata.imu.g = [0;0;0]; metadata.imu.g = [0;0;0];
metadata.imu.omegaCoriolis = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0];
@ -151,7 +151,7 @@ monteCarloMeasurementNoise.cameraPixelNoiseVector = [0; 0];
monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS; monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS;
for k=1:numMonteCarloRuns for k=1:numMonteCarloRuns
fprintf('Monte Carlo Run %d.\n', k'); fprintf('Monte Carlo Run %d...', k');
% Create a random bias for each run % Create a random bias for each run
if options.imuNonzeroBias == 1 if options.imuNonzeroBias == 1
@ -176,7 +176,8 @@ for k=1:numMonteCarloRuns
% optimize % optimize
optimizer = GaussNewtonOptimizer(graph, gtValues); optimizer = GaussNewtonOptimizer(graph, gtValues);
estimate = optimizer.optimize(); estimate = optimizer.optimize();
fprintf('\n\tEstimate error: %g \n', graph.error(estimate) );
fprintf('\tEstimate error (GT): %g \n', graph.error(gtValues) );
figure(1) figure(1)
plot3DTrajectory(estimate, '-b'); plot3DTrajectory(estimate, '-b');
@ -212,6 +213,7 @@ set(gca,'Fontsize',16)
title('NEES and ANEES'); title('NEES and ANEES');
if saveResults if saveResults
saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig'); saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig');
saveas(gcf,horzcat(folderName,'runs-',testName,'.png'),'png');
end end
%% %%
@ -221,6 +223,7 @@ set(gca,'Fontsize',16)
title('Ground truth and estimates for each MC runs'); title('Ground truth and estimates for each MC runs');
if saveResults if saveResults
saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig'); saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig');
saveas(gcf,horzcat(folderName,'gt-',testName,'.png'),'png');
end end
%% Let us compute statistics on the overall NEES %% Let us compute statistics on the overall NEES
@ -248,6 +251,7 @@ set(gca,'Fontsize',16)
title('NEES normalized by dof VS bounds'); title('NEES normalized by dof VS bounds');
if saveResults if saveResults
saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig'); saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig');
saveas(gcf,horzcat(folderName,'ANEES-',testName,'.png'),'png');
logFile = horzcat(folderName,'log-',testName); logFile = horzcat(folderName,'log-',testName);
save(logFile) save(logFile)
end end

4
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -19,7 +19,11 @@ for i=0:length(measurements)
if i==0 if i==0
%% first time step, add priors %% first time step, add priors
% Pose prior (poses used for all factors) % Pose prior (poses used for all factors)
if options.includeBetweenFactors == 1
initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1));
else
initialPose = Pose3;
end
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
% IMU velocity and bias priors % IMU velocity and bias priors