Update calling convention for pre-integrated IMU measurements in IMUKittiExampleGps, fix scope problem in plot3DTrajectory and make sure that all accessed values are of type Pose3

matlab/+gtsam/plot3DTrajectory.m

@@ -7,8 +7,10 @@ if ~exist('frames','var'), scale=[]; end
 
 import gtsam.*
 
+Pose3Values = gtsam.utilities.allPose3s(values);
+
 haveMarginals = exist('marginals', 'var');
-keys = KeyVector(values.keys);
+keys = KeyVector(Pose3Values.keys);
 
 holdstate = ishold;
 hold on
@@ -16,49 +18,48 @@ hold on
 % Plot poses and covariance matrices
 lastIndex = [];
 for i = 0:keys.size-1
-    key = keys.at(i);
+  key = keys.at(i);
-    try
+  try
-        x = values.atPose3(key);
+    x = Pose3Values.atPose3(key);
-        if ~isempty(lastIndex)
-            % Draw line from last pose then covariance ellipse on top of
-            % last pose.
-            lastKey = keys.at(lastIndex);
-            try
-                lastPose = values.atPose3(lastKey);
-                plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
-                if haveMarginals
-                    P = marginals.marginalCovariance(lastKey);
-                else
-                    P = [];
-                end
-                gtsam.plotPose3(lastPose, P, scale);
-            catch err
-                % warning(['no Pose3 at ' lastKey]);
-            end
-        end
-        lastIndex = i;
-    catch
-        % warning(['no Pose3 at ' key]);
-    end
-    
-    % Draw final pose
     if ~isempty(lastIndex)
-        lastKey = keys.at(lastIndex);
+      % Draw line from last pose then covariance ellipse on top of
-        try
+      % last pose.
-            lastPose = values.atPose3(lastKey);
+      lastKey = keys.at(lastIndex);
-            if haveMarginals
+      try
-                P = marginals.marginalCovariance(lastKey);
+        lastPose = Pose3Values.atPose3(lastKey);
-            else
+        plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
-                P = [];
+        if haveMarginals
-            end
+          P = marginals.marginalCovariance(lastKey);
-            gtsam.plotPose3(lastPose, P, scale);
+        else
-        catch
+          P = [];
-            % warning(['no Pose3 at ' lastIndex]);
         end
+        gtsam.plotPose3(lastPose, P, scale);
+      catch err
+        %                 warning(['no Pose3 at ' lastKey]);
+      end
     end
-    
+    lastIndex = i;
-    if ~holdstate
+  catch
-        hold off
+    warning(['no Pose3 at ' key]);
-    end
+  end
-    
+end
+
+% Draw final pose
+if ~isempty(lastIndex)
+  lastKey = keys.at(lastIndex);
+  try
+    lastPose = Pose3Values.atPose3(lastKey);
+    if haveMarginals
+      P = marginals.marginalCovariance(lastKey);
+    else
+      P = [];
+    end
+    gtsam.plotPose3(lastPose, P, scale);
+  catch
+    % warning(['no Pose3 at ' lastIndex]);
+  end
+end
+
+if ~holdstate
+  hold off
 end

matlab/gtsam_examples/IMUKittiExampleGPS.m

@@ -42,6 +42,13 @@ sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadat
 g = [0;0;-9.8];
 w_coriolis = [0;0;0];
 
+IMU_params = PreintegrationParams(g);
+
+IMU_params.setAccelerometerCovariance(IMU_metadata.AccelerometerSigma.^2 * eye(3));
+IMU_params.setGyroscopeCovariance(IMU_metadata.GyroscopeSigma.^2 * eye(3));
+IMU_params.setIntegrationCovariance(IMU_metadata.IntegrationSigma.^2 * eye(3));
+IMU_params.setOmegaCoriolis(w_coriolis);
+
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
@@ -79,9 +86,7 @@ for measurementIndex = firstGPSPose:length(GPS_data)
     %% Summarize IMU data between the previous GPS measurement and now
     IMUindices = find(IMUtimes >= t_previous & IMUtimes <= t);
         
-    currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
+    currentSummarizedMeasurement = PreintegratedImuMeasurements(IMU_params,currentBias);
-      currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
-      IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
     
     for imuIndex = IMUindices
       accMeas = [ IMU_data(imuIndex).accelX; IMU_data(imuIndex).accelY; IMU_data(imuIndex).accelZ ];
@@ -94,7 +99,7 @@ for measurementIndex = firstGPSPose:length(GPS_data)
     newFactors.add(ImuFactor( ...
       currentPoseKey-1, currentVelKey-1, ...
       currentPoseKey, currentVelKey, ...
-      currentBiasKey, currentSummarizedMeasurement, g, w_coriolis));
+      currentBiasKey, currentSummarizedMeasurement));
 
     % Bias evolution as given in the IMU metadata
     newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
@@ -120,9 +125,12 @@ for measurementIndex = firstGPSPose:length(GPS_data)
       newFactors = NonlinearFactorGraph;
       newValues = Values;
 
+      result = isam.calculateEstimate();
+      
       if rem(measurementIndex,10)==0 % plot every 10 time steps
         cla;
-        plot3DTrajectory(isam.calculateEstimate, 'g-');
+        
+        plot3DTrajectory(result, 'g-');
         title('Estimated trajectory using ISAM2 (IMU+GPS)')
         xlabel('[m]')
         ylabel('[m]')
@@ -131,9 +139,10 @@ for measurementIndex = firstGPSPose:length(GPS_data)
         drawnow;
       end
       % =======================================================================
-      currentPoseGlobal = isam.calculateEstimate(currentPoseKey);
+
-      currentVelocityGlobal = isam.calculateEstimate(currentVelKey);
+      currentPoseGlobal = result.atPose3(currentPoseKey);
-      currentBias = isam.calculateEstimate(currentBiasKey);
+      currentVelocityGlobal = result.atVector(currentVelKey);
+      currentBias = result.atConstantBias(currentBiasKey);
     end
   end