diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 1c9311a93..e6149b785 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -3,7 +3,8 @@ import gtsam.*; % Test GTSAM covariances on a factor graph with: % Between Factors % IMU factors (type 1 and type 2) -% Projection factors +% GPS prior factors on poses +% SmartProjectionPoseFactors % Authors: Luca Carlone, David Jensen % Date: 2014/4/6 @@ -19,31 +20,32 @@ if ~exist('externallyConfigured', 'var') %% Configuration % General options options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj - options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses + options.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses - options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) - options.imuFactorType = 2; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) + options.includeIMUFactors = 0; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) + options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements - options.includeCameraFactors = 0; % not fully implemented yet - numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors + options.includeCameraFactors = 1; % if true, SmartProjectionPose3Factors will be used with randomly generated landmarks + options.numberOfLandmarks = 100; % Total number of visual landmarks (randomly generated in a box around the trajectory) options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses options.gpsStartPose = 100; % Pose number to start including GPS factors at - options.trajectoryLength = 209;%209; % length of the ground truth trajectory + options.trajectoryLength = 50;%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 = 5; % number of Monte Carlo runs to perform + numMonteCarloRuns = 2; % number of Monte Carlo runs to perform % Noise values to be adjusted sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2 sigma_cart = 1e-1; % std. deviation for translational noise, typical 1e-1 - sigma_accel = 1e-1; % std. deviation for accelerometer noise, typical 1e-3 - sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5 - sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3 - sigma_gyroBias = 1e-6; % std. deviation for added gyroscope constant bias, typical 1e-5 + sigma_accel = 1e-3; % std. deviation for accelerometer noise, typical 1e-3 + sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5 + sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3 + sigma_gyroBias = 1e-6; % std. deviation for added gyroscope constant bias, typical 1e-5 sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4 + sigma_camera = 1; % std. deviation for noise in camera measurements (pixels) % Set log files testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro) @@ -86,18 +88,21 @@ noiseVectorGPS = sigma_gps * ones(3,1); noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]); %% Camera metadata -K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration -cameraMeasurementNoiseSigma = 1.0; -cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma); +metadata.camera.calibration = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration +metadata.camera.xlims = [-100, 650]; % x limits on area for landmark creation +metadata.camera.ylims = [-100, 700]; % y limits on area for landmark creation +metadata.camera.zlims = [-10, 30]; % z limits on area for landmark creation +metadata.camera.CameraSigma = sigma_camera; +cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2, metadata.camera.CameraSigma); +noiseVectorCamera = metadata.camera.CameraSigma .* ones(2,1); -% Create landmarks +% Create landmarks and smart factors if options.includeCameraFactors == 1 - for i = 1:numberOfLandmarks - gtLandmarkPoints(i) = Point3( ... - ... % 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 z axis with a sigma of 20 + for i = 1:options.numberOfLandmarks + metadata.camera.gtLandmarkPoints(i) = Point3( ... + [rand() * (metadata.camera.xlims(2)-metadata.camera.xlims(1)) + metadata.camera.xlims(1); ... + rand() * (metadata.camera.ylims(2)-metadata.camera.ylims(1)) + metadata.camera.ylims(1); + rand() * (metadata.camera.zlims(2)-metadata.camera.zlims(1)) + metadata.camera.zlims(1)]); end end @@ -113,12 +118,13 @@ gtNoiseModels.noiseBiasBetween = noiseBiasBetween; gtNoiseModels.noisePriorPose = noisePose; gtNoiseModels.noisePriorBias = noisePriorBias; gtNoiseModels.noiseGPS = noiseGPS; +gtNoiseModels.noiseCamera = cameraMeasurementNoise; % Set measurement noise to 0, because this is ground truth gtMeasurementNoise.poseNoiseVector = zeros(6,1); gtMeasurementNoise.imu.accelNoiseVector = zeros(3,1); gtMeasurementNoise.imu.gyroNoiseVector = zeros(3,1); -gtMeasurementNoise.cameraPixelNoiseVector = zeros(2,1); +gtMeasurementNoise.cameraNoiseVector = zeros(2,1); gtMeasurementNoise.gpsNoiseVector = zeros(3,1); % Set IMU biases to zero @@ -140,8 +146,8 @@ gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... figure(1) hold on; plot3DPoints(gtValues); -plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); -%plot3DTrajectory(gtValues, '-r'); +%plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); +plot3DTrajectory(gtValues, '-r'); axis equal % optimize @@ -162,13 +168,14 @@ monteCarloNoiseModels.noiseBiasBetween = noiseBiasBetween; monteCarloNoiseModels.noisePriorPose = noisePose; monteCarloNoiseModels.noisePriorBias = noisePriorBias; monteCarloNoiseModels.noiseGPS = noiseGPS; +monteCarloNoiseModels.noiseCamera = cameraMeasurementNoise; % Set measurement noise for monte carlo runs -monteCarloMeasurementNoise.poseNoiseVector = noiseVectorPose; +monteCarloMeasurementNoise.poseNoiseVector = zeros(6,1); %noiseVectorPose; monteCarloMeasurementNoise.imu.accelNoiseVector = noiseVectorAccel; monteCarloMeasurementNoise.imu.gyroNoiseVector = noiseVectorGyro; -monteCarloMeasurementNoise.cameraPixelNoiseVector = [0; 0]; monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS; +monteCarloMeasurementNoise.cameraNoiseVector = noiseVectorCamera; for k=1:numMonteCarloRuns fprintf('Monte Carlo Run %d...\n', k'); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index d87fa490b..4ce15ced2 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -33,11 +33,12 @@ for i=0:length(measurements) graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias)); end - % Camera priors + %% Create a SmartProjectionFactor for each landmark if options.includeCameraFactors == 1 - pointNoiseSigma = 0.1; - pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); - graph.add(PriorFactorPoint3(symbol('p',1), gtLandmarkPoints(1), pointPriorNoise)); + for j=1:options.numberOfLandmarks + %% UNCOMMENT WHEN SMART FACTORS ARE ADDED TO MATLAB WRAPPER + %SmartProjectionFactors(j) = SmartProjectionPose3Factor(); + end end else @@ -140,26 +141,20 @@ for i=0:length(measurements) 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; + %% Add Camera Factors + if options.includeCameraFactors == 1 + for(j = 1:length(measurements(i).landmarks)) + cameraMeasurmentNoise = measurementNoise.cameraNoiseVector .* randn(2,1); + cameraPixelMeasurement = measurements(i).landmarks(j).vector; + if(cameraPixelMeasurement(1) ~= 0 && cameraPixelMeasurement(2) ~= 0) + cameraPixelMeasurement = cameraPixelMeasurement + cameraMeasurmentNoise; + %% UNCOMMENT WHEN SMART FACTORS ARE ADDED TO MATLAB WRAPPER + % SmartProjectionFactors(j).add( ... + % Point2(cameraPizelMeasurement), ... + % currentPoseKey, noiseModels.noiseCamera, ... + % metadata.camera.calibration); end end - %fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped); end % end of Camera factor creation %% Add GPS factors diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index b66df3fc9..be5991a31 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -18,7 +18,7 @@ if options.useRealData == 1 'VEast', 'VNorth', 'VUp'); % Limit the trajectory length - options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength]); + options.trajectoryLength = min([length(gtScenario.Lat)/options.subsampleStep options.trajectoryLength]); fprintf('Scenario Ind: '); for i=0:options.trajectoryLength @@ -86,6 +86,19 @@ if options.useRealData == 1 measurements(i).gpsPositionVector = gpsPositionVector; end + %% gt Camera measurements + if options.includeCameraFactors == 1 && i > 0 + camera = SimpleCamera(currentPose, metadata.camera.calibration); + for j=1:length(metadata.camera.gtLandmarkPoints) + try + z = camera.project(metadata.camera.gtLandmarkPoints(j)); + measurements(i).landmarks(j) = z; + catch + % point is probably out of the camera's view + end + end + end + end fprintf('\n'); else diff --git a/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m index 818a41388..697b49a6a 100644 --- a/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m +++ b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m @@ -11,28 +11,28 @@ end testOptions = [ ... % 1 2 3 4 5 6 7 8 9 10 11 12 % RealData? Between? IMU? IMUType Bias? Camera? #LndMrk GPS? StrtPose TrajLength Subsample #MCRuns - %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 1 - %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 2 - % 1 0 1 2 0 0 10 0 100 209 20 100 ;... % 3 - %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 4 - %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 5 - %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 6 - % 1 0 1 2 0 0 10 0 100 209 20 100 ;... % 7 - 1 0 1 2 1 0 10 0 100 209 20 100 ;... % 8 - 1 0 1 2 1 0 10 0 100 209 20 100 ]; % 9 + %1 0 1 2 0 0 100 0 100 209 20 100 ;... % 1 + %1 0 1 2 0 0 100 0 100 209 20 100 ;... % 2 + % 1 0 1 2 0 0 100 0 100 209 20 100 ;... % 3 + 1 0 1 2 0 1 100 0 100 209 20 20 ;... % 4 + 1 0 1 2 0 1 100 0 100 209 20 20 ;... % 5 + 1 0 1 2 0 0 100 0 100 209 20 20 ];%... % 6 + % 1 0 1 2 0 0 100 0 100 209 20 100 ;... % 7 + %1 0 1 2 0 0 100 0 100 209 20 1 ;... % 8 + %1 0 1 2 0 0 100 0 100 209 20 1 ]; % 9 noises = [ ... - % 1 2 3 4 5 6 7 - % sigma_ang sigma_cart sigma_accel sigma_gyro sigma_accelBias sigma_gyroBias sigma_gps - %1e-2 1e-1 1e-3 1e-5 0 0 1e-4;... % 1 - %1e-2 1e-1 1e-2 1e-5 0 0 1e-4;... % 2 - % 1e-2 1e-1 1e-1 1e-5 0 0 1e-4;... % 3 - %1e-2 1e-1 1e-3 1e-4 0 0 1e-4;... % 4 - %1e-2 1e-1 1e-3 1e-3 0 0 1e-4;... % 5 - %1e-2 1e-1 1e-3 1e-2 0 0 1e-4;... % 6 - % 1e-2 1e-1 1e-3 1e-1 0 0 1e-4;... % 7 - 1e-2 1e-1 1e-3 1e-5 1e-3 1e-5 1e-4;... % 8 - 1e-2 1e-1 1e-3 1e-5 1e-4 1e-6 1e-4]; % 9 + % 1 2 3 4 5 6 7 8 + % sigma_ang sigma_cart sigma_accel sigma_gyro sigma_accelBias sigma_gyroBias sigma_gps sigma_camera + %1e-2 1e-1 1e-3 1e-5 0 0 1e-4 1;... % 1 + %1e-2 1e-1 1e-2 1e-5 0 0 1e-4 1;... % 2 + % 1e-2 1e-1 1e-1 1e-5 0 0 1e-4 1;... % 3 + 1e-2 1e-1 1e-3 1e-4 0 0 1e-4 1;... % 4 + 1e-2 1e-1 1e-3 1e-3 0 0 1e-4 1;... % 5 + 1e-2 1e-1 1e-3 1e-2 0 0 1e-4 1];%... % 6 + % 1e-2 1e-1 1e-3 1e-1 0 0 1e-4 1;... % 7 + %1e-2 1e-1 1e-3 1e-2 1e-3 1e-5 1e-4 1;... % 8 + %1e-2 1e-1 1e-3 1e-2 1e-4 1e-6 1e-4 1]; % 9 if(size(testOptions,1) ~= size(noises,1)) error('testOptions and noises do not have same number of rows'); @@ -42,7 +42,7 @@ end externallyConfigured = 1; % Set the flag to save the results -saveResults = 1; +saveResults = 0; errorRuns = []; @@ -59,7 +59,7 @@ for i = 1:size(testOptions,1) options.imuFactorType = testOptions(i,4); options.imuNonzeroBias = testOptions(i,5); options.includeCameraFactors = testOptions(i,6); - numberOfLandmarks = testOptions(i,7); + options.numberOfLandmarks = testOptions(i,7); options.includeGPSFactors = testOptions(i,8); options.gpsStartPose = testOptions(i,9); options.trajectoryLength = testOptions(i,10); @@ -73,6 +73,7 @@ for i = 1:size(testOptions,1) sigma_accelBias = noises(i,5); sigma_gyroBias = noises(i,6); sigma_gps = noises(i,7); + sigma_camera = noises(i,8); % Create folder name f_between = ''; @@ -95,7 +96,7 @@ for i = 1:size(testOptions,1) f_between = sprintf('gps_%d_', gpsStartPose); end if (options.includeCameraFactors == 1) - f_camera = sprintf('camera_%d_', numberOfLandmarks); + f_camera = sprintf('camera_%d_', options.numberOfLandmarks); end f_runs = sprintf('mc%d', numMonteCarloRuns);