minor fixes
Luca
parent
e1c13c87d7
commit
9bc0ddd4a2
|
|
@ -1,7 +1,9 @@
|
|||
import gtsam.*;
|
||||
|
||||
% Test GTSAM covariances on a graph with betweenFactors
|
||||
% Optionally, you can also enable IMU factors and Camera factors
|
||||
% Test GTSAM covariances on a factor graph with:
|
||||
% Between Factors
|
||||
% IMU factors
|
||||
% Projection factors
|
||||
% Authors: Luca Carlone, David Jensen
|
||||
% Date: 2014/4/6
|
||||
|
||||
|
|
@ -11,17 +13,20 @@ close all
|
|||
|
||||
%% Configuration
|
||||
options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj
|
||||
options.includeBetweenFactors = 1; % if true, BetweenFactors will be generated between consecutive poses
|
||||
options.includeIMUFactors = 1; % if true, IMU factors will be generated for the trajectory based on options.imuFactorType
|
||||
options.includeBetweenFactors = 1; % 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 = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1)
|
||||
|
||||
options.includeCameraFactors = 0; % not fully implemented yet
|
||||
numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors
|
||||
|
||||
options.trajectoryLength = 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
|
||||
|
||||
%% Camera metadata
|
||||
numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors
|
||||
K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration
|
||||
cameraMeasurementNoiseSigma = 1.0;
|
||||
cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma);
|
||||
|
|
@ -40,29 +45,27 @@ end
|
|||
%% Imu metadata
|
||||
metadata.imu.epsBias = 1e-10; % was 1e-7
|
||||
metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
|
||||
metadata.imu.AccelerometerSigma = 1e-5;
|
||||
metadata.imu.GyroscopeSigma = 1e-7;
|
||||
metadata.imu.IntegrationSigma = 1e-4;
|
||||
metadata.imu.AccelerometerSigma = 1e-3;
|
||||
metadata.imu.GyroscopeSigma = 1e-5;
|
||||
metadata.imu.IntegrationSigma = 1e-5;
|
||||
metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias;
|
||||
metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias;
|
||||
metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias;
|
||||
metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias;
|
||||
metadata.imu.g = [0;0;0];
|
||||
metadata.imu.omegaCoriolis = [0;0;0];
|
||||
noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1
|
||||
noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias);
|
||||
noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-4);
|
||||
noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias); % between on biases
|
||||
noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-6);
|
||||
|
||||
sigma_accel = metadata.imu.AccelerometerSigma;
|
||||
sigma_gyro = metadata.imu.GyroscopeSigma;
|
||||
noiseVectorAccel = [sigma_accel; sigma_accel; sigma_accel];
|
||||
noiseVectorGyro = [sigma_gyro; sigma_gyro; sigma_gyro];
|
||||
|
||||
noiseVectorAccel = sigma_accel * ones(3,1);
|
||||
noiseVectorGyro = sigma_gyro * ones(3,1);
|
||||
|
||||
%% Between metadata
|
||||
sigma_ang = 1e-3; sigma_cart = 1e-3;
|
||||
noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart];
|
||||
sigma_ang = 1e-2; sigma_cart = 1e-1;
|
||||
noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)];
|
||||
noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose);
|
||||
%noisePose = noiseModel.Isotropic.Sigma(6, 1e-3);
|
||||
|
||||
%% Set log files
|
||||
testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart)
|
||||
|
|
|
|||
|
|
@ -17,10 +17,7 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
currentPose = values.at(currentPoseKey);
|
||||
|
||||
if i==0
|
||||
%% first time step, add priors
|
||||
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
|
||||
warning('using identity rotation')
|
||||
|
||||
%% first time step, add priors
|
||||
% Pose prior (poses used for all factors)
|
||||
initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1));
|
||||
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
|
||||
|
|
@ -28,10 +25,12 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
% IMU velocity and bias priors
|
||||
if options.includeIMUFactors == 1
|
||||
currentVelKey = symbol('v', 0);
|
||||
currentBiasKey = symbol('b', 0);
|
||||
currentVel = [0; 0; 0];
|
||||
currentVel = values.at(currentVelKey).vector;
|
||||
graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
|
||||
graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, noiseModels.noisePriorBias));
|
||||
|
||||
currentBiasKey = symbol('b', 0);
|
||||
currentBias = values.at(currentBiasKey);
|
||||
graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias));
|
||||
end
|
||||
|
||||
% Camera priors
|
||||
|
|
@ -84,7 +83,7 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
imuMeasurement, ...
|
||||
metadata.imu.g, metadata.imu.omegaCoriolis, ...
|
||||
noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias)));
|
||||
else % Assumed to be type 1 if type 2 is not selected
|
||||
else % IMU type 1
|
||||
% Initialize preintegration
|
||||
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
|
||||
metadata.imu.zeroBias, ...
|
||||
|
|
@ -100,8 +99,8 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
|
||||
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
|
||||
% Additional prior on zerobias
|
||||
graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ...
|
||||
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
|
||||
%graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ...
|
||||
% noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
|
||||
end
|
||||
|
||||
end % end of IMU factor creation
|
||||
|
|
@ -128,9 +127,9 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
%fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped);
|
||||
end % end of Camera factor creation
|
||||
|
||||
end % end of else
|
||||
end % end of else (i=0)
|
||||
|
||||
end % end of for over trajectory
|
||||
|
||||
end
|
||||
end % end of function
|
||||
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
function [ values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata )
|
||||
function [values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata )
|
||||
% Create a trajectory for running covariance analysis scripts.
|
||||
% 'options' contains fields for including various factor types and setting trajectory length
|
||||
% 'metadata' is a storage variable for miscellaneous factor-specific values
|
||||
|
|
@ -9,6 +9,9 @@ import gtsam.*;
|
|||
|
||||
values = Values;
|
||||
|
||||
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
|
||||
warning('using identity rotation')
|
||||
|
||||
if options.useRealData == 1
|
||||
%% Create a ground truth trajectory from Real data (if available)
|
||||
fprintf('\nUsing real data as ground truth\n');
|
||||
|
|
@ -26,7 +29,7 @@ if options.useRealData == 1
|
|||
% Update the pose key
|
||||
currentPoseKey = symbol('x', i-1);
|
||||
|
||||
% Generate the current pose
|
||||
% Generate the current pose
|
||||
scenarioInd = options.subsampleStep * (i-1) + 1;
|
||||
fprintf('%d, ', scenarioInd);
|
||||
if mod(i,20) == 0
|
||||
|
|
@ -40,7 +43,7 @@ if options.useRealData == 1
|
|||
[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
|
||||
|
||||
gtPosition = [xlt, ylt, zlt]';
|
||||
gtRotation = Rot3; % Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
|
||||
gtRotation = Rot3; %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
|
||||
currentPose = Pose3(gtRotation, Point3(gtPosition));
|
||||
|
||||
% Add values
|
||||
|
|
@ -94,7 +97,7 @@ if options.includeIMUFactors == 1
|
|||
% Update Keys
|
||||
currentVelKey = symbol('v', i);
|
||||
currentBiasKey = symbol('b', i);
|
||||
|
||||
|
||||
if i == 0
|
||||
% Add initial values
|
||||
currentVel = [0 0 0];
|
||||
|
|
@ -105,14 +108,14 @@ if options.includeIMUFactors == 1
|
|||
|
||||
% create accel and gyro measurements based on
|
||||
measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i);
|
||||
|
||||
|
||||
% acc = (deltaPosition - initialVel * dT) * (2/dt^2)
|
||||
measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ...
|
||||
- currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i)));
|
||||
|
||||
- currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i)));
|
||||
|
||||
% Update velocity
|
||||
currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i);
|
||||
|
||||
|
||||
% Add Values: velocity and bias
|
||||
values.insert(currentVelKey, LieVector(currentVel'));
|
||||
values.insert(currentBiasKey, metadata.imu.zeroBias);
|
||||
|
|
@ -120,5 +123,5 @@ if options.includeIMUFactors == 1
|
|||
end
|
||||
end % end of IMU measurements
|
||||
|
||||
end
|
||||
end % end of function
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue