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

replaced identity rotations with ground truth rotations

release/4.3a0
djensen3 2014-05-15 13:21:35 -04:00
parent d0905e65ce
commit e5d4fe3aaf
3 changed files with 20 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -19,7 +19,8 @@ for i=0:length(measurements)
if i==0
%% first time step, add priors
% Pose prior (poses used for all factors)
initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1));
noisyInitialPoseVector = Pose3.Logmap(currentPose) + measurementNoise.poseNoiseVector .* rand(6,1);
initialPose = Pose3.Expmap(noisyInitialPoseVector);
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
% IMU velocity and bias priors
@ -183,10 +184,12 @@ for i=0:length(measurements)
end % end of for over trajectory
%% Add Camera Factors to the graph
% Only factors for landmarks that have been viewed at least once are added
% to the graph
[find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))]
if options.includeCameraFactors == 1
for j = 1:options.numberOfLandmarks
if projectionFactorSeenBy(j) > 5
if projectionFactorSeenBy(j) > 0
graph.add(SmartProjectionFactors(j));
end
end

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

@ -30,7 +30,7 @@ if options.useRealData == 1
currentPoseKey = symbol('x', i);
currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd);
%% FOR TESTING
%currentPose = currentPose.compose(metadata.camera.bodyPoseCamera);
currentPose = currentPose.compose(metadata.camera.bodyPoseCamera);
% add to values
values.insert(currentPoseKey, currentPose);

17
matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m
View File

@ -18,8 +18,19 @@ dY = gtECEF(2) - initialPositionECEF(2);
dZ = gtECEF(3) - initialPositionECEF(3);
[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
gtPosition = [xlt, ylt, zlt]';
gtRotation = Rot3; % Rot3.Expmap(rand(3,1)); %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
currentPose = Pose3(gtRotation, Point3(gtPosition));
gtPosition = Point3([xlt, ylt, zlt]');
% use the gtsam.Rot3.Ypr constructor (yaw, pitch, roll) from the ground truth data
% yaw = measured positively to the right
% pitch = measured positively up
% roll = measured positively to right
% Assumes vehice X forward, Y right, Z down
%
% In the gtScenario data
% heading (yaw) = measured positively to the left from Y-axis
% pitch =
% roll =
% Coordinate frame is Y forward, X is right, Z is up
gtBodyRotation = Rot3.Ypr(-gtScenario.Heading(scenarioInd), -gtScenario.Pitch(scenarioInd), -gtScenario.Roll(scenarioInd));
currentPose = Pose3(gtBodyRotation, gtPosition);
end