From ce13807d10611e0861caf50092363203acc31b56 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 1 May 2014 12:56:24 -0400 Subject: [PATCH] added script to run and save tests in a simplified way --- gtsam_unstable/gtsam_unstable.h | 27 ++- .../+imuSimulator/covarianceAnalysisBetween.m | 115 ++++++------ .../+imuSimulator/runConsistencyTests.m | 172 ++++++++++++++++++ 3 files changed, 257 insertions(+), 57 deletions(-) create mode 100644 matlab/unstable_examples/+imuSimulator/runConsistencyTests.m diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 0e2ef44b1..c0bf66f37 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -20,6 +20,8 @@ virtual class gtsam::NonlinearFactor; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; +virtual class gtsam::SmartFactorBase; +virtual class gtsam::SmartProjectionFactor; class gtsam::GaussianFactorGraph; class gtsam::NonlinearFactorGraph; class gtsam::Ordering; @@ -376,6 +378,26 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { }; +#include +template +virtual class SmartFactorBase : gtsam::NonlinearFactor { + SmartFactorBase(const POSE& body_P_sensor); + + void add(const Point2& measured_i, const Key& poseKey_i, + const SharedNoiseModel& noise_i); + + void serialize() const; +}; + +#include +template +virtual class SmartProjectionFactor : gtsam::SmartFactorBase { + SmartProjectionFactor(double rankTol, double linThreshold, + bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + + void serialize() const; +}; + #include template virtual class SmartProjectionPoseFactor : gtsam::SmartProjectionFactor { @@ -386,12 +408,9 @@ virtual class SmartProjectionPoseFactor : gtsam::SmartProjectionFactor { //void add(gtsam::Point2 measured_i, size_t poseKey_i, gtsam::SharedNoiseModel noise_i, CALIBRATION* K_i); // enabling serialization functionality - // void serialize() const; + void serialize() const; }; -//typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; - - #include template virtual class RangeFactor : gtsam::NonlinearFactor { diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 4f60ae2ef..1c9311a93 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -7,53 +7,56 @@ import gtsam.*; % Authors: Luca Carlone, David Jensen % Date: 2014/4/6 -clc -clear all -close all -saveResults = 0; - -%% Configuration -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.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.imuNonzeroBias = 1; % 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.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.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) - -numMonteCarloRuns = 20; % number of Monte Carlo runs to perform - -%% Camera metadata -K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration -cameraMeasurementNoiseSigma = 1.0; -cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma); - -% Create landmarks -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 - end +% Check for an extneral configuration, used when running multiple tests +if ~exist('externallyConfigured', 'var') + clc + clear all + close all + + saveResults = 0; + + %% 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.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.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.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.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 + + % 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_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4 + + % 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) + folderName = 'results/' +else + fprintf('Tests have been externally configured.\n'); end -%% Imu metadata -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 +%% Between metadata +noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)]; +noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); +%% Imu metadata metadata.imu.epsBias = 1e-10; % was 1e-7 metadata.imu.g = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0]; @@ -78,20 +81,26 @@ noisePriorBias = noiseModel.Diagonal.Sigmas(metadata.imu.BiasAccOmegaInit); noiseVectorAccel = metadata.imu.AccelerometerSigma * ones(3,1); noiseVectorGyro = metadata.imu.GyroscopeSigma * ones(3,1); -%% Between metadata -sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2 -sigma_cart = 1e-1; % std. deviation for translational noise, typical 1e-1 -noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)]; -noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); - %% GPS metadata -sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4 noiseVectorGPS = sigma_gps * ones(3,1); noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]); -%% 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) -folderName = 'results/' +%% Camera metadata +K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration +cameraMeasurementNoiseSigma = 1.0; +cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma); + +% Create landmarks +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 + end +end + %% Create ground truth trajectory and measurements [gtValues, gtMeasurements] = imuSimulator.covarianceAnalysisCreateTrajectory(options, metadata); diff --git a/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m new file mode 100644 index 000000000..818a41388 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m @@ -0,0 +1,172 @@ +% use this script to easily run and save results for multiple consistency +% tests without having to pay attention to the computer every 5 minutes + +import gtsam.*; + +resultsDir = 'results/' +if (~exist(resultsDir, 'dir')) + mkdir(resultsDir); +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 + +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 + +if(size(testOptions,1) ~= size(noises,1)) + error('testOptions and noises do not have same number of rows'); +end + +% Set flag so the script knows there is an external configuration +externallyConfigured = 1; + +% Set the flag to save the results +saveResults = 1; + +errorRuns = []; + +% Go through tests +for i = 1:size(testOptions,1) + % Clean up from last test + close all; + %clc; + + % Set up variables for test + options.useRealData = testOptions(i,1); + options.includeBetweenFactors = testOptions(i,2); + options.includeIMUFactors = testOptions(i,3); + options.imuFactorType = testOptions(i,4); + options.imuNonzeroBias = testOptions(i,5); + options.includeCameraFactors = testOptions(i,6); + numberOfLandmarks = testOptions(i,7); + options.includeGPSFactors = testOptions(i,8); + options.gpsStartPose = testOptions(i,9); + options.trajectoryLength = testOptions(i,10); + options.subsampleStep = testOptions(i,11); + numMonteCarloRuns = testOptions(i,12); + + sigma_ang = noises(i,1); + sigma_cart = noises(i,2); + sigma_accel = noises(i,3); + sigma_gyro = noises(i,4); + sigma_accelBias = noises(i,5); + sigma_gyroBias = noises(i,6); + sigma_gps = noises(i,7); + + % Create folder name + f_between = ''; + f_imu = ''; + f_bias = ''; + f_gps = ''; + f_camera = ''; + f_runs = ''; + + if (options.includeBetweenFactors == 1); + f_between = 'between_'; + end + if (options.includeIMUFactors == 1) + f_imu = sprintf('imu%d_', options.imuFactorType); + if (options.imuNonzeroBias == 1); + f_bias = sprintf('bias_a%1.2g_g%1.2g_', sigma_accelBias, sigma_gyroBias); + end + end + if (options.includeGPSFactors == 1); + f_between = sprintf('gps_%d_', gpsStartPose); + end + if (options.includeCameraFactors == 1) + f_camera = sprintf('camera_%d_', numberOfLandmarks); + end + f_runs = sprintf('mc%d', numMonteCarloRuns); + + folderName = [resultsDir f_between f_imu f_bias f_gps f_camera f_runs '/']; + + % make folder if it doesnt exist + if (~exist(folderName, 'dir')) + mkdir(folderName); + end + + testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro); + + % Run the test + fprintf('Test %d\n\tResults will be saved to:\n\t%s\n\trunning...\n', i, folderName); + fprintf('Test Name: %s\n', testName); + + try + imuSimulator.covarianceAnalysisBetween; + catch + errorRuns = [errorRuns i]; + fprintf('\n*****\n Something went wrong, most likely indeterminant linear system error.\n'); + disp('Test Options:\n'); + disp(testOptions(i,:)); + disp('Noises'); + disp(noises(i,:)); + fprintf('\n*****\n\n'); + end +end + +% Print error summary +fprintf('*************************\n'); +fprintf('%d Runs failed due to errors (data not collected for failed runs)\n', length(errorRuns)); +for i = 1:length(errorRuns) + k = errorRuns(i); + fprintf('\nTest %d:\n', k); + fprintf(' options.useRealData = %d\n', testOptions(k,1)); + fprintf(' options.includeBetweenFactors = %d\n', testOptions(k,2)); + fprintf(' options.includeIMUFactors = %d\n', testOptions(k,3)); + fprintf(' options.imuFactorType = %d\n', testOptions(k,4)); + fprintf(' options.imuNonzeroBias = %d\n', testOptions(k,5)); + fprintf(' options.includeCameraFactors = %d\n', testOptions(k,6)); + fprintf(' numberOfLandmarks = %d\n', testOptions(k,7)); + fprintf(' options.includeGPSFactors = %d\n', testOptions(k,8)); + fprintf(' options.gpsStartPose = %d\n', testOptions(k,9)); + fprintf(' options.trajectoryLength = %d\n', testOptions(k,10)); + fprintf(' options.subsampleStep = %d\n', testOptions(k,11)); + fprintf(' numMonteCarloRuns = %d\n', testOptions(k,12)); + fprintf('\n'); + fprintf(' sigma_ang = %f\n', noises(i,1)); + fprintf(' sigma_cart = %f\n', noises(i,2)); + fprintf(' sigma_accel = %f\n', noises(i,3)); + fprintf(' sigma_gyro = %f\n', noises(i,4)); + fprintf(' sigma_accelBias = %f\n', noises(i,5)); + fprintf(' sigma_gyroBias = %f\n', noises(i,6)); + fprintf(' sigma_gps = %f\n', noises(i,7)); +end + + + + + + + + + + + + + + + + + +