From e8f3a7e459ead84e4eb37cd73d9a8d747b242f3e Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 2 Jul 2014 15:49:12 -0400 Subject: [PATCH] Concurrent calibration now also with IMU --- ...ansformCalProjectionFactorIMUExampleISAM.m | 279 ++++++++++++++++++ 1 file changed, 279 insertions(+) create mode 100644 matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m new file mode 100644 index 000000000..4f29080af --- /dev/null +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -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 + + +