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

87
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
@ -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
gtsam.plotPose3(lastPose, P, scale);
catch
% warning(['no Pose3 at ' lastIndex]);
end
end
if ~holdstate
hold off
end

33
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');
@ -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')