Simulation for concurrent IMU, camera, IMU-camera transform estimation during flight with known landmarks

matlab/unstable_examples/FlightCameraTransformIMU.m

@@ -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

matlab/unstable_examples/flight_trajectory.m

@@ -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
+

matlab/unstable_examples/ground_landmarks.m

@@ -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
+

matlab/unstable_examples/plot_projected_landmarks.m

@@ -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
+

matlab/unstable_examples/project_landmarks.m

@@ -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
+