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::GaussianFactor;
|
||||||
virtual class gtsam::HessianFactor;
|
virtual class gtsam::HessianFactor;
|
||||||
virtual class gtsam::JacobianFactor;
|
virtual class gtsam::JacobianFactor;
|
||||||
|
virtual class gtsam::SmartFactorBase;
|
||||||
|
virtual class gtsam::SmartProjectionFactor;
|
||||||
class gtsam::GaussianFactorGraph;
|
class gtsam::GaussianFactorGraph;
|
||||||
class gtsam::NonlinearFactorGraph;
|
class gtsam::NonlinearFactorGraph;
|
||||||
class gtsam::Ordering;
|
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>
|
#include <gtsam_unstable/slam/SmartProjectionPoseFactor.h>
|
||||||
template<POSE = {gtsam::Pose3},LANDMARK = {gtsam::Point3},CALIBRATION = {gtsam::Cal3_S2}>
|
template<POSE = {gtsam::Pose3},LANDMARK = {gtsam::Point3},CALIBRATION = {gtsam::Cal3_S2}>
|
||||||
virtual class SmartProjectionPoseFactor : gtsam::SmartProjectionFactor {
|
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);
|
//void add(gtsam::Point2 measured_i, size_t poseKey_i, gtsam::SharedNoiseModel noise_i, CALIBRATION* K_i);
|
||||||
|
|
||||||
// enabling serialization functionality
|
// 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>
|
#include <gtsam/slam/RangeFactor.h>
|
||||||
template<POSE, POINT>
|
template<POSE, POINT>
|
||||||
virtual class RangeFactor : gtsam::NonlinearFactor {
|
virtual class RangeFactor : gtsam::NonlinearFactor {
|
||||||
|
|
|
||||||
|
|
@ -7,53 +7,56 @@ import gtsam.*;
|
||||||
% Authors: Luca Carlone, David Jensen
|
% Authors: Luca Carlone, David Jensen
|
||||||
% Date: 2014/4/6
|
% Date: 2014/4/6
|
||||||
|
|
||||||
clc
|
|
||||||
clear all
|
|
||||||
close all
|
|
||||||
|
|
||||||
saveResults = 0;
|
% Check for an extneral configuration, used when running multiple tests
|
||||||
|
if ~exist('externallyConfigured', 'var')
|
||||||
|
clc
|
||||||
|
clear all
|
||||||
|
close all
|
||||||
|
|
||||||
%% Configuration
|
saveResults = 0;
|
||||||
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)
|
%% Configuration
|
||||||
options.imuFactorType = 2; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1)
|
% General options
|
||||||
options.imuNonzeroBias = 1; % if true, a nonzero bias is applied to IMU measurements
|
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.includeCameraFactors = 0; % not fully implemented yet
|
options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities)
|
||||||
numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors
|
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.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses
|
options.includeCameraFactors = 0; % not fully implemented yet
|
||||||
options.gpsStartPose = 100; % Pose number to start including GPS factors at
|
numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors
|
||||||
|
|
||||||
options.trajectoryLength = 209;%209; % length of the ground truth trajectory
|
options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses
|
||||||
options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories)
|
options.gpsStartPose = 100; % Pose number to start including GPS factors at
|
||||||
|
|
||||||
numMonteCarloRuns = 20; % number of Monte Carlo runs to perform
|
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)
|
||||||
|
|
||||||
%% Camera metadata
|
numMonteCarloRuns = 5; % number of Monte Carlo runs to perform
|
||||||
K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration
|
|
||||||
cameraMeasurementNoiseSigma = 1.0;
|
|
||||||
cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma);
|
|
||||||
|
|
||||||
% Create landmarks
|
% Noise values to be adjusted
|
||||||
if options.includeCameraFactors == 1
|
sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2
|
||||||
for i = 1:numberOfLandmarks
|
sigma_cart = 1e-1; % std. deviation for translational noise, typical 1e-1
|
||||||
gtLandmarkPoints(i) = Point3( ...
|
sigma_accel = 1e-1; % std. deviation for accelerometer noise, typical 1e-3
|
||||||
... % uniformly distributed in the x axis along 120% of the trajectory length, starting after 15 poses
|
sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5
|
||||||
[rand()*20*(options.trajectoryLength*1.2) + 15*20; ...
|
sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3
|
||||||
randn()*20; ... % normally distributed in the y axis with a sigma of 20
|
sigma_gyroBias = 1e-6; % std. deviation for added gyroscope constant bias, typical 1e-5
|
||||||
randn()*20]); % normally distributed in the z axis with a sigma of 20
|
sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4
|
||||||
end
|
|
||||||
|
% 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
|
end
|
||||||
|
|
||||||
%% Imu metadata
|
%% Between metadata
|
||||||
sigma_accel = 1e-3; % std. deviation for accelerometer noise, typical 1e-3
|
noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)];
|
||||||
sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5
|
noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose);
|
||||||
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
|
|
||||||
|
|
||||||
|
%% Imu metadata
|
||||||
metadata.imu.epsBias = 1e-10; % was 1e-7
|
metadata.imu.epsBias = 1e-10; % was 1e-7
|
||||||
metadata.imu.g = [0;0;0];
|
metadata.imu.g = [0;0;0];
|
||||||
metadata.imu.omegaCoriolis = [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);
|
noiseVectorAccel = metadata.imu.AccelerometerSigma * ones(3,1);
|
||||||
noiseVectorGyro = metadata.imu.GyroscopeSigma * 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
|
%% GPS metadata
|
||||||
sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4
|
|
||||||
noiseVectorGPS = sigma_gps * ones(3,1);
|
noiseVectorGPS = sigma_gps * ones(3,1);
|
||||||
noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]);
|
noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]);
|
||||||
|
|
||||||
%% Set log files
|
%% Camera metadata
|
||||||
testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro)
|
K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration
|
||||||
folderName = 'results/'
|
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
|
%% Create ground truth trajectory and measurements
|
||||||
[gtValues, gtMeasurements] = imuSimulator.covarianceAnalysisCreateTrajectory(options, metadata);
|
[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