From b5f98622749b7ed196026e0261786b2d20cb16cd Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 16 Apr 2014 16:20:10 -0400 Subject: [PATCH] fixed bug with missing priors. Added IMU noise measurements to Monte Carlo runs --- .../+imuSimulator/covarianceAnalysisBetween.m | 76 +++++++++---------- .../covarianceAnalysisCreateFactorGraph.m | 18 +++-- .../covarianceAnalysisCreateTrajectory.m | 39 ++++++---- 3 files changed, 70 insertions(+), 63 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 4be7678c1..335ce9f6a 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 5ca14021e..f27dbd02f 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -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, ... diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 6a6c3b594..d19c5cf79 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -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