From d0905e65ce5c73d9ebab438e1c265474d948ecae Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 15 May 2014 10:01:53 -0400 Subject: [PATCH] Working on smart projection factors for consistency tests --- .../+imuSimulator/covarianceAnalysisBetween.m | 28 +++++++++--- .../covarianceAnalysisCreateFactorGraph.m | 44 ++++++++++++++----- .../covarianceAnalysisCreateTrajectory.m | 22 +++++++--- 3 files changed, 71 insertions(+), 23 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index e6149b785..9f6648b8e 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 4ce15ced2..f7fd84989 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index be5991a31..8b55f85a6 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -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