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

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

@ -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
@ -18,13 +20,13 @@ lastIndex = [];
for i = 0:keys.size-1
key = keys.at(i);
try
x = values.atPose3(key);
x = Pose3Values.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);
lastPose = Pose3Values.atPose3(lastKey);
plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
@ -38,14 +40,15 @@ for i = 0:keys.size-1
end
lastIndex = i;
catch
% warning(['no Pose3 at ' key]);
warning(['no Pose3 at ' key]);
end
end
% Draw final pose
if ~isempty(lastIndex)
% Draw final pose
if ~isempty(lastIndex)
lastKey = keys.at(lastIndex);
try
lastPose = values.atPose3(lastKey);
lastPose = Pose3Values.atPose3(lastKey);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
else
@ -55,10 +58,8 @@ for i = 0:keys.size-1
catch
% warning(['no Pose3 at ' lastIndex]);
end
end
if ~holdstate
hold off
end
end
if ~holdstate
hold off
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];
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');
@ -79,9 +86,7 @@ for measurementIndex = firstGPSPose:length(GPS_data)
%% 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,7 +99,7 @@ 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)), ...
@ -120,9 +125,12 @@ for measurementIndex = firstGPSPose:length(GPS_data)
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,9 +139,10 @@ 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