diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 9f6648b8e..2eddf75ee 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -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) ); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index b6f79bf02..00ae4b9c2 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index eb735a5d7..2cceea753 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -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) diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m index 070c0fb4c..42dc1db60 100644 --- a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m +++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m @@ -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 \ No newline at end of file