fixed bug with missing priors. Added IMU noise measurements to Monte Carlo runs
djensen3
parent
1432fb773b
commit
b5f9862274
|
|
@ -10,11 +10,11 @@ clear all
|
|||
close all
|
||||
|
||||
%% Configuration
|
||||
options.useRealData = 0; % 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 generated between consecutive poses
|
||||
options.includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the trajectory
|
||||
options.includeCameraFactors = 0; % not fully implemented yet
|
||||
options.trajectoryLength = 4; % length of the ground truth trajectory
|
||||
options.trajectoryLength = 209; % length of the ground truth trajectory
|
||||
options.subsampleStep = 20;
|
||||
|
||||
numMonteCarloRuns = 2;
|
||||
|
|
@ -51,8 +51,14 @@ noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1
|
|||
noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias);
|
||||
noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-4);
|
||||
|
||||
sigma_accel = metadata.imu.AccelerometerSigma;
|
||||
sigma_gyro = metadata.imu.GyroscopeSigma;
|
||||
noiseVectorAccel = [sigma_accel; sigma_accel; sigma_accel];
|
||||
noiseVectorGyro = [sigma_gyro; sigma_gyro; sigma_gyro];
|
||||
|
||||
|
||||
%% Between metadata
|
||||
sigma_ang = 1e-2; sigma_cart = 1;
|
||||
sigma_ang = 1e-3; sigma_cart = 1e-3;
|
||||
noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart];
|
||||
noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose);
|
||||
%noisePose = noiseModel.Isotropic.Sigma(6, 1e-3);
|
||||
|
|
@ -73,10 +79,10 @@ gtNoiseModels.noisePriorPose = noisePose;
|
|||
gtNoiseModels.noisePriorBias = noisePriorBias;
|
||||
|
||||
% Set measurement noise to 0, because this is ground truth
|
||||
gtMeasurementNoise.poseNoiseVector = [0 0 0 0 0 0];
|
||||
gtMeasurementNoise.imu.accelNoiseVector = [0 0 0];
|
||||
gtMeasurementNoise.imu.gyroNoiseVector = [0 0 0];
|
||||
gtMeasurementNoise.cameraPixelNoiseVector = [0 0];
|
||||
gtMeasurementNoise.poseNoiseVector = [0; 0; 0; 0; 0; 0;];
|
||||
gtMeasurementNoise.imu.accelNoiseVector = [0; 0; 0];
|
||||
gtMeasurementNoise.imu.gyroNoiseVector = [0; 0; 0];
|
||||
gtMeasurementNoise.cameraPixelNoiseVector = [0; 0];
|
||||
|
||||
gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
|
||||
gtMeasurements, ... % ground truth measurements
|
||||
|
|
@ -87,8 +93,8 @@ gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
|
|||
metadata); % misc data necessary for factor creation
|
||||
|
||||
%% Display, printing, and plotting of ground truth
|
||||
gtGraph.print(sprintf('\nGround Truth Factor graph:\n'));
|
||||
gtValues.print(sprintf('\nGround Truth Values:\n '));
|
||||
%gtGraph.print(sprintf('\nGround Truth Factor graph:\n'));
|
||||
%gtValues.print(sprintf('\nGround Truth Values:\n '));
|
||||
|
||||
warning('Additional prior on zerobias')
|
||||
warning('Additional PriorFactorLieVector on velocities')
|
||||
|
|
@ -102,40 +108,32 @@ axis equal
|
|||
disp('Plotted ground truth')
|
||||
|
||||
%% Monte Carlo Runs
|
||||
|
||||
% Set up noise models
|
||||
monteCarloNoiseModels.noisePose = noisePose;
|
||||
monteCarloNoiseModels.noiseVel = noiseVel;
|
||||
monteCarloNoiseModels.noiseBias = noiseBias;
|
||||
monteCarloNoiseModels.noisePriorPose = noisePose;
|
||||
monteCarloNoiseModels.noisePriorBias = noisePriorBias;
|
||||
|
||||
% Set measurement noise for monte carlo runs
|
||||
monteCarloMeasurementNoise.poseNoiseVector = noiseVectorPose;
|
||||
monteCarloMeasurementNoise.imu.accelNoiseVector = noiseVectorAccel;
|
||||
monteCarloMeasurementNoise.imu.gyroNoiseVector = noiseVectorGyro;
|
||||
monteCarloMeasurementNoise.cameraPixelNoiseVector = [0; 0];
|
||||
|
||||
for k=1:numMonteCarloRuns
|
||||
fprintf('Monte Carlo Run %d.\n', k');
|
||||
|
||||
% create a new graph
|
||||
graph = NonlinearFactorGraph;
|
||||
|
||||
% noisy prior
|
||||
currentPoseKey = symbol('x', 0);
|
||||
currentPose = gtValues.at(currentPoseKey);
|
||||
gtMeasurements.posePrior = currentPose;
|
||||
noisyDelta = noiseVectorPose .* randn(6,1);
|
||||
noisyInitialPose = Pose3.Expmap(noisyDelta);
|
||||
graph.add(PriorFactorPose3(currentPoseKey, noisyInitialPose, noisePose));
|
||||
|
||||
for i=1:size(gtMeasurements.deltaMatrix,1)
|
||||
currentPoseKey = symbol('x', i);
|
||||
|
||||
% for each measurement: add noise and add to graph
|
||||
noisyDelta = gtMeasurements.deltaMatrix(i,:)' + (noiseVectorPose .* randn(6,1));
|
||||
noisyDeltaPose = Pose3.Expmap(noisyDelta);
|
||||
|
||||
% Add the factors to the factor graph
|
||||
graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose));
|
||||
end
|
||||
|
||||
% graph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
|
||||
% gtMeasurements, ... % ground truth measurements
|
||||
% gtValues, ... % ground truth Values
|
||||
% gtNoiseModels, ... % noise models to use in this graph
|
||||
% gtMeasurementNoise, ... % noise to apply to measurements
|
||||
% options, ... % options for the graph (e.g. which factors to include)
|
||||
% metadata); % misc data necessary for factor creation
|
||||
% Create a new graph using noisy measurements
|
||||
graph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
|
||||
gtMeasurements, ... % ground truth measurements
|
||||
gtValues, ... % ground truth Values
|
||||
monteCarloNoiseModels, ... % noise models to use in this graph
|
||||
monteCarloMeasurementNoise, ... % noise to apply to measurements
|
||||
options, ... % options for the graph (e.g. which factors to include)
|
||||
metadata); % misc data necessary for factor creation
|
||||
|
||||
|
||||
%graph.print('graph')
|
||||
|
||||
% optimize
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
function [ graph, values ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata)
|
||||
function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata)
|
||||
% Create a factor graph based on provided measurements, values, and noises.
|
||||
% Used for covariance analysis scripts.
|
||||
% 'options' contains fields for including various factor types.
|
||||
|
|
@ -20,19 +20,21 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
%% first time step, add priors
|
||||
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
|
||||
warning('using identity rotation')
|
||||
graph.add(PriorFactorPose3(currentPoseKey, currentPose, noiseModels.noisePose));
|
||||
measurements.posePrior = currentPose;
|
||||
|
||||
% Pose prior (poses used for all factors)
|
||||
initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1));
|
||||
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
|
||||
|
||||
% IMU velocity and bias priors
|
||||
if options.includeIMUFactors == 1
|
||||
currentVelKey = symbol('v', 0);
|
||||
currentBiasKey = symbol('b', 0);
|
||||
currentVel = [0; 0; 0];
|
||||
values.insert(currentVelKey, LieVector(currentVel));
|
||||
values.insert(currentBiasKey, metadata.imu.zeroBias);
|
||||
graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
|
||||
graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, noiseModels.noisePriorBias));
|
||||
end
|
||||
|
||||
% Camera priors
|
||||
if options.includeCameraFactors == 1
|
||||
pointNoiseSigma = 0.1;
|
||||
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
|
||||
|
|
@ -46,7 +48,7 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
if options.includeBetweenFactors == 1
|
||||
% Create the noisy pose estimate
|
||||
deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)' ...
|
||||
+ (measurementNoise.poseNoiseVector' .* randn(6,1))); % added noise
|
||||
+ (measurementNoise.poseNoiseVector .* randn(6,1))); % added noise
|
||||
% Add the between factor to the graph
|
||||
graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose));
|
||||
end % end of Between pose factor creation
|
||||
|
|
@ -58,9 +60,9 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false
|
||||
% Generate IMU measurements with noise
|
||||
imuAccel = measurements.imu.accel(i,:)' ...
|
||||
+ (measurementNoise.imu.accelNoiseVector' .* randn(3,1)); % added noise
|
||||
+ (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise
|
||||
imuGyro = measurements.imu.gyro(i,:)' ...
|
||||
+ (measurementNoise.imu.gyroNoiseVector' .* randn(3,1)); % added noise
|
||||
+ (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise
|
||||
% Initialize preintegration
|
||||
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
|
||||
metadata.imu.zeroBias, ...
|
||||
|
|
|
|||
|
|
@ -81,30 +81,37 @@ end % end of else
|
|||
|
||||
%% Create IMU measurements and Values for the trajectory
|
||||
if options.includeIMUFactors == 1
|
||||
currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements)
|
||||
deltaT = 0.1; % amount of time between IMU measurements
|
||||
|
||||
currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements)
|
||||
deltaT = 0.1; % amount of time between IMU measurements
|
||||
|
||||
% Iterate over the deltaMatrix to generate appropriate IMU measurements
|
||||
for i = 1:size(measurements.deltaMatrix, 1)
|
||||
for i = 0:size(measurements.deltaMatrix, 1)
|
||||
% Update Keys
|
||||
currentVelKey = symbol('v', i);
|
||||
currentBiasKey = symbol('b', i);
|
||||
|
||||
measurements.imu.deltaT(i) = deltaT;
|
||||
if i == 0
|
||||
% Add initial values
|
||||
currentVel = [0 0 0];
|
||||
values.insert(currentVelKey, LieVector(currentVel'));
|
||||
values.insert(currentBiasKey, metadata.imu.zeroBias);
|
||||
else
|
||||
measurements.imu.deltaT(i) = deltaT;
|
||||
|
||||
% create accel and gyro measurements based on
|
||||
measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i);
|
||||
|
||||
% create accel and gyro measurements based on
|
||||
measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i);
|
||||
% acc = (deltaPosition - initialVel * dT) * (2/dt^2)
|
||||
measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ...
|
||||
- currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i)));
|
||||
|
||||
% acc = (deltaPosition - initialVel * dT) * (2/dt^2)
|
||||
measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ...
|
||||
- currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i)));
|
||||
% Update velocity
|
||||
currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i);
|
||||
|
||||
% Update velocity
|
||||
currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i);
|
||||
|
||||
% Add Values: velocity and bias
|
||||
values.insert(currentVelKey, LieVector(currentVel'));
|
||||
values.insert(currentBiasKey, metadata.imu.zeroBias);
|
||||
% Add Values: velocity and bias
|
||||
values.insert(currentVelKey, LieVector(currentVel'));
|
||||
values.insert(currentBiasKey, metadata.imu.zeroBias);
|
||||
end
|
||||
end
|
||||
end % end of IMU measurements
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue