diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 35b2dd52d..65ab61dad 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -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) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index ca0ce61b8..70a9a5bda 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index d0e58afdf..9c54af687 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -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