Concurrent calibration now also with IMU
cbeall3
parent
8bc87e8f4f
commit
e8f3a7e459
|
|
@ -0,0 +1,279 @@
|
||||||
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||||
|
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
% Atlanta, Georgia 30332-0415
|
||||||
|
% All Rights Reserved
|
||||||
|
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
%
|
||||||
|
% See LICENSE for the license information
|
||||||
|
%
|
||||||
|
% @brief Estimate trajectory, calibration, landmarks, body-camera offset,
|
||||||
|
% IMU
|
||||||
|
% @author Chris Beall
|
||||||
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||||
|
clear all;
|
||||||
|
clc;
|
||||||
|
import gtsam.*
|
||||||
|
|
||||||
|
write_video = true;
|
||||||
|
|
||||||
|
if(write_video)
|
||||||
|
videoObj = VideoWriter('test.avi');
|
||||||
|
videoObj.Quality = 100;
|
||||||
|
videoObj.FrameRate = 2;
|
||||||
|
open(videoObj);
|
||||||
|
end
|
||||||
|
|
||||||
|
%% generate some landmarks
|
||||||
|
nrPoints = 8;
|
||||||
|
landmarks = {Point3([20 15 1]'),...
|
||||||
|
Point3([22 7 -1]'),...
|
||||||
|
Point3([20 20 6]'),...
|
||||||
|
Point3([24 19 -4]'),...
|
||||||
|
Point3([26 17 -2]'),...
|
||||||
|
Point3([12 15 4]'),...
|
||||||
|
Point3([25 11 -6]'),...
|
||||||
|
Point3([23 10 4]')};
|
||||||
|
|
||||||
|
IMU_metadata.AccelerometerSigma = 1e-2;
|
||||||
|
IMU_metadata.GyroscopeSigma = 1e-2;
|
||||||
|
IMU_metadata.AccelerometerBiasSigma = 1e-3;
|
||||||
|
IMU_metadata.GyroscopeBiasSigma = 1e-3;
|
||||||
|
IMU_metadata.IntegrationSigma = 1e-1;
|
||||||
|
|
||||||
|
curvature = 5.0;
|
||||||
|
transformKey = 1000;
|
||||||
|
calibrationKey = 2000;
|
||||||
|
|
||||||
|
fg = NonlinearFactorGraph;
|
||||||
|
initial = Values;
|
||||||
|
|
||||||
|
%% intial landmarks and camera trajectory shifted in + y-direction
|
||||||
|
y_shift = Point3(0,1,0);
|
||||||
|
|
||||||
|
% insert shifted points
|
||||||
|
for i=1:nrPoints
|
||||||
|
initial.insert(100+i,landmarks{i}.compose(y_shift));
|
||||||
|
end
|
||||||
|
|
||||||
|
figure(1);
|
||||||
|
cla
|
||||||
|
hold on;
|
||||||
|
|
||||||
|
%% initial pose priors
|
||||||
|
pose_cov = noiseModel.Diagonal.Sigmas([0.1*pi/180; 0.1*pi/180; 0.1*pi/180; 1e-9; 1e-9; 1e-9]);
|
||||||
|
|
||||||
|
%% Actual camera translation coincides with odometry, but -90deg Z-X rotation
|
||||||
|
camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift);
|
||||||
|
actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3());
|
||||||
|
|
||||||
|
trans_cov = noiseModel.Diagonal.Sigmas([1*pi/180; 1*pi/180; 1*pi/180; 20; 1e-6; 1e-6]);
|
||||||
|
|
||||||
|
|
||||||
|
move_forward = Pose3(Rot3(),Point3(1,0,0));
|
||||||
|
move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(1,0,0));
|
||||||
|
covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
|
||||||
|
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
|
||||||
|
|
||||||
|
%% calibration initialization
|
||||||
|
K = Cal3_S2(900,900,0,640,480);
|
||||||
|
K_corrupt = Cal3_S2(910,890,0,650,470);
|
||||||
|
K_cov = noiseModel.Diagonal.Sigmas([20; 20; 0.001; 20; 20]);
|
||||||
|
|
||||||
|
cheirality_exception_count = 0;
|
||||||
|
|
||||||
|
isamParams = gtsam.ISAM2Params;
|
||||||
|
isamParams.setFactorization('QR');
|
||||||
|
isam = ISAM2(isamParams);
|
||||||
|
|
||||||
|
currentIMUPoseGlobal = Pose3();
|
||||||
|
|
||||||
|
%% Get initial conditions for the estimated trajectory
|
||||||
|
currentVelocityGlobal = LieVector([1;0;0]); % the vehicle is stationary at the beginning
|
||||||
|
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
|
||||||
|
|
||||||
|
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
|
||||||
|
sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]);
|
||||||
|
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ];
|
||||||
|
g = [0;0;-9.8];
|
||||||
|
w_coriolis = [0;0;0];
|
||||||
|
|
||||||
|
|
||||||
|
for i=1:20
|
||||||
|
|
||||||
|
t = i-1;
|
||||||
|
|
||||||
|
currentVelKey = symbol('v',i);
|
||||||
|
currentBiasKey = symbol('b',i);
|
||||||
|
|
||||||
|
initial.insert(currentVelKey, currentVelocityGlobal);
|
||||||
|
initial.insert(currentBiasKey, currentBias);
|
||||||
|
|
||||||
|
if i==1
|
||||||
|
|
||||||
|
% Pose Priors
|
||||||
|
fg.add(PriorFactorPose3(1, Pose3(),pose_cov));
|
||||||
|
fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov));
|
||||||
|
|
||||||
|
% insert first
|
||||||
|
initial.insert(1, Pose3());
|
||||||
|
|
||||||
|
% camera transform
|
||||||
|
initial.insert(transformKey,camera_transform);
|
||||||
|
fg.add(PriorFactorPose3(transformKey,camera_transform,trans_cov));
|
||||||
|
|
||||||
|
% calibration
|
||||||
|
initial.insert(2000, K_corrupt);
|
||||||
|
fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov));
|
||||||
|
|
||||||
|
% velocity and bias evolution
|
||||||
|
fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
|
||||||
|
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
|
||||||
|
|
||||||
|
result = initial;
|
||||||
|
else
|
||||||
|
if i < 11
|
||||||
|
step = move_forward;
|
||||||
|
else
|
||||||
|
step = move_circle;
|
||||||
|
end
|
||||||
|
|
||||||
|
initial.insert(i,result.at(i-1).compose(step));
|
||||||
|
% fg.add(BetweenFactorPose3(i-1,i, step, covariance));
|
||||||
|
|
||||||
|
deltaT = 1;
|
||||||
|
logmap = Pose3.Logmap(step);
|
||||||
|
omega = logmap(1:3);
|
||||||
|
velocity = logmap(4:6);
|
||||||
|
%% Simulate IMU measurements, considering Coriolis effect
|
||||||
|
% (in this simple example we neglect gravity and there are no other forces acting on the body)
|
||||||
|
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
|
||||||
|
omega, omega, velocity, velocity, deltaT);
|
||||||
|
|
||||||
|
[ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
|
||||||
|
currentIMUPoseGlobal, omega, velocity, velocity, deltaT);
|
||||||
|
|
||||||
|
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
|
||||||
|
currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
|
||||||
|
IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
|
||||||
|
|
||||||
|
accMeas = acc_omega(1:3)-g;
|
||||||
|
omegaMeas = acc_omega(4:6);
|
||||||
|
currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);
|
||||||
|
|
||||||
|
%% create IMU factor
|
||||||
|
fg.add(ImuFactor( ...
|
||||||
|
i-1, currentVelKey-1, ...
|
||||||
|
i, currentVelKey, ...
|
||||||
|
currentBiasKey, currentSummarizedMeasurement, g, w_coriolis));
|
||||||
|
|
||||||
|
% Bias evolution as given in the IMU metadata
|
||||||
|
fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
|
||||||
|
noiseModel.Diagonal.Sigmas(sqrt(20) * sigma_between_b)));
|
||||||
|
|
||||||
|
end
|
||||||
|
|
||||||
|
% generate some camera measurements
|
||||||
|
cam_pose = initial.at(i).compose(actual_transform);
|
||||||
|
% gtsam.plotPose3(cam_pose);
|
||||||
|
cam = SimpleCamera(cam_pose,K);
|
||||||
|
i
|
||||||
|
% result
|
||||||
|
for j=1:nrPoints
|
||||||
|
% All landmarks seen in every frame
|
||||||
|
try
|
||||||
|
z = cam.project(landmarks{j});
|
||||||
|
fg.add(TransformCalProjectionFactorCal3_S2(z, z_cov, i, transformKey, 100+j, calibrationKey));
|
||||||
|
catch
|
||||||
|
cheirality_exception_count = cheirality_exception_count + 1;
|
||||||
|
end % end try/catch
|
||||||
|
end
|
||||||
|
|
||||||
|
if i > 1
|
||||||
|
disp('ISAM Update');
|
||||||
|
isam.update(fg, initial);
|
||||||
|
result = isam.calculateEstimate();
|
||||||
|
|
||||||
|
%% reset
|
||||||
|
initial = Values;
|
||||||
|
fg = NonlinearFactorGraph;
|
||||||
|
|
||||||
|
currentVelocityGlobal = isam.calculateEstimate(currentVelKey);
|
||||||
|
currentBias = isam.calculateEstimate(currentBiasKey);
|
||||||
|
end
|
||||||
|
|
||||||
|
hold off;
|
||||||
|
|
||||||
|
clf;
|
||||||
|
subplot(3,1,1:2);
|
||||||
|
hold on;
|
||||||
|
|
||||||
|
%% plot the integrated IMU frame (not from
|
||||||
|
gtsam.plotPose3(currentIMUPoseGlobal, [], 2);
|
||||||
|
|
||||||
|
%% plot results
|
||||||
|
result_camera_transform = result.at(transformKey);
|
||||||
|
for j=1:i
|
||||||
|
gtsam.plotPose3(result.at(j),[],0.5);
|
||||||
|
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
|
||||||
|
end
|
||||||
|
|
||||||
|
xlabel('x (m)');
|
||||||
|
ylabel('y (m)');
|
||||||
|
|
||||||
|
title(sprintf('Curvature %g deg, iteration %g', curvature, i));
|
||||||
|
|
||||||
|
axis([0 20 0 20 -10 10]);
|
||||||
|
view(-37,40);
|
||||||
|
% axis equal
|
||||||
|
|
||||||
|
for l=101:100+nrPoints
|
||||||
|
plotPoint3(result.at(l),'g');
|
||||||
|
end
|
||||||
|
|
||||||
|
ty = result.at(transformKey).translation().y();
|
||||||
|
fx = result.at(calibrationKey).fx();
|
||||||
|
fy = result.at(calibrationKey).fy();
|
||||||
|
text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty));
|
||||||
|
text(1,5,3,sprintf('fx(900): %.0f',fx));
|
||||||
|
text(1,5,1,sprintf('fy(900): %.0f',fy));
|
||||||
|
|
||||||
|
fxs(i) = fx;
|
||||||
|
fys(i) = fy;
|
||||||
|
subplot(3,1,3);
|
||||||
|
hold on;
|
||||||
|
plot(1:20,repmat(K.fx,1,20),'b--');
|
||||||
|
plot(1:i,fxs,'b');
|
||||||
|
|
||||||
|
plot(1:20,repmat(K.fy,1,20),'g--');
|
||||||
|
plot(1:i,fys,'g');
|
||||||
|
|
||||||
|
legend('f_x', 'f_x''', 'f_y', 'f_y''', 'Location', 'SouthWest');
|
||||||
|
|
||||||
|
|
||||||
|
if(write_video)
|
||||||
|
currFrame = getframe(gcf);
|
||||||
|
writeVideo(videoObj, currFrame)
|
||||||
|
else
|
||||||
|
pause(0.1);
|
||||||
|
end
|
||||||
|
|
||||||
|
|
||||||
|
end
|
||||||
|
|
||||||
|
if(write_video)
|
||||||
|
close(videoObj);
|
||||||
|
end
|
||||||
|
|
||||||
|
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
|
||||||
|
|
||||||
|
disp('Transform after optimization');
|
||||||
|
result.at(transformKey)
|
||||||
|
|
||||||
|
disp('Calibration after optimization');
|
||||||
|
result.at(calibrationKey)
|
||||||
|
|
||||||
|
disp('Bias after optimization');
|
||||||
|
currentBias
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue