Simulation for concurrent IMU, camera, IMU-camera transform estimation during flight with known landmarks
cbeall3
parent
52a090d1c1
commit
5b9954ab11
|
|
@ -0,0 +1,295 @@
|
|||
clear all;
|
||||
clf;
|
||||
|
||||
import gtsam.*;
|
||||
|
||||
write_video = false;
|
||||
|
||||
use_camera = true;
|
||||
use_camera_transform_noise = true;
|
||||
gps_noise = 0.5;
|
||||
|
||||
camera_transform_noise = Pose3(Rot3.RzRyRx(0.1,0.1,0.1),Point3(0,0.02,0));
|
||||
|
||||
if(write_video)
|
||||
videoObj = VideoWriter('FlightCameraIMU_transform_GPS.avi');
|
||||
videoObj.Quality = 100;
|
||||
videoObj.FrameRate = 10;
|
||||
open(videoObj);
|
||||
end
|
||||
|
||||
IMU_metadata.AccelerometerSigma = 1e-2;
|
||||
IMU_metadata.GyroscopeSigma = 1e-2;
|
||||
IMU_metadata.AccelerometerBiasSigma = 1e-6;
|
||||
IMU_metadata.GyroscopeBiasSigma = 1e-6;
|
||||
IMU_metadata.IntegrationSigma = 1e-1;
|
||||
|
||||
transformKey = 1000;
|
||||
calibrationKey = 2000;
|
||||
|
||||
fg = NonlinearFactorGraph;
|
||||
initial = Values;
|
||||
|
||||
%% some noise models
|
||||
trans_cov = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 20; 20; 0.1]);
|
||||
GPS_trans_cov = noiseModel.Diagonal.Sigmas([3; 3; 4]);
|
||||
K_cov = noiseModel.Diagonal.Sigmas([20; 20; 0.001; 20; 20]);
|
||||
|
||||
l_cov = noiseModel.Diagonal.Sigmas([1e-2; 1e-2; 1e-2]);
|
||||
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
|
||||
|
||||
%% calibration initialization
|
||||
K = Cal3_S2(20,1280,960);
|
||||
|
||||
% initialize K incorrectly
|
||||
K_corrupt = Cal3_S2(K.fx()+10,K.fy()+10,0,K.px(),K.py());
|
||||
|
||||
isamParams = gtsam.ISAM2Params;
|
||||
isamParams.setFactorization('QR');
|
||||
isam = ISAM2(isamParams);
|
||||
|
||||
%% Get initial conditions for the estimated trajectory
|
||||
currentVelocityGlobal = LieVector([10;0;0]); % (This is slightly wrong!)
|
||||
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];
|
||||
|
||||
|
||||
%% generate trajectory and landmarks
|
||||
trajectory = flight_trajectory();
|
||||
landmarks = ground_landmarks(100);
|
||||
|
||||
figure(1);
|
||||
% 3D map subplot
|
||||
a1 = subplot(2,2,1);
|
||||
grid on;
|
||||
|
||||
plot3DTrajectory(trajectory,'-b',true,5);
|
||||
plot3DPoints(landmarks,'*g');
|
||||
axis([-800 800 -800 800 0 1600]);
|
||||
axis equal;
|
||||
hold on;
|
||||
view(-37,40);
|
||||
|
||||
% camera subplot
|
||||
a2 = subplot(2,2,2);
|
||||
if ~use_camera
|
||||
title('Camera Off');
|
||||
end
|
||||
|
||||
% IMU-cam transform subplot
|
||||
a3 = subplot(2,2,3);
|
||||
view(-37,40);
|
||||
axis([-1 1 -1 1 -1 1]);
|
||||
grid on;
|
||||
xlabel('x');
|
||||
ylabel('y');
|
||||
zlabel('z');
|
||||
title('Estimated vs. actual IMU-cam transform');
|
||||
axis equal;
|
||||
|
||||
for i=1:size(trajectory)-1
|
||||
xKey = symbol('x',i);
|
||||
pose = trajectory.at(xKey);
|
||||
pose_t = pose.translation();
|
||||
|
||||
if exist('h_cursor','var')
|
||||
delete(h_cursor);
|
||||
end
|
||||
|
||||
h_cursor = plot3(a1, pose_t.x,pose_t.y,pose_t.z,'*');
|
||||
|
||||
camera_transform = Pose3(Rot3.RzRyRx(-pi, 0, -pi/2),Point3());
|
||||
camera_pose = pose.compose(camera_transform);
|
||||
|
||||
axes(a2);
|
||||
if use_camera
|
||||
% project (and plot inside)
|
||||
measurements = project_landmarks(camera_pose,landmarks, K);
|
||||
plot_projected_landmarks(a1, landmarks, measurements);
|
||||
else
|
||||
measurements = Values;
|
||||
end
|
||||
|
||||
%% ISAM stuff
|
||||
currentVelKey = symbol('v',i);
|
||||
currentBiasKey = symbol('b',i);
|
||||
|
||||
initial.insert(currentVelKey, currentVelocityGlobal);
|
||||
initial.insert(currentBiasKey, currentBias);
|
||||
|
||||
% prior on translation, sort of like GPS with noise!
|
||||
gps_pose = pose.retract([0; 0; 0; normrnd(0,gps_noise,3,1)]);
|
||||
fg.add(PoseTranslationPrior3D(xKey, gps_pose, GPS_trans_cov));
|
||||
|
||||
if i==1
|
||||
% camera transform
|
||||
if use_camera_transform_noise
|
||||
camera_transform_init = camera_transform.compose(camera_transform_noise);
|
||||
else
|
||||
camera_transform_init = camera_transform;
|
||||
end
|
||||
initial.insert(transformKey,camera_transform_init);
|
||||
fg.add(PriorFactorPose3(transformKey,camera_transform_init,trans_cov));
|
||||
|
||||
% calibration
|
||||
initial.insert(2000, K_corrupt);
|
||||
fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov));
|
||||
|
||||
initial.insert(xKey, pose);
|
||||
|
||||
result = initial;
|
||||
end
|
||||
|
||||
% priors on first two poses
|
||||
if i < 3
|
||||
% fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
|
||||
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
|
||||
|
||||
|
||||
end
|
||||
|
||||
%% the 'normal' case
|
||||
if i > 1
|
||||
|
||||
xKey_prev = symbol('x',i-1);
|
||||
pose_prev = trajectory.at(xKey_prev);
|
||||
|
||||
step = pose_prev.between(pose);
|
||||
|
||||
% insert estimate for current pose with some normal noise on
|
||||
% translation
|
||||
initial.insert(xKey,result.at(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)])));
|
||||
|
||||
% visual measurements
|
||||
if measurements.size > 0 && use_camera
|
||||
measurementKeys = KeyVector(measurements.keys);
|
||||
|
||||
for zz = 0:measurementKeys.size-1
|
||||
zKey = measurementKeys.at(zz);
|
||||
lKey = symbol('l',symbolIndex(zKey));
|
||||
|
||||
fg.add(TransformCalProjectionFactorCal3_S2(measurements.at(zKey), ...
|
||||
z_cov, xKey, transformKey, lKey, calibrationKey, false, true));
|
||||
|
||||
% only add landmark to values if doesn't exist yet
|
||||
if ~result.exists(lKey)
|
||||
initial.insert(lKey, landmarks.at(lKey));
|
||||
|
||||
% and add a prior since its position is known
|
||||
fg.add(PriorFactorPoint3(lKey, landmarks.at(lKey),l_cov));
|
||||
end
|
||||
end
|
||||
end % end landmark observations
|
||||
|
||||
|
||||
%% IMU
|
||||
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( ...
|
||||
xKey_prev, currentVelKey-1, ...
|
||||
xKey, 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(10) * sigma_between_b)));
|
||||
|
||||
% ISAM update
|
||||
isam.update(fg, initial);
|
||||
result = isam.calculateEstimate();
|
||||
|
||||
%% reset
|
||||
initial = Values;
|
||||
fg = NonlinearFactorGraph;
|
||||
|
||||
currentVelocityGlobal = result.at(currentVelKey);
|
||||
currentBias = result.at(currentBiasKey);
|
||||
|
||||
%% plot current pose result
|
||||
isam_pose = result.at(xKey);
|
||||
pose_t = isam_pose.translation();
|
||||
|
||||
if exist('h_result','var')
|
||||
delete(h_result);
|
||||
end
|
||||
|
||||
h_result = plot3(a1, pose_t.x,pose_t.y,pose_t.z,'^b', 'MarkerSize', 10);
|
||||
title(a1, sprintf('Step %d', i));
|
||||
|
||||
if exist('h_text1(1)', 'var')
|
||||
delete(h_text1(1));
|
||||
% delete(h_text2(1));
|
||||
end
|
||||
ty = result.at(transformKey).translation().y();
|
||||
K_estimate = result.at(calibrationKey);
|
||||
K_errors = K.localCoordinates(K_estimate);
|
||||
|
||||
camera_transform_estimate = result.at(transformKey);
|
||||
|
||||
fx = result.at(calibrationKey).fx();
|
||||
fy = result.at(calibrationKey).fy();
|
||||
% h_text1 = text(-600,0,0,sprintf('Y-Transform(0.0): %0.2f',ty));
|
||||
text(0,1100,0,sprintf('Calibration and IMU-cam transform errors:'));
|
||||
|
||||
entries = [{' f_x', ' f_y', ' s', 'p_x', 'p_y'}; num2cell(K_errors')];
|
||||
h_text1 = text(0,1550,0,sprintf('%s = %0.1f\n', entries{:}));
|
||||
|
||||
camera_transform_errors = camera_transform.localCoordinates(camera_transform_estimate);
|
||||
entries1 = [{'ax', 'ay', 'az', 'tx', 'ty', 'tz'}; num2cell(camera_transform_errors')];
|
||||
h_text2 = text(600,1500,0,sprintf('%s = %0.2f\n', entries1{:}));
|
||||
|
||||
% marginal is really huge
|
||||
% marginal_camera_transform = isam.marginalCovariance(transformKey);
|
||||
% plot transform
|
||||
axes(a3);
|
||||
cla;
|
||||
|
||||
plotPose3(camera_transform,[],1);
|
||||
plotPose3(camera_transform_estimate,[],0.5);
|
||||
|
||||
end
|
||||
|
||||
drawnow;
|
||||
if(write_video)
|
||||
currFrame = getframe(gcf);
|
||||
writeVideo(videoObj, currFrame)
|
||||
else
|
||||
pause(0.00001);
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
% print out final camera transform
|
||||
result.at(transformKey);
|
||||
|
||||
|
||||
if(write_video)
|
||||
close(videoObj);
|
||||
end
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
function [ values ] = flight_trajectory( input_args )
|
||||
%UNTITLED2 Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
import gtsam.*;
|
||||
|
||||
values = Values;
|
||||
curvature = 2;
|
||||
|
||||
|
||||
forward = Pose3(Rot3(),Point3(10,0,0));
|
||||
left = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(10,0,0));
|
||||
right = Pose3(Rot3.RzRyRx(0.0,0.0,-curvature*pi/180),Point3(10,0,0));
|
||||
|
||||
pose = Pose3(Rot3.RzRyRx(0,0,0),Point3(0,0,1000));
|
||||
|
||||
plan(1).direction = right;
|
||||
plan(1).steps = 20;
|
||||
|
||||
plan(2).direction = forward;
|
||||
plan(2).steps = 5;
|
||||
|
||||
plan(3).direction = left;
|
||||
plan(3).steps = 100;
|
||||
|
||||
plan(4).direction = forward;
|
||||
plan(4).steps = 50;
|
||||
|
||||
plan(5).direction = left;
|
||||
plan(5).steps = 80;
|
||||
|
||||
plan(6).direction = forward;
|
||||
plan(6).steps = 50;
|
||||
|
||||
plan(7).direction = right;
|
||||
plan(7).steps = 100;
|
||||
|
||||
plan_steps = numel(plan);
|
||||
|
||||
values_i = 0;
|
||||
|
||||
for i=1:plan_steps
|
||||
direction = plan(i).direction;
|
||||
segment_steps = plan(i).steps;
|
||||
|
||||
for j=1:segment_steps
|
||||
pose = pose.compose(direction);
|
||||
values.insert(symbol('x',values_i), pose);
|
||||
values_i = values_i + 1;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
function [ values ] = ground_landmarks( nrPoints )
|
||||
%UNTITLED2 Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
import gtsam.*;
|
||||
|
||||
values = Values;
|
||||
|
||||
x = -800+1600.*rand(nrPoints,1);
|
||||
y = -800+1600.*rand(nrPoints,1);
|
||||
z = 3 * rand(nrPoints,1);
|
||||
|
||||
for i=1:nrPoints
|
||||
values.insert(symbol('l',i),gtsam.Point3(x(i),y(i),z(i)));
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
function [] = plot_projected_landmarks( a, landmarks, measurements )
|
||||
%UNTITLED4 Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
persistent h;
|
||||
|
||||
if ishghandle(h)
|
||||
delete(h);
|
||||
end
|
||||
|
||||
measurement_keys = gtsam.KeyVector(measurements.keys);
|
||||
nrMeasurements = measurement_keys.size;
|
||||
|
||||
if nrMeasurements == 0
|
||||
return;
|
||||
end
|
||||
|
||||
x = zeros(1,nrMeasurements);
|
||||
y = zeros(1,nrMeasurements);
|
||||
z = zeros(1,nrMeasurements);
|
||||
|
||||
% Plot points and covariance matrices
|
||||
for i = 0:measurement_keys.size-1
|
||||
key = measurement_keys.at(i);
|
||||
key_index = gtsam.symbolIndex(key);
|
||||
p = landmarks.at(gtsam.symbol('l',key_index));
|
||||
|
||||
x(i+1) = p.x;
|
||||
y(i+1) = p.y;
|
||||
z(i+1) = p.z;
|
||||
|
||||
end
|
||||
|
||||
h = plot3(a, x,y,z,'rd', 'LineWidth',3);
|
||||
|
||||
end
|
||||
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
function [ measurements ] = project_landmarks( pose, landmarks, K )
|
||||
%UNTITLED3 Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
import gtsam.*;
|
||||
|
||||
camera = SimpleCamera(pose,K);
|
||||
measurements = Values;
|
||||
|
||||
for i=1:size(landmarks)-1
|
||||
z = camera.project(landmarks.at(symbol('l',i)));
|
||||
|
||||
% check bounding box
|
||||
if z.x < 0 || z.x > 1280
|
||||
continue
|
||||
elseif z.y < 0 || z.y > 960
|
||||
continue
|
||||
end
|
||||
|
||||
measurements.insert(symbol('z',i),z);
|
||||
end
|
||||
|
||||
% persistent h;
|
||||
%
|
||||
% if isempty(h)
|
||||
% h = figure();
|
||||
% else
|
||||
% figure(h);
|
||||
% end
|
||||
% clf;
|
||||
|
||||
if measurements.size == 0
|
||||
return
|
||||
end
|
||||
|
||||
cla;
|
||||
plot2DPoints(measurements,'*g');
|
||||
|
||||
axis equal;
|
||||
axis([0 1280 0 960]);
|
||||
|
||||
set(gca, 'YDir', 'reverse');
|
||||
xlabel('u (pixels)');
|
||||
ylabel('v (pixels)');
|
||||
set(gca, 'XAxisLocation', 'top');
|
||||
|
||||
|
||||
end
|
||||
|
||||
Loading…
Reference in New Issue