12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added infrastructure for SmartProjectionPose3Factors

release/4.3a0
djensen3 2014-05-08 15:27:32 -04:00
parent 0f03cd736c
commit 49b3836c49
4 changed files with 89 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
View File

@ -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');

39
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -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

15
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
View File

@ -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

47
matlab/unstable_examples/+imuSimulator/runConsistencyTests.m
View File

@ -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);