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

fixed use of 2nd order integration in matlab wrapper

release/4.3a0
Luca 2014-04-17 16:23:29 -04:00
parent a0a955e5a5
commit 26c296603f
3 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam.h
View File

@ -2286,6 +2286,7 @@ virtual class ConstantBias : gtsam::Value {
#include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/ImuFactor.h>
class ImuFactorPreintegratedMeasurements { class ImuFactorPreintegratedMeasurements {
// Standard Constructor // Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);

4
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -59,12 +59,14 @@ for i=0:length(measurements)
currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false
if options.imuFactorType == 1 if options.imuFactorType == 1
use2ndOrderIntegration = true;
% Initialize preintegration % Initialize preintegration
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
metadata.imu.zeroBias, ... metadata.imu.zeroBias, ...
metadata.imu.AccelerometerSigma.^2 * eye(3), ... metadata.imu.AccelerometerSigma.^2 * eye(3), ...
metadata.imu.GyroscopeSigma.^2 * eye(3), ... metadata.imu.GyroscopeSigma.^2 * eye(3), ...
metadata.imu.IntegrationSigma.^2 * eye(3)); metadata.imu.IntegrationSigma.^2 * eye(3), ...
use2ndOrderIntegration);
% Generate IMU measurements with noise % Generate IMU measurements with noise
for j=1:length(measurements(i).imu) % all measurements to preintegrate for j=1:length(measurements(i).imu) % all measurements to preintegrate
imuAccel = measurements(i).imu(j).accel ... imuAccel = measurements(i).imu(j).accel ...

2
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
View File

@ -9,7 +9,7 @@ import gtsam.*;
values = Values; values = Values;
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data - using identity rotation') warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data - currently using identity rotation')
if options.useRealData == 1 if options.useRealData == 1
%% Create a ground truth trajectory from Real data (if available) %% Create a ground truth trajectory from Real data (if available)