77 lines
2.6 KiB
Matlab
77 lines
2.6 KiB
Matlab
import gtsam.*;
|
|
|
|
% Test GTSAM covariances on a graph with betweenFactors
|
|
|
|
clc
|
|
clear all
|
|
close all
|
|
|
|
%% Create ground truth trajectory
|
|
trajectoryLength = 5;
|
|
|
|
% possibly create random trajectory
|
|
currentPoseKey = symbol('x', 0);
|
|
currentPose = Pose3;
|
|
gtValues = Values;
|
|
gtValues.insert(currentPoseKey, currentPose);
|
|
gtGraph = NonlinearFactorGraph;
|
|
gtNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.1; 0.05; 0.05; 0.05]); % Noise for GT measurements
|
|
|
|
for i=1:trajectoryLength
|
|
currentPoseKey = symbol('x', i);
|
|
deltaPosition = 0.5*rand(3,1) + [1;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5
|
|
deltaRotation = 0.1*rand(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad)
|
|
deltaPose = Pose3.Expmap([deltaRotation; deltaPosition]);
|
|
deltaPoseNoise = gtNoise;
|
|
|
|
% "Deduce" ground truth measurements
|
|
% deltaPose are the gt measurements - save them in some structure
|
|
gtMeasurementPose(i) = deltaPose;
|
|
currentPose = currentPose.compose(deltaPose);
|
|
gtValues.insert(currentPoseKey, currentPose);
|
|
|
|
% Add the factor to the factor graph
|
|
if(i == 1)
|
|
gtGraph.add(PriorFactorPose3(currentPoseKey, deltaPose, deltaPoseNoise));
|
|
else
|
|
gtGraph.add(BetweenFactorPose3(previousPoseKey, currentPoseKey, deltaPose, deltaPoseNoise));
|
|
end
|
|
previousPoseKey = currentPoseKey;
|
|
end
|
|
|
|
gtGraph.print(sprintf('\nGround Truth - Factor graph:\n'));
|
|
gtValues.print(sprintf('\nGround Truth - Values:\n'));
|
|
|
|
%% Create gt graph (using between with ground truth measurements)
|
|
% Optimize using Levenberg-Marquardt
|
|
optimizer = LevenbergMarquardtOptimizer(gtGraph, gtValues);
|
|
gtResult = optimizer.optimizeSafely();
|
|
gtResult.print(sprintf('\nGround Truth - Final Result:\n'));
|
|
|
|
% Plot trajectory and covariance ellipses
|
|
% Couldn't get this to work in the modified example (OdometryExample3D).
|
|
% Something strange with 3D trajectories?
|
|
cla;
|
|
hold on;
|
|
|
|
plot3DTrajectory(gtResult, [], Marginals(gtGraph, gtResult));
|
|
axis equal
|
|
|
|
% Compute covariances using gtGraph and gtValues (for visualization)
|
|
|
|
% decide measurement covariance
|
|
|
|
%% for k=1:numMonteCarloRuns
|
|
% create a new graph
|
|
% for each measurement. add noise and add to graph
|
|
% optimize
|
|
% compute covariances:
|
|
% compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances
|
|
% "estimationError = estimatedValues - gtValues" only holds in a linear case
|
|
% in a nonlinear case estimationError = LogMap ((estimatedValues.inverse) * gtValues)
|
|
% in GTSAM you should check "localCoordinates"
|
|
|
|
%% compute statistics: ANEES, plots
|
|
|
|
|