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

working on GPS factors

release/4.3a0
djensen3 2014-04-23 12:39:47 -04:00
parent 13d47fcee4
commit 2ab81ae997
3 changed files with 17 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,9 +21,11 @@ options.includeIMUFactors = 1; % if true, IMU factors will be added between
options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1)
options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements
options.includeCameraFactors = 0; % not fully implemented yet
options.includeCameraFactors = 0; % not fully implementemeasurements, values, noiseModels, measurementNoise, options, metadatad yet
numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors
options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses
options.trajectoryLength = 209; % length of the ground truth trajectory
options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories)

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

@ -145,6 +145,13 @@ for i=0:length(measurements)
%fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped);
end % end of Camera factor creation
%% Add GPS factors
if options.includeGPSFactors == 1
currentGPSKey = symbol('g', i);
graph.add(GPSPriorFactor(currentPoseKey, measurements(i).gpsPosition, ...
noiseModel.Isotropic.Sigma(3, 1e-4)));
end % end of GPS factor creation
end % end of else (i=0)
end % end of for over trajectory

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

@ -24,7 +24,7 @@ if options.useRealData == 1
for i=0:options.trajectoryLength
scenarioInd = options.subsampleStep * i + 1;
fprintf('%d, ', scenarioInd);
if (mod(i,20) == 0) fprintf('\n'); end
if (mod(i,12) == 0) fprintf('\n'); end
%% Generate the current pose
currentPoseKey = symbol('x', i);
@ -80,6 +80,12 @@ if options.useRealData == 1
values.insert(currentBiasKey, metadata.imu.zeroBias);
end
%% gt GPS measurements
if options.includeGPSFactors == 1 && i > 0
gpsPosition = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation.vector;
measurements(i).gpsPosition = Point3(gpsPosition);
end
end
fprintf('\n');
else