From 646f56413f0ec5f79000d46b3a567de7b30236be Mon Sep 17 00:00:00 2001 From: AltNav NUC Date: Mon, 14 May 2018 17:40:48 -0700 Subject: [PATCH] Update calling convention for pre-integrated IMU measurements in IMUKittiExampleGps, fix scope problem in plot3DTrajectory and make sure that all accessed values are of type Pose3 --- matlab/+gtsam/plot3DTrajectory.m | 87 +++++++++++----------- matlab/gtsam_examples/IMUKittiExampleGPS.m | 33 +++++--- 2 files changed, 65 insertions(+), 55 deletions(-) mode change 100644 => 100755 matlab/+gtsam/plot3DTrajectory.m mode change 100644 => 100755 matlab/gtsam_examples/IMUKittiExampleGPS.m diff --git a/matlab/+gtsam/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m old mode 100644 new mode 100755 index 2900aed99..ad8d71c4c --- a/matlab/+gtsam/plot3DTrajectory.m +++ b/matlab/+gtsam/plot3DTrajectory.m @@ -7,8 +7,10 @@ if ~exist('frames','var'), scale=[]; end import gtsam.* +Pose3Values = gtsam.utilities.allPose3s(values); + haveMarginals = exist('marginals', 'var'); -keys = KeyVector(values.keys); +keys = KeyVector(Pose3Values.keys); holdstate = ishold; hold on @@ -16,49 +18,48 @@ hold on % Plot poses and covariance matrices lastIndex = []; for i = 0:keys.size-1 - key = keys.at(i); - try - x = values.atPose3(key); - if ~isempty(lastIndex) - % Draw line from last pose then covariance ellipse on top of - % last pose. - lastKey = keys.at(lastIndex); - try - lastPose = values.atPose3(lastKey); - plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - else - P = []; - end - gtsam.plotPose3(lastPose, P, scale); - catch err - % warning(['no Pose3 at ' lastKey]); - end - end - lastIndex = i; - catch - % warning(['no Pose3 at ' key]); - end - - % Draw final pose + key = keys.at(i); + try + x = Pose3Values.atPose3(key); if ~isempty(lastIndex) - lastKey = keys.at(lastIndex); - try - lastPose = values.atPose3(lastKey); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - else - P = []; - end - gtsam.plotPose3(lastPose, P, scale); - catch - % warning(['no Pose3 at ' lastIndex]); + % Draw line from last pose then covariance ellipse on top of + % last pose. + lastKey = keys.at(lastIndex); + try + lastPose = Pose3Values.atPose3(lastKey); + plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; end + gtsam.plotPose3(lastPose, P, scale); + catch err + % warning(['no Pose3 at ' lastKey]); + end end - - if ~holdstate - hold off + lastIndex = i; + catch + warning(['no Pose3 at ' key]); + end +end + +% Draw final pose +if ~isempty(lastIndex) + lastKey = keys.at(lastIndex); + try + lastPose = Pose3Values.atPose3(lastKey); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; end - -end \ No newline at end of file + gtsam.plotPose3(lastPose, P, scale); + catch + % warning(['no Pose3 at ' lastIndex]); + end +end + +if ~holdstate + hold off +end diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m old mode 100644 new mode 100755 index 32f61e47f..c80b6ec3e --- a/matlab/gtsam_examples/IMUKittiExampleGPS.m +++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m @@ -42,6 +42,13 @@ sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadat g = [0;0;-9.8]; w_coriolis = [0;0;0]; +IMU_params = PreintegrationParams(g); + +IMU_params.setAccelerometerCovariance(IMU_metadata.AccelerometerSigma.^2 * eye(3)); +IMU_params.setGyroscopeCovariance(IMU_metadata.GyroscopeSigma.^2 * eye(3)); +IMU_params.setIntegrationCovariance(IMU_metadata.IntegrationSigma.^2 * eye(3)); +IMU_params.setOmegaCoriolis(w_coriolis); + %% Solver object isamParams = ISAM2Params; isamParams.setFactorization('CHOLESKY'); @@ -65,7 +72,7 @@ for measurementIndex = firstGPSPose:length(GPS_data) currentVelKey = symbol('v',measurementIndex); currentBiasKey = symbol('b',measurementIndex); t = GPS_data(measurementIndex, 1).Time; - + if measurementIndex == firstGPSPose %% Create initial estimate and prior on initial pose, velocity, and biases newValues.insert(currentPoseKey, currentPoseGlobal); @@ -78,10 +85,8 @@ for measurementIndex = firstGPSPose:length(GPS_data) t_previous = GPS_data(measurementIndex-1, 1).Time; %% Summarize IMU data between the previous GPS measurement and now IMUindices = find(IMUtimes >= t_previous & IMUtimes <= t); - - currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... - currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ... - IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3)); + + currentSummarizedMeasurement = PreintegratedImuMeasurements(IMU_params,currentBias); for imuIndex = IMUindices accMeas = [ IMU_data(imuIndex).accelX; IMU_data(imuIndex).accelY; IMU_data(imuIndex).accelZ ]; @@ -94,8 +99,8 @@ for measurementIndex = firstGPSPose:length(GPS_data) newFactors.add(ImuFactor( ... currentPoseKey-1, currentVelKey-1, ... currentPoseKey, currentVelKey, ... - currentBiasKey, currentSummarizedMeasurement, g, w_coriolis)); - + currentBiasKey, currentSummarizedMeasurement)); + % Bias evolution as given in the IMU metadata newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... noiseModel.Diagonal.Sigmas(sqrt(numel(IMUindices)) * sigma_between_b))); @@ -119,10 +124,13 @@ for measurementIndex = firstGPSPose:length(GPS_data) isam.update(newFactors, newValues); newFactors = NonlinearFactorGraph; newValues = Values; + + result = isam.calculateEstimate(); if rem(measurementIndex,10)==0 % plot every 10 time steps cla; - plot3DTrajectory(isam.calculateEstimate, 'g-'); + + plot3DTrajectory(result, 'g-'); title('Estimated trajectory using ISAM2 (IMU+GPS)') xlabel('[m]') ylabel('[m]') @@ -131,12 +139,13 @@ for measurementIndex = firstGPSPose:length(GPS_data) drawnow; end % ======================================================================= - currentPoseGlobal = isam.calculateEstimate(currentPoseKey); - currentVelocityGlobal = isam.calculateEstimate(currentVelKey); - currentBias = isam.calculateEstimate(currentBiasKey); + + currentPoseGlobal = result.atPose3(currentPoseKey); + currentVelocityGlobal = result.atVector(currentVelKey); + currentBias = result.atConstantBias(currentBiasKey); end end - + end % end main loop disp('-- Reached end of sensor data')