restructuring code to utilize functions and reduce size of primary script
djensen3
parent
8367d45e48
commit
b85ebb501d
|
|
@ -10,15 +10,12 @@ clear all
|
||||||
close all
|
close all
|
||||||
|
|
||||||
%% Configuration
|
%% Configuration
|
||||||
useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj
|
options.useRealData = 0; % 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.
|
options.includeIMUFactors = 0; % if true, IMU type 1 Factors will be generated for the trajectory
|
||||||
includeBetweenFactors = 1; % if true, BetweenFactors will be generated between consecutive poses
|
options.includeCameraFactors = 0; % not fully implemented yet
|
||||||
includeIMUFactors = 0; % if true, IMU type 1 Factors will be generated for the random trajectory
|
options.trajectoryLength = 4; % length of the ground truth trajectory
|
||||||
includeCameraFactors = 0; % not fully implemented yet
|
options.subsampleStep = 20;
|
||||||
|
|
||||||
trajectoryLength = 4; % length of the ground truth trajectory
|
|
||||||
subsampleStep = 20;
|
|
||||||
|
|
||||||
numMonteCarloRuns = 2;
|
numMonteCarloRuns = 2;
|
||||||
|
|
||||||
|
|
@ -29,26 +26,29 @@ cameraMeasurementNoiseSigma = 1.0;
|
||||||
cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma);
|
cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma);
|
||||||
|
|
||||||
% Create landmarks
|
% Create landmarks
|
||||||
if includeCameraFactors == 1
|
if options.includeCameraFactors == 1
|
||||||
for i = 1:numberOfLandmarks
|
for i = 1:numberOfLandmarks
|
||||||
gtLandmarkPoints(i) = Point3( ...
|
gtLandmarkPoints(i) = Point3( ...
|
||||||
[rand()*20*(trajectoryLength*1.2) + 15*20; ... % uniformly distributed in the x axis along 120% of the trajectory length, starting after 15 poses
|
... % uniformly distributed in the x axis along 120% of the trajectory length, starting after 15 poses
|
||||||
|
[rand()*20*(options.trajectoryLength*1.2) + 15*20; ...
|
||||||
randn()*20; ... % normally distributed in the y axis with a sigma of 20
|
randn()*20; ... % normally distributed in the y axis with a sigma of 20
|
||||||
randn()*20]); % normally distributed in the z axis with a sigma of 20
|
randn()*20]); % normally distributed in the z axis with a sigma of 20
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
|
||||||
%% Imu metadata
|
%% Imu metadata
|
||||||
epsBias = 1e-10; % was 1e-7
|
metadata.imu.epsBias = 1e-10; % was 1e-7
|
||||||
zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
|
metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
|
||||||
IMU_metadata.AccelerometerSigma = 1e-5;
|
metadata.imu.AccelerometerSigma = 1e-5;
|
||||||
IMU_metadata.GyroscopeSigma = 1e-7;
|
metadata.imu.GyroscopeSigma = 1e-7;
|
||||||
IMU_metadata.IntegrationSigma = 1e-4;
|
metadata.imu.IntegrationSigma = 1e-4;
|
||||||
IMU_metadata.BiasAccelerometerSigma = epsBias;
|
metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias;
|
||||||
IMU_metadata.BiasGyroscopeSigma = epsBias;
|
metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias;
|
||||||
IMU_metadata.BiasAccOmegaInit = epsBias;
|
metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias;
|
||||||
|
metadata.imu.g = [0;0;0];
|
||||||
|
metadata.imu.omegaCoriolis = [0;0;0];
|
||||||
noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1
|
noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1
|
||||||
noiseBias = noiseModel.Isotropic.Sigma(6, epsBias);
|
noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias);
|
||||||
noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-4);
|
noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-4);
|
||||||
|
|
||||||
%% Between metadata
|
%% Between metadata
|
||||||
|
|
@ -61,170 +61,32 @@ noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose);
|
||||||
testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart)
|
testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart)
|
||||||
folderName = 'results/'
|
folderName = 'results/'
|
||||||
|
|
||||||
%% Create ground truth trajectory
|
%% Create ground truth trajectory and measurements
|
||||||
gtValues = Values;
|
[gtValues, gtMeasurements] = imuSimulator.covarianceAnalysisCreateTrajectory(options, metadata);
|
||||||
|
|
||||||
if useRealData == 1
|
%% Create ground truth graph
|
||||||
%% Create a ground truth trajectory from Real data (if available)
|
% Set up noise models
|
||||||
fprintf('\nUsing real data as ground truth\n');
|
gtNoiseModels.noisePose = noisePose;
|
||||||
gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',...
|
gtNoiseModels.noiseVel = noiseVel;
|
||||||
'VEast', 'VNorth', 'VUp');
|
gtNoiseModels.noiseBias = noiseBias;
|
||||||
|
gtNoiseModels.noisePriorPose = noisePose;
|
||||||
Org_lat = gtScenario.Lat(1);
|
gtNoiseModels.noisePriorBias = noisePriorBias;
|
||||||
Org_lon = gtScenario.Lon(1);
|
|
||||||
initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]);
|
|
||||||
|
|
||||||
% Limit the trajectory length
|
|
||||||
trajectoryLength = min([length(gtScenario.Lat) trajectoryLength]);
|
|
||||||
|
|
||||||
for i=1:trajectoryLength
|
|
||||||
currentPoseKey = symbol('x', i-1);
|
|
||||||
scenarioInd = subsampleStep * (i-1) + 1
|
|
||||||
gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
|
|
||||||
% truth in ENU
|
|
||||||
dX = gtECEF(1) - initialPositionECEF(1);
|
|
||||||
dY = gtECEF(2) - initialPositionECEF(2);
|
|
||||||
dZ = gtECEF(3) - initialPositionECEF(3);
|
|
||||||
[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
|
|
||||||
|
|
||||||
gtPosition = [xlt, ylt, zlt]';
|
|
||||||
gtRotation = Rot3; % Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
|
|
||||||
currentPose = Pose3(gtRotation, Point3(gtPosition));
|
|
||||||
|
|
||||||
% Add values
|
|
||||||
gtValues.insert(currentPoseKey, currentPose);
|
|
||||||
end
|
|
||||||
else
|
|
||||||
%% Create a random trajectory as ground truth
|
|
||||||
currentVel = [0; 0; 0]; % initial velocity (used to generate IMU measurements)
|
|
||||||
currentPose = Pose3; % initial pose % initial pose
|
|
||||||
deltaT = 0.1; % amount of time between IMU measurements
|
|
||||||
g = [0; 0; 0]; % gravity
|
|
||||||
omegaCoriolis = [0; 0; 0]; % Coriolis
|
|
||||||
|
|
||||||
unsmooth_DP = 0.5; % controls smoothness on translation norm
|
|
||||||
unsmooth_DR = 0.1; % controls smoothness on rotation norm
|
|
||||||
|
|
||||||
fprintf('\nCreating a random ground truth trajectory\n');
|
|
||||||
currentPoseKey = symbol('x', 0);
|
|
||||||
gtValues.insert(currentPoseKey, currentPose);
|
|
||||||
|
|
||||||
for i=1:trajectoryLength
|
|
||||||
currentPoseKey = symbol('x', i);
|
|
||||||
|
|
||||||
gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [20 0 0]
|
|
||||||
gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0]
|
|
||||||
measurements.gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition];
|
|
||||||
gtMeasurements.deltaPose = Pose3.Expmap(measurements.gtDeltaMatrix(i,:)');
|
|
||||||
|
|
||||||
% "Deduce" ground truth measurements
|
|
||||||
% deltaPose are the gt measurements - save them in some structure
|
|
||||||
currentPose = currentPose.compose(gtMeasurements.deltaPose);
|
|
||||||
gtValues.insert(currentPoseKey, currentPose);
|
|
||||||
end
|
|
||||||
end
|
|
||||||
% we computed gtValues
|
|
||||||
|
|
||||||
gtGraph = NonlinearFactorGraph;
|
% Set measurement noise to 0, because this is ground truth
|
||||||
for i=0:trajectoryLength
|
gtMeasurementNoise.poseNoiseVector = [0 0 0 0 0 0];
|
||||||
|
gtMeasurementNoise.imu.accelNoiseVector = [0 0 0];
|
||||||
currentPoseKey = symbol('x', i);
|
gtMeasurementNoise.imu.gyroNoiseVector = [0 0 0];
|
||||||
currentPose = gtValues.at(currentPoseKey);
|
gtMeasurementNoise.cameraPixelNoiseVector = [0 0];
|
||||||
|
|
||||||
if i==0
|
[gtGraph, gtValues] = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
|
||||||
%% first time step, add priors
|
gtMeasurements, ... % ground truth measurements
|
||||||
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
|
gtValues, ... % ground truth Values
|
||||||
warning('using identity rotation')
|
gtNoiseModels, ... % noise models to use in this graph
|
||||||
gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose));
|
gtMeasurementNoise, ... % noise to apply to measurements
|
||||||
measurements.posePrior = currentPose;
|
options, ... % options for the graph (e.g. which factors to include)
|
||||||
|
metadata); % misc data necessary for factor creation
|
||||||
if includeIMUFactors == 1
|
|
||||||
currentVelKey = symbol('v', 0);
|
|
||||||
currentBiasKey = symbol('b', 0);
|
|
||||||
gtValues.insert(currentVelKey, LieVector(currentVel));
|
|
||||||
gtValues.insert(currentBiasKey, zeroBias);
|
|
||||||
gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel));
|
|
||||||
gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, noisePriorBias));
|
|
||||||
end
|
|
||||||
|
|
||||||
if includeCameraFactors == 1
|
|
||||||
pointNoiseSigma = 0.1;
|
|
||||||
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
|
|
||||||
gtGraph.add(PriorFactorPoint3(symbol('p',1), gtLandmarkPoints(1), pointPriorNoise));
|
|
||||||
end
|
|
||||||
|
|
||||||
else
|
|
||||||
%% other factors
|
|
||||||
if includeBetweenFactors == 1
|
|
||||||
prevPoseKey = symbol('x', i-1);
|
|
||||||
prevPose = gtValues.at(prevPoseKey);
|
|
||||||
deltaPose = prevPose.between(currentPose);
|
|
||||||
measurements.gtDeltaMatrix(i,:) = Pose3.Logmap(deltaPose);
|
|
||||||
|
|
||||||
% Add the factor to the factor graph
|
|
||||||
gtGraph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noisePose));
|
|
||||||
end
|
|
||||||
|
|
||||||
%% Add IMU factors
|
|
||||||
if includeIMUFactors == 1
|
|
||||||
currentVelKey = symbol('v', i); % not used if includeIMUFactors is false
|
|
||||||
currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false
|
|
||||||
|
|
||||||
% create accel and gyro measurements based on
|
|
||||||
gtMeasurements.imu.gyro = measurements.gtDeltaMatrix(i, 1:3)'./deltaT;
|
|
||||||
% acc = (deltaPosition - initialVel * dT) * (2/dt^2)
|
|
||||||
gtMeasurements.imu.accel = (measurements.gtDeltaMatrix(i, 4:6)' - currentVel.*deltaT).*(2/(deltaT*deltaT));
|
|
||||||
% Initialize preintegration
|
|
||||||
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
|
|
||||||
zeroBias, ...
|
|
||||||
IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
|
|
||||||
IMU_metadata.GyroscopeSigma.^2 * eye(3), ...
|
|
||||||
IMU_metadata.IntegrationSigma.^2 * eye(3));
|
|
||||||
% Preintegrate
|
|
||||||
imuMeasurement.integrateMeasurement(gtMeasurements.imu.accel, gtMeasurements.imu.gyro, deltaT);
|
|
||||||
% Add Imu factor
|
|
||||||
gtGraph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ...
|
|
||||||
currentBiasKey-1, imuMeasurement, g, omegaCoriolis));
|
|
||||||
% Add between on biases
|
|
||||||
gtGraph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, zeroBias, ...
|
|
||||||
noiseModel.Isotropic.Sigma(6, epsBias)));
|
|
||||||
% Additional prior on zerobias
|
|
||||||
gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, ...
|
|
||||||
noiseModel.Isotropic.Sigma(6, epsBias)));
|
|
||||||
|
|
||||||
% update current velocity
|
|
||||||
currentVel = measurements.gtDeltaMatrix(i,4:6)'./deltaT;
|
|
||||||
gtValues.insert(currentVelKey, LieVector(currentVel));
|
|
||||||
|
|
||||||
gtValues.insert(currentBiasKey, zeroBias);
|
|
||||||
end % end of IMU factor creation
|
|
||||||
|
|
||||||
%% Add Camera factors
|
|
||||||
if includeCameraFactors == 1
|
|
||||||
% Create camera with the current pose and calibration K (specified above)
|
|
||||||
gtCamera = SimpleCamera(currentPose, K);
|
|
||||||
% Project landmarks into the camera
|
|
||||||
numSkipped = 0;
|
|
||||||
for j = 1:length(gtLandmarkPoints)
|
|
||||||
landmarkKey = symbol('p', j);
|
|
||||||
try
|
|
||||||
Z = gtCamera.project(gtLandmarkPoints(j));
|
|
||||||
%% TO-DO probably want to do some type of filtering on the measurement values, because
|
|
||||||
% they might not all be valid
|
|
||||||
gtGraph.add(GenericProjectionFactorCal3_S2(Z, cameraMeasurementNoise, currentPoseKey, landmarkKey, K));
|
|
||||||
catch
|
|
||||||
% Most likely the point is not within the camera's view, which
|
|
||||||
% is fine
|
|
||||||
numSkipped = numSkipped + 1;
|
|
||||||
end
|
|
||||||
end
|
|
||||||
%fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped);
|
|
||||||
end % end of Camera factor creation
|
|
||||||
|
|
||||||
end % end of else
|
|
||||||
|
|
||||||
end % end of for over trajectory
|
|
||||||
|
|
||||||
|
%% Display, printing, and plotting of ground truth
|
||||||
%gtGraph.print(sprintf('\nGround Truth Factor graph:\n'));
|
%gtGraph.print(sprintf('\nGround Truth Factor graph:\n'));
|
||||||
%gtValues.print(sprintf('\nGround Truth Values:\n '));
|
%gtValues.print(sprintf('\nGround Truth Values:\n '));
|
||||||
|
|
||||||
|
|
@ -239,6 +101,7 @@ axis equal
|
||||||
|
|
||||||
disp('Plotted ground truth')
|
disp('Plotted ground truth')
|
||||||
|
|
||||||
|
%% Monte Carlo Runs
|
||||||
for k=1:numMonteCarloRuns
|
for k=1:numMonteCarloRuns
|
||||||
fprintf('Monte Carlo Run %d.\n', k');
|
fprintf('Monte Carlo Run %d.\n', k');
|
||||||
% create a new graph
|
% create a new graph
|
||||||
|
|
@ -246,17 +109,17 @@ for k=1:numMonteCarloRuns
|
||||||
|
|
||||||
% noisy prior
|
% noisy prior
|
||||||
currentPoseKey = symbol('x', 0);
|
currentPoseKey = symbol('x', 0);
|
||||||
measurements.posePrior = currentPose;
|
currentPose = gtValues.at(currentPoseKey);
|
||||||
|
gtMeasurements.posePrior = currentPose;
|
||||||
noisyDelta = noiseVectorPose .* randn(6,1);
|
noisyDelta = noiseVectorPose .* randn(6,1);
|
||||||
noisyInitialPose = Pose3.Expmap(noisyDelta);
|
noisyInitialPose = Pose3.Expmap(noisyDelta);
|
||||||
graph.add(PriorFactorPose3(currentPoseKey, noisyInitialPose, noisePose));
|
graph.add(PriorFactorPose3(currentPoseKey, noisyInitialPose, noisePose));
|
||||||
|
|
||||||
for i=1:size(measurements.gtDeltaMatrix,1)
|
for i=1:size(gtMeasurements.deltaMatrix,1)
|
||||||
i
|
|
||||||
currentPoseKey = symbol('x', i);
|
currentPoseKey = symbol('x', i);
|
||||||
|
|
||||||
% for each measurement: add noise and add to graph
|
% for each measurement: add noise and add to graph
|
||||||
noisyDelta = measurements.gtDeltaMatrix(i,:)' + (noiseVectorPose .* randn(6,1));
|
noisyDelta = gtMeasurements.deltaMatrix(i,:)' + (noiseVectorPose .* randn(6,1));
|
||||||
noisyDeltaPose = Pose3.Expmap(noisyDelta);
|
noisyDeltaPose = Pose3.Expmap(noisyDelta);
|
||||||
|
|
||||||
% Add the factors to the factor graph
|
% Add the factors to the factor graph
|
||||||
|
|
@ -275,7 +138,7 @@ for k=1:numMonteCarloRuns
|
||||||
marginals = Marginals(graph, estimate);
|
marginals = Marginals(graph, estimate);
|
||||||
|
|
||||||
% for each pose in the trajectory
|
% for each pose in the trajectory
|
||||||
for i=1:size(measurements.gtDeltaMatrix,1)+1
|
for i=1:size(gtMeasurements.deltaMatrix,1)+1
|
||||||
% compute estimation errors
|
% compute estimation errors
|
||||||
currentPoseKey = symbol('x', i-1);
|
currentPoseKey = symbol('x', i-1);
|
||||||
gtPosition = gtValues.at(currentPoseKey).translation.vector;
|
gtPosition = gtValues.at(currentPoseKey).translation.vector;
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,110 @@
|
||||||
|
function [ graph, values ] = 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.
|
||||||
|
% 'metadata' is a storage variable for miscellaneous factor-specific values
|
||||||
|
% Authors: Luca Carlone, David Jensen
|
||||||
|
% Date: 2014/04/16
|
||||||
|
|
||||||
|
import gtsam.*;
|
||||||
|
|
||||||
|
graph = NonlinearFactorGraph;
|
||||||
|
|
||||||
|
% Iterate through the trajectory
|
||||||
|
for i=0:size(measurements.deltaMatrix, 1);
|
||||||
|
% Get the current pose
|
||||||
|
currentPoseKey = symbol('x', i);
|
||||||
|
currentPose = values.at(currentPoseKey);
|
||||||
|
|
||||||
|
if i==0
|
||||||
|
%% 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;
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
if options.includeCameraFactors == 1
|
||||||
|
pointNoiseSigma = 0.1;
|
||||||
|
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
|
||||||
|
graph.add(PriorFactorPoint3(symbol('p',1), gtLandmarkPoints(1), pointPriorNoise));
|
||||||
|
end
|
||||||
|
|
||||||
|
else
|
||||||
|
prevPoseKey = symbol('x', i-1);
|
||||||
|
|
||||||
|
%% Add Between factors
|
||||||
|
if options.includeBetweenFactors == 1
|
||||||
|
% Create the noisy pose estimate
|
||||||
|
deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)' ...
|
||||||
|
+ (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
|
||||||
|
|
||||||
|
%% Add IMU factors
|
||||||
|
if options.includeIMUFactors == 1
|
||||||
|
% Update keys
|
||||||
|
currentVelKey = symbol('v', i); % not used if includeIMUFactors is false
|
||||||
|
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
|
||||||
|
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)));
|
||||||
|
end % end of IMU factor creation
|
||||||
|
|
||||||
|
%% Add Camera factors - UNDER CONSTRUCTION !!!! -
|
||||||
|
if options.includeCameraFactors == 1
|
||||||
|
% Create camera with the current pose and calibration K (specified above)
|
||||||
|
gtCamera = SimpleCamera(currentPose, K);
|
||||||
|
% Project landmarks into the camera
|
||||||
|
numSkipped = 0;
|
||||||
|
for j = 1:length(gtLandmarkPoints)
|
||||||
|
landmarkKey = symbol('p', j);
|
||||||
|
try
|
||||||
|
Z = gtCamera.project(gtLandmarkPoints(j));
|
||||||
|
%% TO-DO probably want to do some type of filtering on the measurement values, because
|
||||||
|
% they might not all be valid
|
||||||
|
graph.add(GenericProjectionFactorCal3_S2(Z, cameraMeasurementNoise, currentPoseKey, landmarkKey, K));
|
||||||
|
catch
|
||||||
|
% Most likely the point is not within the camera's view, which
|
||||||
|
% is fine
|
||||||
|
numSkipped = numSkipped + 1;
|
||||||
|
end
|
||||||
|
end
|
||||||
|
%fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped);
|
||||||
|
end % end of Camera factor creation
|
||||||
|
|
||||||
|
end % end of else
|
||||||
|
|
||||||
|
end % end of for over trajectory
|
||||||
|
|
||||||
|
end
|
||||||
|
|
||||||
|
|
@ -0,0 +1,112 @@
|
||||||
|
function [ values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata )
|
||||||
|
% Create a trajectory for running covariance analysis scripts.
|
||||||
|
% 'options' contains fields for including various factor types and setting trajectory length
|
||||||
|
% 'metadata' is a storage variable for miscellaneous factor-specific values
|
||||||
|
% Authors: Luca Carlone, David Jensen
|
||||||
|
% Date: 2014/04/16
|
||||||
|
|
||||||
|
import gtsam.*;
|
||||||
|
|
||||||
|
values = Values;
|
||||||
|
|
||||||
|
if options.useRealData == 1
|
||||||
|
%% Create a ground truth trajectory from Real data (if available)
|
||||||
|
fprintf('\nUsing real data as ground truth\n');
|
||||||
|
gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',...
|
||||||
|
'VEast', 'VNorth', 'VUp');
|
||||||
|
|
||||||
|
Org_lat = gtScenario.Lat(1);
|
||||||
|
Org_lon = gtScenario.Lon(1);
|
||||||
|
initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]);
|
||||||
|
|
||||||
|
% Limit the trajectory length
|
||||||
|
options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength+1]);
|
||||||
|
|
||||||
|
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
|
||||||
|
gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
|
||||||
|
% truth in ENU
|
||||||
|
dX = gtECEF(1) - initialPositionECEF(1);
|
||||||
|
dY = gtECEF(2) - initialPositionECEF(2);
|
||||||
|
dZ = gtECEF(3) - initialPositionECEF(3);
|
||||||
|
[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
|
||||||
|
|
||||||
|
gtPosition = [xlt, ylt, zlt]';
|
||||||
|
gtRotation = Rot3; % Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
|
||||||
|
currentPose = Pose3(gtRotation, Point3(gtPosition));
|
||||||
|
|
||||||
|
% Add values
|
||||||
|
values.insert(currentPoseKey, currentPose);
|
||||||
|
|
||||||
|
% Generate the measurement. The first pose is considered the prior, so
|
||||||
|
% it has no measurement
|
||||||
|
if i > 1
|
||||||
|
prevPose = values.at(currentPoseKey - 1);
|
||||||
|
deltaPose = prevPose.between(currentPose);
|
||||||
|
measurements.deltaMatrix(i-1,:) = Pose3.Logmap(deltaPose);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
else
|
||||||
|
%% Create a random trajectory as ground truth
|
||||||
|
currentPose = Pose3; % initial pose % initial pose
|
||||||
|
|
||||||
|
unsmooth_DP = 0.5; % controls smoothness on translation norm
|
||||||
|
unsmooth_DR = 0.1; % controls smoothness on rotation norm
|
||||||
|
|
||||||
|
fprintf('\nCreating a random ground truth trajectory\n');
|
||||||
|
currentPoseKey = symbol('x', 0);
|
||||||
|
values.insert(currentPoseKey, currentPose);
|
||||||
|
|
||||||
|
for i=1:options.trajectoryLength
|
||||||
|
% Update the pose key
|
||||||
|
currentPoseKey = symbol('x', i);
|
||||||
|
|
||||||
|
% Generate the new measurements
|
||||||
|
gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [20 0 0]
|
||||||
|
gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0]
|
||||||
|
measurements.deltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition];
|
||||||
|
|
||||||
|
% Create the next pose
|
||||||
|
deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)');
|
||||||
|
currentPose = currentPose.compose(deltaPose);
|
||||||
|
|
||||||
|
% Add the current pose as a value
|
||||||
|
values.insert(currentPoseKey, currentPose);
|
||||||
|
end % end of random trajectory creation
|
||||||
|
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
|
||||||
|
|
||||||
|
% Iterate over the deltaMatrix to generate appropriate IMU measurements
|
||||||
|
for i = 1:size(measurements.deltaMatrix, 1)
|
||||||
|
% Update Keys
|
||||||
|
currentVelKey = symbol('v', i);
|
||||||
|
currentBiasKey = symbol('b', i);
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
% 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);
|
||||||
|
|
||||||
|
% Add Values: velocity and bias
|
||||||
|
values.insert(currentVelKey, LieVector(currentVel'));
|
||||||
|
values.insert(currentBiasKey, metadata.imu.zeroBias);
|
||||||
|
end
|
||||||
|
end % end of IMU measurements
|
||||||
|
|
||||||
|
end
|
||||||
|
|
||||||
Loading…
Reference in New Issue