working covariance example
Luca
parent
d394ec5574
commit
d325fd1251
|
|
@ -1,6 +1,8 @@
|
|||
import gtsam.*;
|
||||
|
||||
% Test GTSAM covariances on a graph with betweenFactors
|
||||
% Authors: Luca Carlone, David Jensen
|
||||
% Date: 2014/4/6
|
||||
|
||||
clc
|
||||
clear all
|
||||
|
|
@ -8,69 +10,150 @@ close all
|
|||
|
||||
%% Create ground truth trajectory
|
||||
trajectoryLength = 5;
|
||||
unsmooth_DP = 0.5; % controls smoothness on translation norm
|
||||
unsmooth_DR = 0.1; % controls smoothness on translation norm
|
||||
|
||||
% 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
|
||||
|
||||
noiseVector = [0.01; 0.0001; 0.0001; 0.1; 0.1; 0.1];
|
||||
noise = noiseModel.Diagonal.Sigmas(noiseVector);
|
||||
|
||||
currentPoseKey = symbol('x', 0);
|
||||
currentPose = Pose3; % initial pose
|
||||
gtValues.insert(currentPoseKey, currentPose);
|
||||
gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise));
|
||||
|
||||
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;
|
||||
gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5
|
||||
gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad)
|
||||
gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition];
|
||||
deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)');
|
||||
|
||||
% "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;
|
||||
% Add the factors to the factor graph
|
||||
gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise));
|
||||
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;
|
||||
figure(1)
|
||||
hold on;
|
||||
|
||||
plot3DTrajectory(gtResult, [], Marginals(gtGraph, gtResult));
|
||||
plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
|
||||
% plot3DTrajectory(values,linespec,frames,scale,marginals)
|
||||
axis equal
|
||||
|
||||
% Compute covariances using gtGraph and gtValues (for visualization)
|
||||
numMonteCarloRuns = 100;
|
||||
|
||||
% decide measurement covariance
|
||||
for k=1:numMonteCarloRuns
|
||||
% create a new graph
|
||||
graph = NonlinearFactorGraph;
|
||||
|
||||
% noisy prior
|
||||
currentPoseKey = symbol('x', 0);
|
||||
noisyDelta = noiseVector .* randn(6,1);
|
||||
initialPose = Pose3.Expmap(noisyDelta);
|
||||
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise));
|
||||
|
||||
for i=1:trajectoryLength
|
||||
currentPoseKey = symbol('x', i);
|
||||
|
||||
% for each measurement. add noise and add to graph
|
||||
noisyDelta = gtDeltaMatrix(i,:)' + (noiseVector .* randn(6,1));
|
||||
noisyDeltaPose = Pose3.Expmap(noisyDelta);
|
||||
|
||||
% Add the factors to the factor graph
|
||||
graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noise));
|
||||
end
|
||||
|
||||
% optimize
|
||||
optimizer = GaussNewtonOptimizer(graph, gtValues);
|
||||
estimate = optimizer.optimize();
|
||||
|
||||
figure(1)
|
||||
plot3DTrajectory(estimate, '-b');
|
||||
|
||||
marginals = Marginals(graph, estimate);
|
||||
|
||||
% for each pose in the trajectory
|
||||
for i=1:trajectoryLength+1
|
||||
% compute estimation errors
|
||||
currentPoseKey = symbol('x', i-1);
|
||||
gtPosition = gtValues.at(currentPoseKey).translation.vector;
|
||||
estPosition = estimate.at(currentPoseKey).translation.vector;
|
||||
errPosition = estPosition - gtPosition;
|
||||
|
||||
% compute covariances:
|
||||
cov = marginals.marginalCovariance(currentPoseKey);
|
||||
covPosition = cov(4:6,4:6);
|
||||
|
||||
% compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances
|
||||
NEES(k,i) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof
|
||||
end
|
||||
|
||||
%% 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"
|
||||
figure(2)
|
||||
hold on
|
||||
plot(NEES(k,:),'-b','LineWidth',1.5)
|
||||
end
|
||||
ANEES = mean(NEES);
|
||||
plot(ANEES,'-r','LineWidth',2)
|
||||
plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof
|
||||
box on
|
||||
set(gca,'Fontsize',16)
|
||||
title('NEES and ANEES');
|
||||
|
||||
%% compute statistics: ANEES, plots
|
||||
figure(1)
|
||||
box on
|
||||
title('Ground truth and estimates for each MC runs');
|
||||
set(gca,'Fontsize',16)
|
||||
|
||||
%% Let us compute statistics on the overall NEES
|
||||
n = 3; % position vector dimension
|
||||
N = numMonteCarloRuns; % number of runs
|
||||
alpha = 0.01; % confidence level
|
||||
|
||||
% mean_value = n*N; % mean value of the Chi-square distribution
|
||||
% (we divide by n * N and for this reason we expect ANEES around 1)
|
||||
r1 = chi2inv(alpha, n * N) / (n * N);
|
||||
r2 = chi2inv(1-alpha, n * N) / (n * N);
|
||||
|
||||
% output here
|
||||
fprintf(1, 'r1 = %g\n', r1);
|
||||
fprintf(1, 'r2 = %g\n', r2);
|
||||
|
||||
figure(3)
|
||||
hold on
|
||||
plot(ANEES/n,'-b','LineWidth',2)
|
||||
plot(ones(size(ANEES,2),1),'r-');
|
||||
plot(r1*ones(size(ANEES,2),1),'k-.');
|
||||
plot(r2*ones(size(ANEES,2),1),'k-.');
|
||||
box on
|
||||
title('NEES normalized by dof VS bounds');
|
||||
set(gca,'Fontsize',16)
|
||||
|
||||
%% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4)
|
||||
% the nees for a single experiment (i) is defined as
|
||||
% NEES_i = xtilda' * inv(P) * xtilda,
|
||||
% where xtilda in R^n is the estimation
|
||||
% error, and P is the covariance estimated by the approach we want to test
|
||||
%
|
||||
% Average NEES. Given N Monte Carlo simulations, i=1,...,N, the average
|
||||
% NEES is:
|
||||
% ANEES = sum(NEES_i)/N
|
||||
% The quantity N*ANEES is distributed according to a Chi-square
|
||||
% distribution with N*n degrees of freedom.
|
||||
%
|
||||
% For the single run case, N=1, therefore NEES = ANEES is distributed
|
||||
% according to a chi-square distribution with n degrees of freedom (e.g. n=3
|
||||
% if we are testing a position estimate)
|
||||
% Therefore its mean should be n (difficult to see from a single run)
|
||||
% and, with probability alpha, it should hold:
|
||||
%
|
||||
% NEES in [r1, r2]
|
||||
%
|
||||
% where r1 and r2 are built from the Chi-square distribution
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue