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

Fixed error with IMU+Camera

release/4.3a0
djensen3 2014-05-15 15:57:22 -04:00
parent e5d4fe3aaf
commit 950e95b015
4 changed files with 42 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -20,22 +20,22 @@ if ~exist('externallyConfigured', 'var')
%% Configuration
% General options
options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj
options.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses
options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses
options.includeIMUFactors = 0; % if true, IMU factors will be added between consecutive states (biases, poses, velocities)
options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities)
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 = 1; % if true, SmartProjectionPose3Factors will be used with randomly generated landmarks
options.numberOfLandmarks = 1000; % Total number of visual landmarks (randomly generated in a box around the trajectory)
options.numberOfLandmarks = 1000; % Total number of visual landmarks (randomly generated in a box around the trajectory)
options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses
options.gpsStartPose = 100; % Pose number to start including GPS factors at
options.trajectoryLength = 20;%209; % length of the ground truth trajectory
options.trajectoryLength = 100;%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)
numMonteCarloRuns = 1; % number of Monte Carlo runs to perform
numMonteCarloRuns = 2; % number of Monte Carlo runs to perform
% Noise values to be adjusted
sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2
@ -93,7 +93,7 @@ metadata.camera.xlims = [-100, 650]; % x limits on area for landmark creation
metadata.camera.ylims = [-100, 700]; % y limits on area for landmark creation
metadata.camera.zlims = [-30, 30]; % z limits on area for landmark creation
metadata.camera.visualRange = 100; % maximum distance from the camera that a landmark can be seen (meters)
metadata.camera.bodyPoseCamera = Pose3.Expmap([-pi/2;0;0;0;0;0]); % pose of camera in body
metadata.camera.bodyPoseCamera = Pose3; % pose of camera in body
metadata.camera.CameraSigma = sigma_camera;
cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2, metadata.camera.CameraSigma);
noiseVectorCamera = metadata.camera.CameraSigma .* ones(2,1);
@ -103,7 +103,7 @@ if options.includeCameraFactors == 1
for i = 1:options.numberOfLandmarks
metadata.camera.gtLandmarkPoints(i) = Point3( ...
[rand() * (metadata.camera.xlims(2)-metadata.camera.xlims(1)) + metadata.camera.xlims(1); ...
rand() * (metadata.camera.ylims(2)-metadata.camera.ylims(1)) + metadata.camera.ylims(1);
rand() * (metadata.camera.ylims(2)-metadata.camera.ylims(1)) + metadata.camera.ylims(1); ...
rand() * (metadata.camera.zlims(2)-metadata.camera.zlims(1)) + metadata.camera.zlims(1)]);
end
end
@ -147,26 +147,31 @@ metadata.imu.gyroConstantBiasVector = zeros(3,1);
figure(1)
hold on;
b = [-1000 2000 -2000 2000 -30 30];
for i = 1:size(metadata.camera.gtLandmarkPoints,2)
p = metadata.camera.gtLandmarkPoints(i).vector;
if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6))
plot3(p(1), p(2), p(3), 'k+');
end
end
pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0));
for i = 1:length(pointsToPlot)
p = pointsToPlot(i).vector;
plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10);
if options.includeCameraFactors
b = [-1000 2000 -2000 2000 -30 30];
for i = 1:size(metadata.camera.gtLandmarkPoints,2)
p = metadata.camera.gtLandmarkPoints(i).vector;
if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6))
plot3(p(1), p(2), p(3), 'k+');
end
end
pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0));
for i = 1:length(pointsToPlot)
p = pointsToPlot(i).vector;
plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10);
end
end
plot3DPoints(gtValues);
%plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
plot3DTrajectory(gtValues, '-r');
axis equal
% optimize
optimizer = GaussNewtonOptimizer(gtGraph, gtValues);
gtEstimate = optimizer.optimize();
plot3DTrajectory(gtEstimate, '-k');
% estimate should match gtValues if graph is correct.
fprintf('Error in ground truth graph at gtValues: %g \n', gtGraph.error(gtValues) );
fprintf('Error in ground truth graph at gtEstimate: %g \n', gtGraph.error(gtEstimate) );

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

@ -19,7 +19,7 @@ for i=0:length(measurements)
if i==0
%% first time step, add priors
% Pose prior (poses used for all factors)
noisyInitialPoseVector = Pose3.Logmap(currentPose) + measurementNoise.poseNoiseVector .* rand(6,1);
noisyInitialPoseVector = Pose3.Logmap(currentPose) + measurementNoise.poseNoiseVector .* randn(6,1);
initialPose = Pose3.Expmap(noisyInitialPoseVector);
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
@ -35,9 +35,10 @@ for i=0:length(measurements)
end
%% Create a SmartProjectionFactor for each landmark
projectionFactorSeenBy = [];
if options.includeCameraFactors == 1
for j=1:options.numberOfLandmarks
SmartProjectionFactors(j) = SmartProjectionPose3Factor();
SmartProjectionFactors(j) = SmartProjectionPose3Factor(0.01);
% Use constructor with default values, but express the pose of the
% camera as a 90 degree rotation about the X axis
% SmartProjectionFactors(j) = SmartProjectionPose3Factor( ...
@ -46,7 +47,6 @@ for i=0:length(measurements)
% false, ... % manageDegeneracy
% false, ... % enableEPI
% metadata.camera.bodyPoseCamera); % Pose of camera in body frame
end
projectionFactorSeenBy = zeros(options.numberOfLandmarks,1);
end
@ -186,7 +186,7 @@ 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))]
%[find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))]
if options.includeCameraFactors == 1
for j = 1:options.numberOfLandmarks
if projectionFactorSeenBy(j) > 0

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

@ -9,7 +9,8 @@ import gtsam.*;
values = Values;
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data - currently using identity rotation')
warning('Rotating Pose inside getPoseFromGtScenario! TODO: Use a body_P_sensor transform in the factors')
if options.useRealData == 1
%% Create a ground truth trajectory from Real data (if available)
@ -29,8 +30,10 @@ if options.useRealData == 1
%% Generate the current pose
currentPoseKey = symbol('x', i);
currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd);
%% FOR TESTING
currentPose = currentPose.compose(metadata.camera.bodyPoseCamera);
%% FOR TESTING - this is currently moved to getPoseFromGtScenario
%currentPose = currentPose.compose(metadata.camera.bodyPoseCamera);
%currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0]));
% add to values
values.insert(currentPoseKey, currentPose);
@ -57,7 +60,7 @@ if options.useRealData == 1
scenarioIndIMU2 = previousScenarioInd+j;
IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1);
IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2);
IMURot2 = IMUPose2.rotation.matrix;
IMURot1 = IMUPose1.rotation.matrix;
IMUdeltaPose = IMUPose1.between(IMUPose2);
IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose);
@ -71,10 +74,10 @@ if options.useRealData == 1
% deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
% acc = (deltaPosition - initialVel * dT) * (2/dt^2)
measurements(i).imu(j).accel = IMURot2' * (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT));
measurements(i).imu(j).accel = IMURot1' * (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT));
% Update velocity
currentVel = currentVel + IMURot2 * measurements(i).imu(j).accel * deltaT;
currentVel = currentVel + IMURot1 * measurements(i).imu(j).accel * deltaT;
end
end
@ -94,8 +97,8 @@ if options.useRealData == 1
% Create the camera based on the current pose and the pose of the
% camera in the body
cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera);
%camera = SimpleCamera(cameraPose, metadata.camera.calibration);
camera = SimpleCamera(currentPose, metadata.camera.calibration);
camera = SimpleCamera(cameraPose, metadata.camera.calibration);
%camera = SimpleCamera(currentPose, metadata.camera.calibration);
% Record measurements if the landmark is within visual range
for j=1:length(metadata.camera.gtLandmarkPoints)

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

@ -30,7 +30,10 @@ gtPosition = Point3([xlt, ylt, zlt]');
% 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));
gtBodyRotation = Rot3.Ypr(-gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
currentPose = Pose3(gtBodyRotation, gtPosition);
%% Rotate the pose
currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0]));
end