added script to run and save tests in a simplified way
djensen3
parent
32e1a8f994
commit
ce13807d10
|
|
@ -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 <gtsam_unstable/slam/SmartFactorBase.h>
|
||||
template<POSE, CALIBRATION, D>
|
||||
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 <gtsam_unstable/slam/SmartProjectionFactor.h>
|
||||
template<POSE, LANDMARK, CALIBRATION, D>
|
||||
virtual class SmartProjectionFactor : gtsam::SmartFactorBase {
|
||||
SmartProjectionFactor(double rankTol, double linThreshold,
|
||||
bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor);
|
||||
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam_unstable/slam/SmartProjectionPoseFactor.h>
|
||||
template<POSE = {gtsam::Pose3},LANDMARK = {gtsam::Point3},CALIBRATION = {gtsam::Cal3_S2}>
|
||||
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<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2 > SmartProjectionPose3Factor;
|
||||
|
||||
|
||||
#include <gtsam/slam/RangeFactor.h>
|
||||
template<POSE, POINT>
|
||||
virtual class RangeFactor : gtsam::NonlinearFactor {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue