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

Working on smart projection factors for consistency tests

release/4.3a0
djensen3 2014-05-15 10:01:53 -04:00
parent b83a996731
commit d0905e65ce
3 changed files with 71 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -27,15 +27,15 @@ if ~exist('externallyConfigured', 'var')
options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements
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.numberOfLandmarks = 1000; % 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 = 50;%209; % length of the ground truth trajectory
options.trajectoryLength = 20;%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 = 2; % number of Monte Carlo runs to perform
numMonteCarloRuns = 1; % number of Monte Carlo runs to perform
% Noise values to be adjusted
sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2
@ -88,10 +88,12 @@ noiseVectorGPS = sigma_gps * ones(3,1);
noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]);
%% Camera metadata
metadata.camera.calibration = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration
metadata.camera.calibration = Cal3_S2(500,500,0,1920/2,1200/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.zlims = [-30, 30]; % z limits on area for landmark creation
metadata.camera.visualRange = 100; % maximum distance from the camera that a landmark can be seen (meters)
metadata.camera.bodyPoseCamera = Pose3.Expmap([-pi/2;0;0;0;0;0]); % pose of camera in body
metadata.camera.CameraSigma = sigma_camera;
cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2, metadata.camera.CameraSigma);
noiseVectorCamera = metadata.camera.CameraSigma .* ones(2,1);
@ -131,7 +133,7 @@ gtMeasurementNoise.gpsNoiseVector = zeros(3,1);
metadata.imu.accelConstantBiasVector = zeros(3,1);
metadata.imu.gyroConstantBiasVector = zeros(3,1);
gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
[gtGraph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
gtMeasurements, ... % ground truth measurements
gtValues, ... % ground truth Values
gtNoiseModels, ... % noise models to use in this graph
@ -145,6 +147,18 @@ gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
figure(1)
hold on;
b = [-1000 2000 -2000 2000 -30 30];
for i = 1:size(metadata.camera.gtLandmarkPoints,2)
p = metadata.camera.gtLandmarkPoints(i).vector;
if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6))
plot3(p(1), p(2), p(3), 'k+');
end
end
pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0));
for i = 1:length(pointsToPlot)
p = pointsToPlot(i).vector;
plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10);
end
plot3DPoints(gtValues);
%plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
plot3DTrajectory(gtValues, '-r');
@ -192,7 +206,7 @@ for k=1:numMonteCarloRuns
end
% Create a new graph using noisy measurements
graph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
[graph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
gtMeasurements, ... % ground truth measurements
gtValues, ... % ground truth Values
monteCarloNoiseModels, ... % noise models to use in this graph

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

@ -1,4 +1,4 @@
function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata)
function [ graph projectionFactorSeenBy] = 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.
@ -36,9 +36,18 @@ for i=0:length(measurements)
%% Create a SmartProjectionFactor for each landmark
if options.includeCameraFactors == 1
for j=1:options.numberOfLandmarks
%% UNCOMMENT WHEN SMART FACTORS ARE ADDED TO MATLAB WRAPPER
%SmartProjectionFactors(j) = SmartProjectionPose3Factor();
SmartProjectionFactors(j) = SmartProjectionPose3Factor();
% Use constructor with default values, but express the pose of the
% camera as a 90 degree rotation about the X axis
% SmartProjectionFactors(j) = SmartProjectionPose3Factor( ...
% 1, ... % rankTol
% -1, ... % linThreshold
% false, ... % manageDegeneracy
% false, ... % enableEPI
% metadata.camera.bodyPoseCamera); % Pose of camera in body frame
end
projectionFactorSeenBy = zeros(options.numberOfLandmarks,1);
end
else
@ -141,18 +150,21 @@ for i=0:length(measurements)
end % end of IMU factor creation
%% Add Camera Factors
%% Build Camera Factors
if options.includeCameraFactors == 1
for(j = 1:length(measurements(i).landmarks))
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)
% Only add the measurement if it is in the image frame (based on calibration)
if(cameraPixelMeasurement(1) > 0 && cameraPixelMeasurement(2) > 0 ...
&& cameraPixelMeasurement(1) < 2*metadata.camera.calibration.px ...
&& cameraPixelMeasurement(2) < 2*metadata.camera.calibration.py)
cameraPixelMeasurement = cameraPixelMeasurement + cameraMeasurmentNoise;
%% UNCOMMENT WHEN SMART FACTORS ARE ADDED TO MATLAB WRAPPER
% SmartProjectionFactors(j).add( ...
% Point2(cameraPizelMeasurement), ...
% currentPoseKey, noiseModels.noiseCamera, ...
% metadata.camera.calibration);
SmartProjectionFactors(j).add( ...
Point2(cameraPixelMeasurement), ...
currentPoseKey, noiseModels.noiseCamera, ...
metadata.camera.calibration);
projectionFactorSeenBy(j) = projectionFactorSeenBy(j)+1;
end
end
end % end of Camera factor creation
@ -170,5 +182,15 @@ for i=0:length(measurements)
end % end of for over trajectory
%% Add Camera Factors to the graph
[find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))]
if options.includeCameraFactors == 1
for j = 1:options.numberOfLandmarks
if projectionFactorSeenBy(j) > 5
graph.add(SmartProjectionFactors(j));
end
end
end
end % end of function

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

@ -29,6 +29,9 @@ if options.useRealData == 1
%% Generate the current pose
currentPoseKey = symbol('x', i);
currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd);
%% FOR TESTING
%currentPose = currentPose.compose(metadata.camera.bodyPoseCamera);
% add to values
values.insert(currentPoseKey, currentPose);
@ -87,14 +90,23 @@ if options.useRealData == 1
end
%% gt Camera measurements
if options.includeCameraFactors == 1 && i > 0
if options.includeCameraFactors == 1 && i > 0
% Create the camera based on the current pose and the pose of the
% camera in the body
cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera);
%camera = SimpleCamera(cameraPose, metadata.camera.calibration);
camera = SimpleCamera(currentPose, metadata.camera.calibration);
% Record measurements if the landmark is within visual range
for j=1:length(metadata.camera.gtLandmarkPoints)
try
z = camera.project(metadata.camera.gtLandmarkPoints(j));
measurements(i).landmarks(j) = z;
catch
distanceToLandmark = camera.pose.range(metadata.camera.gtLandmarkPoints(j));
if distanceToLandmark < metadata.camera.visualRange
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