code reorganization

matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m

@@ -100,9 +100,6 @@ gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
 %gtGraph.print(sprintf('\nGround Truth Factor graph:\n'));
 %gtValues.print(sprintf('\nGround Truth Values:\n  '));
 
-warning('Additional prior on zerobias')
-warning('Additional PriorFactorLieVector on velocities')
-
 figure(1)
 hold on;
 plot3DPoints(gtValues);
@@ -150,9 +147,9 @@ for k=1:numMonteCarloRuns
   marginals = Marginals(graph, estimate);
   
   % for each pose in the trajectory
-  for i=1:size(gtMeasurements.deltaMatrix,1)+1
+  for i=0:options.trajectoryLength
     % compute estimation errors
-    currentPoseKey = symbol('x', i-1);
+    currentPoseKey = symbol('x', i);
     gtPosition  = gtValues.at(currentPoseKey).translation.vector;
     estPosition = estimate.at(currentPoseKey).translation.vector;
     estR = estimate.at(currentPoseKey).rotation.matrix;
@@ -163,7 +160,7 @@ for k=1:numMonteCarloRuns
     covPosition = estR * cov(4:6,4:6) * estR';
     
     % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances
-    NEES(k,i) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof
+    NEES(k,i+1) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof
   end
   
   figure(2)
@@ -215,7 +212,7 @@ title('NEES normalized by dof VS bounds');
 saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig');
 
 logFile = horzcat(folderName,'log-',testName);
-save(logFile)
+%save(logFile)
 
 %% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4)
 % the nees for a single experiment (i) is defined as

matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m

@@ -1,4 +1,4 @@
-function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata) 
+function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata)
 % Create a factor graph based on provided measurements, values, and noises.
 % Used for covariance analysis scripts.
 % 'options' contains fields for including various factor types.
@@ -10,14 +10,14 @@ import gtsam.*;
 
 graph = NonlinearFactorGraph;
 
-% Iterate through the trajectory 
+% Iterate through the trajectory
-for i=0:size(measurements.deltaMatrix, 1);
+for i=0:length(measurements)
   % Get the current pose
   currentPoseKey = symbol('x', i);
-  currentPose = values.at(currentPoseKey); 
+  currentPose = values.at(currentPoseKey);
   
   if i==0
-    %% first time step, add priors    
+    %% first time step, add priors
     % Pose prior (poses used for all factors)
     initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1));
     graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
@@ -25,11 +25,11 @@ for i=0:size(measurements.deltaMatrix, 1);
     % IMU velocity and bias priors
     if options.includeIMUFactors == 1
       currentVelKey = symbol('v', 0);
-      currentVel = values.at(currentVelKey).vector; 
+      currentVel = values.at(currentVelKey).vector;
       graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
       
       currentBiasKey = symbol('b', 0);
-      currentBias = values.at(currentBiasKey);  
+      currentBias = values.at(currentBiasKey);
       graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias));
     end
     
@@ -41,12 +41,12 @@ for i=0:size(measurements.deltaMatrix, 1);
     end
     
   else
-    prevPoseKey = symbol('x', i-1);
+    
-
     %% Add Between factors
     if options.includeBetweenFactors == 1
+      prevPoseKey = symbol('x', i-1);
       % Create the noisy pose estimate
-      deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)' ...
+      deltaPose = Pose3.Expmap(measurements(i).deltaVector ...
         + (measurementNoise.poseNoiseVector .* randn(6,1)));  % added noise
       % Add the between factor to the graph
       graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose));
@@ -57,52 +57,55 @@ for i=0:size(measurements.deltaMatrix, 1);
       % Update keys
       currentVelKey = symbol('v', i);  % not used if includeIMUFactors is false
       currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false
-      % Generate IMU measurements with noise
+      
-      imuAccel = measurements.imu.accel(i,:)' ...
+      if options.imuFactorType == 1
-          + (measurementNoise.imu.accelNoiseVector .* randn(3,1));  % added noise
-      imuGyro = measurements.imu.gyro(i,:)' ...
-          + (measurementNoise.imu.gyroNoiseVector .* randn(3,1));   % added noise
-
-      if options.imuFactorType == 2
-        % Initialize preintegration
-        imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
-          metadata.imu.zeroBias, ...
-          metadata.imu.AccelerometerSigma.^2 * eye(3), ...
-          metadata.imu.GyroscopeSigma.^2 * eye(3), ...
-          metadata.imu.IntegrationSigma.^2 * eye(3), ...
-          metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ...
-          metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ...
-          metadata.imu.BiasAccOmegaInit.^2 * eye(6));
-        % Preintegrate
-        imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
-        % Add Imu factor
-        graph.add(CombinedImuFactor( ...
-          currentPoseKey-1, currentVelKey-1, ...
-          currentPoseKey, currentVelKey, ...
-          currentBiasKey-1, currentBiasKey, ...
-          imuMeasurement, ...
-          metadata.imu.g, metadata.imu.omegaCoriolis, ...
-          noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias)));
-      else % IMU type 1 
         % Initialize preintegration
         imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
           metadata.imu.zeroBias, ...
           metadata.imu.AccelerometerSigma.^2 * eye(3), ...
           metadata.imu.GyroscopeSigma.^2 * eye(3), ...
           metadata.imu.IntegrationSigma.^2 * eye(3));
-        % Preintegrate
+        % Generate IMU measurements with noise
-        imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
+        for j=1:length(measurements(i).imu) % all measurements to preintegrate
-        % Add Imu factor
+          imuAccel = measurements(i).imu(j).accel ...
-        graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ...
+            + (measurementNoise.imu.accelNoiseVector .* randn(3,1));  % added noise
-          currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis));
+          imuGyro = measurements(i).imu(j).gyro ...
-        % Add between factor on biases
+            + (measurementNoise.imu.gyroNoiseVector .* randn(3,1));   % added noise
-        graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
+          
-          noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
+          % Preintegrate
-        % Additional prior on zerobias
+          imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT);
-        %graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ...
+          % Add Imu factor
-        %  noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); 
+          graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ...
+            currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis));
+          % Add between factor on biases
+          graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
+            noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
+        end
       end
-     
+      
+      
+      if options.imuFactorType == 2
+        %         % Initialize preintegration
+        %         imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
+        %           metadata.imu.zeroBias, ...
+        %           metadata.imu.AccelerometerSigma.^2 * eye(3), ...
+        %           metadata.imu.GyroscopeSigma.^2 * eye(3), ...
+        %           metadata.imu.IntegrationSigma.^2 * eye(3), ...
+        %           metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ...
+        %           metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ...
+        %           metadata.imu.BiasAccOmegaInit.^2 * eye(6));
+        %         % Preintegrate
+        %         imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
+        %         % Add Imu factor
+        %         graph.add(CombinedImuFactor( ...
+        %           currentPoseKey-1, currentVelKey-1, ...
+        %           currentPoseKey, currentVelKey, ...
+        %           currentBiasKey-1, currentBiasKey, ...
+        %           imuMeasurement, ...
+        %           metadata.imu.g, metadata.imu.omegaCoriolis, ...
+        %           noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias)));
+      end
+      
     end % end of IMU factor creation
     
     %% Add Camera factors - UNDER CONSTRUCTION !!!! -
@@ -115,7 +118,7 @@ for i=0:size(measurements.deltaMatrix, 1);
         landmarkKey = symbol('p', j);
         try
           Z = gtCamera.project(gtLandmarkPoints(j));
-          %% TO-DO probably want to do some type of filtering on the measurement values, because
+          % TO-DO probably want to do some type of filtering on the measurement values, because
           % they might not all be valid
           graph.add(GenericProjectionFactorCal3_S2(Z, cameraMeasurementNoise, currentPoseKey, landmarkKey, K));
         catch

matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m

@@ -9,8 +9,8 @@ import gtsam.*;
 
 values = Values;
 
-    warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
+warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
-    warning('using identity rotation')
+warning('using identity rotation')
 
 if options.useRealData == 1
   %% Create a ground truth trajectory from Real data (if available)
@@ -18,47 +18,69 @@ if options.useRealData == 1
   gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',...
     'VEast', 'VNorth', 'VUp');
   
-  Org_lat = gtScenario.Lat(1);
-  Org_lon = gtScenario.Lon(1);
-  initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]);
-  
   % Limit the trajectory length
-  options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength+1]);
+  options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength]);
   fprintf('Scenario Ind: ');
-  for i=1:options.trajectoryLength+1
+  
-    % Update the pose key
+  for i=0:options.trajectoryLength
-    currentPoseKey = symbol('x', i-1);
+    scenarioInd = options.subsampleStep * i + 1;
-    
-    % Generate the current pose
-    scenarioInd = options.subsampleStep * (i-1) + 1;
     fprintf('%d, ', scenarioInd);
-    if mod(i,20) == 0
+    if (mod(i,20) == 0) fprintf('\n'); end
-      fprintf('\n');
-    end
-    gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
-    % truth in ENU
-    dX = gtECEF(1) - initialPositionECEF(1);
-    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]';
+    %% Generate the current pose
-    gtRotation = Rot3; %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
+    currentPoseKey = symbol('x', i);
-    currentPose = Pose3(gtRotation, Point3(gtPosition));
+    currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd);
-    
+    % add to values
-    % Add values
     values.insert(currentPoseKey, currentPose);
     
-    % Generate the measurement. The first pose is considered the prior, so
+    %% gt Between measurements
-    % it has no measurement
+    if options.includeBetweenFactors == 1 && i > 0
-    if i > 1
       prevPose = values.at(currentPoseKey - 1);
       deltaPose = prevPose.between(currentPose);
-      measurements.deltaMatrix(i-1,:) = Pose3.Logmap(deltaPose);
+      measurements(i).deltaVector = Pose3.Logmap(deltaPose);
     end
+    
+    %% gt IMU measurements
+    if options.includeIMUFactors == 1
+      currentVelKey = symbol('v', i);
+      currentBiasKey = symbol('b', i);
+      deltaT = 0.01;   % amount of time between IMU measurements
+      if i == 0
+        currentVel = [0 0 0]';
+      else
+        % integrate & store intermediate measurements
+        for j=1:options.subsampleStep % we integrate all the intermediate measurements
+          scenarioIndIMU1 = scenarioInd+j-1;
+          scenarioIndIMU2 = scenarioInd+j;
+          IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1);
+          IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2);
+          IMUdeltaPose = IMUPose1.between(IMUPose2);
+          IMUdeltaPoseVector     = Pose3.Logmap(IMUdeltaPose);
+          IMUdeltaRotVector      = IMUdeltaPoseVector(1:3);
+          IMUdeltaPositionVector = IMUPose2.translation.vector - IMUPose1.translation.vector; % translation in absolute frame
+          
+          measurements(i).imu(j).deltaT = deltaT;
+          
+          % gyro rate: Logmap(R_i' * R_i+1) / deltaT
+          measurements(i).imu(j).gyro = IMUdeltaRotVector./deltaT;
+          
+          % acc = (deltaPosition - initialVel * dT) * (2/dt^2)
+          measurements(i).imu(j).accel = (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT));
+          
+          % Update velocity
+          currentVel = IMUdeltaPositionVector./deltaT;
+        end
+      end
+      
+      % Add Values: velocity and bias
+      values.insert(currentVelKey, LieVector(currentVel));
+      values.insert(currentBiasKey, metadata.imu.zeroBias);
+    end
+    
   end
   fprintf('\n');
 else
+  error('Please use RealData')
   %% Create a random trajectory as ground truth
   currentPose = Pose3; % initial pose  % initial pose
   
@@ -87,41 +109,5 @@ else
   end  % end of random trajectory creation
 end % end of else
 
-%% Create IMU measurements and Values for the trajectory
-if options.includeIMUFactors == 1
-  currentVel = [0 0 0];  % initial velocity (used to generate IMU measurements)
-  deltaT = 0.1;          % amount of time between IMU measurements
-  
-  % Iterate over the deltaMatrix to generate appropriate IMU measurements
-  for i = 0:size(measurements.deltaMatrix, 1)
-    % Update Keys
-    currentVelKey = symbol('v', i);
-    currentBiasKey = symbol('b', i);
-    
-    if i == 0
-      % Add initial values
-      currentVel = [0 0 0];
-      values.insert(currentVelKey, LieVector(currentVel'));
-      values.insert(currentBiasKey, metadata.imu.zeroBias);
-    else
-      measurements.imu.deltaT(i) = deltaT;
-      
-      % create accel and gyro measurements based on
-      measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i);
-      
-      % acc = (deltaPosition - initialVel * dT) * (2/dt^2)
-      measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ...
-        - currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i)));
-      
-      % Update velocity
-      currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i);
-      
-      % Add Values: velocity and bias
-      values.insert(currentVelKey, LieVector(currentVel'));
-      values.insert(currentBiasKey, metadata.imu.zeroBias);
-    end
-  end
-end % end of IMU measurements
-
 end % end of function
 

matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m

@@ -0,0 +1,25 @@
+function currentPose = getPoseFromGtScenario(gtScenario,scenarioInd)
+% gtScenario contains vectors (Lat, Lon, Alt, Roll, Pitch, Yaw)
+% The function picks the index 'scenarioInd' in those vectors and 
+% computes the corresponding pose by
+% 1) Converting (Lat,Lon,Alt) to local coordinates
+% 2) Converting (Roll,Pitch,Yaw) to a rotation matrix
+
+import gtsam.*;
+
+Org_lat = gtScenario.Lat(1);
+Org_lon = gtScenario.Lon(1);
+initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]);
+
+gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
+% truth in ENU
+dX = gtECEF(1) - initialPositionECEF(1);
+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.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
+currentPose = Pose3(gtRotation, Point3(gtPosition));
+
+end