fixed bug with missing priors. Added IMU noise measurements to Monte Carlo runs
							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