Added option for type 2 IMU factors
djensen3
parent
b5f9862274
commit
e1c13c87d7
|
|
@ -12,12 +12,13 @@ close all
|
|||
%% Configuration
|
||||
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.includeIMUFactors = 1; % if true, IMU factors will be generated for the trajectory based on options.imuFactorType
|
||||
options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1)
|
||||
options.includeCameraFactors = 0; % not fully implemented yet
|
||||
options.trajectoryLength = 209; % length of the ground truth trajectory
|
||||
options.subsampleStep = 20;
|
||||
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)
|
||||
|
||||
numMonteCarloRuns = 2;
|
||||
numMonteCarloRuns = 2; % number of Monte Carlo runs to perform
|
||||
|
||||
%% Camera metadata
|
||||
numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors
|
||||
|
|
|
|||
|
|
@ -63,23 +63,47 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
+ (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise
|
||||
imuGyro = measurements.imu.gyro(i,:)' ...
|
||||
+ (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise
|
||||
% Initialize preintegration
|
||||
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
|
||||
metadata.imu.zeroBias, ...
|
||||
metadata.imu.AccelerometerSigma.^2 * eye(3), ...
|
||||
metadata.imu.GyroscopeSigma.^2 * eye(3), ...
|
||||
metadata.imu.IntegrationSigma.^2 * eye(3));
|
||||
% Preintegrate
|
||||
imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
|
||||
% Add Imu factor
|
||||
graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ...
|
||||
currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis));
|
||||
% Add between factor on biases
|
||||
graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
|
||||
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
|
||||
% Additional prior on zerobias
|
||||
graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ...
|
||||
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
|
||||
|
||||
if options.imuFactorType == 2
|
||||
% Initialize preintegration
|
||||
imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
|
||||
metadata.imu.zeroBias, ...
|
||||
metadata.imu.AccelerometerSigma.^2 * eye(3), ...
|
||||
metadata.imu.GyroscopeSigma.^2 * eye(3), ...
|
||||
metadata.imu.IntegrationSigma.^2 * eye(3), ...
|
||||
metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ...
|
||||
metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ...
|
||||
metadata.imu.BiasAccOmegaInit.^2 * eye(6));
|
||||
% Preintegrate
|
||||
imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
|
||||
% Add Imu factor
|
||||
graph.add(CombinedImuFactor( ...
|
||||
currentPoseKey-1, currentVelKey-1, ...
|
||||
currentPoseKey, currentVelKey, ...
|
||||
currentBiasKey-1, currentBiasKey, ...
|
||||
imuMeasurement, ...
|
||||
metadata.imu.g, metadata.imu.omegaCoriolis, ...
|
||||
noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias)));
|
||||
else % Assumed to be type 1 if type 2 is not selected
|
||||
% Initialize preintegration
|
||||
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
|
||||
metadata.imu.zeroBias, ...
|
||||
metadata.imu.AccelerometerSigma.^2 * eye(3), ...
|
||||
metadata.imu.GyroscopeSigma.^2 * eye(3), ...
|
||||
metadata.imu.IntegrationSigma.^2 * eye(3));
|
||||
% Preintegrate
|
||||
imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
|
||||
% Add Imu factor
|
||||
graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ...
|
||||
currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis));
|
||||
% Add between factor on biases
|
||||
graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
|
||||
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
|
||||
% Additional prior on zerobias
|
||||
graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ...
|
||||
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
|
||||
end
|
||||
|
||||
end % end of IMU factor creation
|
||||
|
||||
%% Add Camera factors - UNDER CONSTRUCTION !!!! -
|
||||
|
|
|
|||
|
|
@ -21,13 +21,17 @@ if options.useRealData == 1
|
|||
|
||||
% Limit the trajectory length
|
||||
options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength+1]);
|
||||
|
||||
fprintf('Scenario Ind: ');
|
||||
for i=1:options.trajectoryLength+1
|
||||
% Update the pose key
|
||||
currentPoseKey = symbol('x', i-1);
|
||||
|
||||
% Generate the current pose
|
||||
scenarioInd = options.subsampleStep * (i-1) + 1
|
||||
scenarioInd = options.subsampleStep * (i-1) + 1;
|
||||
fprintf('%d, ', scenarioInd);
|
||||
if mod(i,20) == 0
|
||||
fprintf('\n');
|
||||
end
|
||||
gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
|
||||
% truth in ENU
|
||||
dX = gtECEF(1) - initialPositionECEF(1);
|
||||
|
|
@ -50,6 +54,7 @@ if options.useRealData == 1
|
|||
measurements.deltaMatrix(i-1,:) = Pose3.Logmap(deltaPose);
|
||||
end
|
||||
end
|
||||
fprintf('\n');
|
||||
else
|
||||
%% Create a random trajectory as ground truth
|
||||
currentPose = Pose3; % initial pose % initial pose
|
||||
|
|
|
|||
Loading…
Reference in New Issue