12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

release/4.3a0
AltNav NUC 2018-05-14 17:40:48 -07:00
parent 0d7997a6aa
commit 646f56413f
2 changed files with 65 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
matlab/+gtsam/plot3DTrajectory.m Normal file → Executable file
View File

@ -7,8 +7,10 @@ if ~exist('frames','var'), scale=[]; end
import gtsam.* import gtsam.*
Pose3Values = gtsam.utilities.allPose3s(values);
haveMarginals = exist('marginals', 'var'); haveMarginals = exist('marginals', 'var');
keys = KeyVector(values.keys); keys = KeyVector(Pose3Values.keys);
holdstate = ishold; holdstate = ishold;
hold on hold on
@ -18,13 +20,13 @@ lastIndex = [];
for i = 0:keys.size-1 for i = 0:keys.size-1
key = keys.at(i); key = keys.at(i);
try try
x = values.atPose3(key); x = Pose3Values.atPose3(key);
if ~isempty(lastIndex) if ~isempty(lastIndex)
% Draw line from last pose then covariance ellipse on top of % Draw line from last pose then covariance ellipse on top of
% last pose. % last pose.
lastKey = keys.at(lastIndex); lastKey = keys.at(lastIndex);
try try
lastPose = values.atPose3(lastKey); lastPose = Pose3Values.atPose3(lastKey);
plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
if haveMarginals if haveMarginals
P = marginals.marginalCovariance(lastKey); P = marginals.marginalCovariance(lastKey);
@ -38,14 +40,15 @@ for i = 0:keys.size-1
end end
lastIndex = i; lastIndex = i;
catch catch
% warning(['no Pose3 at ' key]); warning(['no Pose3 at ' key]);
end
end end
% Draw final pose % Draw final pose
if ~isempty(lastIndex) if ~isempty(lastIndex)
lastKey = keys.at(lastIndex); lastKey = keys.at(lastIndex);
try try
lastPose = values.atPose3(lastKey); lastPose = Pose3Values.atPose3(lastKey);
if haveMarginals if haveMarginals
P = marginals.marginalCovariance(lastKey); P = marginals.marginalCovariance(lastKey);
else else
@ -60,5 +63,3 @@ for i = 0:keys.size-1
if ~holdstate if ~holdstate
hold off hold off
end end
end

25
matlab/gtsam_examples/IMUKittiExampleGPS.m Normal file → Executable file
View File

@ -42,6 +42,13 @@ sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadat
g = [0;0;-9.8]; g = [0;0;-9.8];
w_coriolis = [0;0;0]; 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 %% Solver object
isamParams = ISAM2Params; isamParams = ISAM2Params;
isamParams.setFactorization('CHOLESKY'); isamParams.setFactorization('CHOLESKY');
@ -79,9 +86,7 @@ for measurementIndex = firstGPSPose:length(GPS_data)
%% Summarize IMU data between the previous GPS measurement and now %% Summarize IMU data between the previous GPS measurement and now
IMUindices = find(IMUtimes >= t_previous & IMUtimes <= t); IMUindices = find(IMUtimes >= t_previous & IMUtimes <= t);
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... currentSummarizedMeasurement = PreintegratedImuMeasurements(IMU_params,currentBias);
currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
for imuIndex = IMUindices for imuIndex = IMUindices
accMeas = [ IMU_data(imuIndex).accelX; IMU_data(imuIndex).accelY; IMU_data(imuIndex).accelZ ]; accMeas = [ IMU_data(imuIndex).accelX; IMU_data(imuIndex).accelY; IMU_data(imuIndex).accelZ ];
@ -94,7 +99,7 @@ for measurementIndex = firstGPSPose:length(GPS_data)
newFactors.add(ImuFactor( ... newFactors.add(ImuFactor( ...
currentPoseKey-1, currentVelKey-1, ... currentPoseKey-1, currentVelKey-1, ...
currentPoseKey, currentVelKey, ... currentPoseKey, currentVelKey, ...
currentBiasKey, currentSummarizedMeasurement, g, w_coriolis)); currentBiasKey, currentSummarizedMeasurement));
% Bias evolution as given in the IMU metadata % Bias evolution as given in the IMU metadata
newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
@ -120,9 +125,12 @@ for measurementIndex = firstGPSPose:length(GPS_data)
newFactors = NonlinearFactorGraph; newFactors = NonlinearFactorGraph;
newValues = Values; newValues = Values;
result = isam.calculateEstimate();
if rem(measurementIndex,10)==0 % plot every 10 time steps if rem(measurementIndex,10)==0 % plot every 10 time steps
cla; cla;
plot3DTrajectory(isam.calculateEstimate, 'g-');
plot3DTrajectory(result, 'g-');
title('Estimated trajectory using ISAM2 (IMU+GPS)') title('Estimated trajectory using ISAM2 (IMU+GPS)')
xlabel('[m]') xlabel('[m]')
ylabel('[m]') ylabel('[m]')
@ -131,9 +139,10 @@ for measurementIndex = firstGPSPose:length(GPS_data)
drawnow; drawnow;
end end
% ======================================================================= % =======================================================================
currentPoseGlobal = isam.calculateEstimate(currentPoseKey);
currentVelocityGlobal = isam.calculateEstimate(currentVelKey); currentPoseGlobal = result.atPose3(currentPoseKey);
currentBias = isam.calculateEstimate(currentBiasKey); currentVelocityGlobal = result.atVector(currentVelKey);
currentBias = result.atConstantBias(currentBiasKey);
end end
end end