12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

minor fixes

release/4.3a0
Luca 2014-04-17 14:11:18 -04:00
parent e1c13c87d7
commit 9bc0ddd4a2
3 changed files with 43 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
View File

@ -1,7 +1,9 @@
import gtsam.*; import gtsam.*;
% Test GTSAM covariances on a graph with betweenFactors % Test GTSAM covariances on a factor graph with:
% Optionally, you can also enable IMU factors and Camera factors % Between Factors
% IMU factors
% Projection factors
% Authors: Luca Carlone, David Jensen % Authors: Luca Carlone, David Jensen
% Date: 2014/4/6 % Date: 2014/4/6
@ -11,17 +13,20 @@ close all
%% Configuration %% Configuration
options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj 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.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses
options.includeIMUFactors = 1; % if true, IMU factors will be generated for the trajectory based on options.imuFactorType
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.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 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.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) 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 numMonteCarloRuns = 2; % number of Monte Carlo runs to perform
%% Camera metadata %% 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 K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration
cameraMeasurementNoiseSigma = 1.0; cameraMeasurementNoiseSigma = 1.0;
cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma); cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma);
@ -40,29 +45,27 @@ end
%% Imu metadata %% Imu metadata
metadata.imu.epsBias = 1e-10; % was 1e-7 metadata.imu.epsBias = 1e-10; % was 1e-7
metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
metadata.imu.AccelerometerSigma = 1e-5; metadata.imu.AccelerometerSigma = 1e-3;
metadata.imu.GyroscopeSigma = 1e-7; metadata.imu.GyroscopeSigma = 1e-5;
metadata.imu.IntegrationSigma = 1e-4; metadata.imu.IntegrationSigma = 1e-5;
metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias; 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.BiasAccOmegaInit = metadata.imu.epsBias;
metadata.imu.g = [0;0;0]; metadata.imu.g = [0;0;0];
metadata.imu.omegaCoriolis = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0];
noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1
noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias); noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias); % between on biases
noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-4); noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-6);
sigma_accel = metadata.imu.AccelerometerSigma; sigma_accel = metadata.imu.AccelerometerSigma;
sigma_gyro = metadata.imu.GyroscopeSigma; sigma_gyro = metadata.imu.GyroscopeSigma;
noiseVectorAccel = [sigma_accel; sigma_accel; sigma_accel]; noiseVectorAccel = sigma_accel * ones(3,1);
noiseVectorGyro = [sigma_gyro; sigma_gyro; sigma_gyro]; noiseVectorGyro = sigma_gyro * ones(3,1);
%% Between metadata %% Between metadata
sigma_ang = 1e-3; sigma_cart = 1e-3; sigma_ang = 1e-2; sigma_cart = 1e-1;
noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)];
noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose);
%noisePose = noiseModel.Isotropic.Sigma(6, 1e-3);
%% Set log files %% Set log files
testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart) testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart)

21
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -18,9 +18,6 @@ for i=0:size(measurements.deltaMatrix, 1);
if i==0 if i==0
%% first time step, add priors %% first time step, add priors
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
warning('using identity rotation')
% Pose prior (poses used for all factors) % Pose prior (poses used for all factors)
initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1));
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
@ -28,10 +25,12 @@ for i=0:size(measurements.deltaMatrix, 1);
% IMU velocity and bias priors % IMU velocity and bias priors
if options.includeIMUFactors == 1 if options.includeIMUFactors == 1
currentVelKey = symbol('v', 0); currentVelKey = symbol('v', 0);
currentBiasKey = symbol('b', 0); currentVel = values.at(currentVelKey).vector;
currentVel = [0; 0; 0];
graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel)); 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 end
% Camera priors % Camera priors
@ -84,7 +83,7 @@ for i=0:size(measurements.deltaMatrix, 1);
imuMeasurement, ... imuMeasurement, ...
metadata.imu.g, metadata.imu.omegaCoriolis, ... metadata.imu.g, metadata.imu.omegaCoriolis, ...
noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias))); 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 % Initialize preintegration
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
metadata.imu.zeroBias, ... metadata.imu.zeroBias, ...
@ -100,8 +99,8 @@ for i=0:size(measurements.deltaMatrix, 1);
graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
% Additional prior on zerobias % Additional prior on zerobias
graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ... %graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ...
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); % noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
end end
end % end of IMU factor creation 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); %fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped);
end % end of Camera factor creation end % end of Camera factor creation
end % end of else end % end of else (i=0)
end % end of for over trajectory end % end of for over trajectory
end end % end of function

11
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
View File

@ -1,4 +1,4 @@
function [ values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata ) function [values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata )
% Create a trajectory for running covariance analysis scripts. % Create a trajectory for running covariance analysis scripts.
% 'options' contains fields for including various factor types and setting trajectory length % 'options' contains fields for including various factor types and setting trajectory length
% 'metadata' is a storage variable for miscellaneous factor-specific values % 'metadata' is a storage variable for miscellaneous factor-specific values
@ -9,6 +9,9 @@ import gtsam.*;
values = Values; values = Values;
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
warning('using identity rotation')
if options.useRealData == 1 if options.useRealData == 1
%% Create a ground truth trajectory from Real data (if available) %% Create a ground truth trajectory from Real data (if available)
fprintf('\nUsing real data as ground truth\n'); fprintf('\nUsing real data as ground truth\n');
@ -40,7 +43,7 @@ if options.useRealData == 1
[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
gtPosition = [xlt, ylt, zlt]'; 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)); currentPose = Pose3(gtRotation, Point3(gtPosition));
% Add values % Add values
@ -108,7 +111,7 @@ if options.includeIMUFactors == 1
% acc = (deltaPosition - initialVel * dT) * (2/dt^2) % acc = (deltaPosition - initialVel * dT) * (2/dt^2)
measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ... 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 % Update velocity
currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i); currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i);
@ -120,5 +123,5 @@ if options.includeIMUFactors == 1
end end
end % end of IMU measurements end % end of IMU measurements
end end % end of function