113 lines
4.7 KiB
Matlab
113 lines
4.7 KiB
Matlab
function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata)
|
|
% Create a factor graph based on provided measurements, values, and noises.
|
|
% Used for covariance analysis scripts.
|
|
% 'options' contains fields for including various factor types.
|
|
% 'metadata' is a storage variable for miscellaneous factor-specific values
|
|
% Authors: Luca Carlone, David Jensen
|
|
% Date: 2014/04/16
|
|
|
|
import gtsam.*;
|
|
|
|
graph = NonlinearFactorGraph;
|
|
|
|
% Iterate through the trajectory
|
|
for i=0:size(measurements.deltaMatrix, 1);
|
|
% Get the current pose
|
|
currentPoseKey = symbol('x', i);
|
|
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')
|
|
|
|
% Pose prior (poses used for all factors)
|
|
initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1));
|
|
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
|
|
|
|
% IMU velocity and bias priors
|
|
if options.includeIMUFactors == 1
|
|
currentVelKey = symbol('v', 0);
|
|
currentBiasKey = symbol('b', 0);
|
|
currentVel = [0; 0; 0];
|
|
graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
|
|
graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, noiseModels.noisePriorBias));
|
|
end
|
|
|
|
% Camera priors
|
|
if options.includeCameraFactors == 1
|
|
pointNoiseSigma = 0.1;
|
|
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
|
|
graph.add(PriorFactorPoint3(symbol('p',1), gtLandmarkPoints(1), pointPriorNoise));
|
|
end
|
|
|
|
else
|
|
prevPoseKey = symbol('x', i-1);
|
|
|
|
%% Add Between factors
|
|
if options.includeBetweenFactors == 1
|
|
% Create the noisy pose estimate
|
|
deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)' ...
|
|
+ (measurementNoise.poseNoiseVector .* randn(6,1))); % added noise
|
|
% Add the between factor to the graph
|
|
graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose));
|
|
end % end of Between pose factor creation
|
|
|
|
%% Add IMU factors
|
|
if options.includeIMUFactors == 1
|
|
% Update keys
|
|
currentVelKey = symbol('v', i); % not used if includeIMUFactors is false
|
|
currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false
|
|
% Generate IMU measurements with noise
|
|
imuAccel = measurements.imu.accel(i,:)' ...
|
|
+ (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise
|
|
imuGyro = measurements.imu.gyro(i,:)' ...
|
|
+ (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise
|
|
% Initialize preintegration
|
|
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
|
|
metadata.imu.zeroBias, ...
|
|
metadata.imu.AccelerometerSigma.^2 * eye(3), ...
|
|
metadata.imu.GyroscopeSigma.^2 * eye(3), ...
|
|
metadata.imu.IntegrationSigma.^2 * eye(3));
|
|
% Preintegrate
|
|
imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
|
|
% Add Imu factor
|
|
graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ...
|
|
currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis));
|
|
% Add between factor on biases
|
|
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)));
|
|
end % end of IMU factor creation
|
|
|
|
%% Add Camera factors - UNDER CONSTRUCTION !!!! -
|
|
if options.includeCameraFactors == 1
|
|
% Create camera with the current pose and calibration K (specified above)
|
|
gtCamera = SimpleCamera(currentPose, K);
|
|
% Project landmarks into the camera
|
|
numSkipped = 0;
|
|
for j = 1:length(gtLandmarkPoints)
|
|
landmarkKey = symbol('p', j);
|
|
try
|
|
Z = gtCamera.project(gtLandmarkPoints(j));
|
|
%% TO-DO probably want to do some type of filtering on the measurement values, because
|
|
% they might not all be valid
|
|
graph.add(GenericProjectionFactorCal3_S2(Z, cameraMeasurementNoise, currentPoseKey, landmarkKey, K));
|
|
catch
|
|
% Most likely the point is not within the camera's view, which
|
|
% is fine
|
|
numSkipped = numSkipped + 1;
|
|
end
|
|
end
|
|
%fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped);
|
|
end % end of Camera factor creation
|
|
|
|
end % end of else
|
|
|
|
end % end of for over trajectory
|
|
|
|
end
|
|
|