Merged in fix/matlab_wrapper (pull request #242)

Fix/matlab_wrapper

gtsam.h

@@ -2489,10 +2489,30 @@ class NavState {
   gtsam::Pose3 pose() const;
 };
 
+#include <gtsam/navigation/PreintegratedRotation.h>
+virtual class PreintegratedRotationParams {
+  PreintegratedRotationParams();
+  void setGyroscopeCovariance(Matrix cov);
+  void setOmegaCoriolis(Vector omega);
+  void setBodyPSensor(const gtsam::Pose3& pose);
+
+  Matrix getGyroscopeCovariance() const;
+
+  // TODO(frank): allow optional
+  //  boost::optional<Vector3> getOmegaCoriolis() const;
+  //  boost::optional<Pose3>   getBodyPSensor()   const;
+};
+
 #include <gtsam/navigation/PreintegrationParams.h>
-class PreintegrationParams {
+virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
   PreintegrationParams(Vector n_gravity);
-  // TODO(frank): add setters/getters or make this MATLAB wrapper feature
+  void setAccelerometerCovariance(Matrix cov);
+  void setIntegrationCovariance(Matrix cov);
+  void setUse2ndOrderCoriolis(bool flag);
+
+  Matrix getAccelerometerCovariance() const;
+  Matrix getIntegrationCovariance()   const;
+  bool   getUse2ndOrderCoriolis()     const;
 };
 
 #include <gtsam/navigation/PreintegrationBase.h>

gtsam/navigation/PreintegratedRotation.h

@@ -26,6 +26,38 @@
 
 namespace gtsam {
 
+/// Parameters for pre-integration:
+/// Usage: Create just a single Params and pass a shared pointer to the constructor
+struct PreintegratedRotationParams {
+  Matrix3 gyroscopeCovariance;  ///< continuous-time "Covariance" of gyroscope measurements
+  boost::optional<Vector3> omegaCoriolis;  ///< Coriolis constant
+  boost::optional<Pose3> body_P_sensor;    ///< The pose of the sensor in the body frame
+
+  PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
+
+  virtual void print(const std::string& s) const;
+  virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
+
+  void setGyroscopeCovariance(const Matrix3& cov)   { gyroscopeCovariance = cov;  }
+  void setOmegaCoriolis(const Vector3& omega)       { omegaCoriolis.reset(omega); }
+  void setBodyPSensor(const Pose3& pose)            { body_P_sensor.reset(pose);  }
+
+  const Matrix3& getGyroscopeCovariance()     const { return gyroscopeCovariance; }
+  boost::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
+  boost::optional<Pose3>   getBodyPSensor()   const { return body_P_sensor; }
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    namespace bs = ::boost::serialization;
+    ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
+    ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
+    ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
+  }
+};
+
 /**
  * PreintegratedRotation is the base class for all PreintegratedMeasurements
  * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor).
@@ -33,29 +65,7 @@ namespace gtsam {
  */
 class PreintegratedRotation {
  public:
-  /// Parameters for pre-integration:
+  typedef PreintegratedRotationParams Params;
-  /// Usage: Create just a single Params and pass a shared pointer to the constructor
-  struct Params {
-    Matrix3 gyroscopeCovariance;  ///< continuous-time "Covariance" of gyroscope measurements
-    boost::optional<Vector3> omegaCoriolis;  ///< Coriolis constant
-    boost::optional<Pose3> body_P_sensor;    ///< The pose of the sensor in the body frame
-
-    Params() : gyroscopeCovariance(I_3x3) {}
-
-    virtual void print(const std::string& s) const;
-    virtual bool equals(const Params& other, double tol=1e-9) const;
-
-   private:
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      namespace bs = ::boost::serialization;
-      ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
-      ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
-      ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
-    }
-  };
 
  protected:
   /// Parameters

gtsam/navigation/PreintegrationParams.h

@@ -23,7 +23,7 @@ namespace gtsam {
 
 /// Parameters for pre-integration:
 /// Usage: Create just a single Params and pass a shared pointer to the constructor
-struct PreintegrationParams: PreintegratedRotation::Params {
+struct PreintegrationParams: PreintegratedRotationParams {
   Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
   Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
   bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
@@ -50,6 +50,14 @@ struct PreintegrationParams: PreintegratedRotation::Params {
   void print(const std::string& s) const;
   bool equals(const PreintegratedRotation::Params& other, double tol) const;
 
+  void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
+  void setIntegrationCovariance(const Matrix3& cov)   { integrationCovariance = cov; }
+  void setUse2ndOrderCoriolis(bool flag)              { use2ndOrderCoriolis = flag; }
+
+  const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; }
+  const Matrix3& getIntegrationCovariance()   const { return integrationCovariance; }
+  bool           getUse2ndOrderCoriolis()     const { return use2ndOrderCoriolis; }
+
 protected:
   /// Default constructor for serialization only: uninitialized!
   PreintegrationParams() {}
@@ -60,7 +68,7 @@ protected:
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     namespace bs = ::boost::serialization;
     ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
-         boost::serialization::base_object<PreintegratedRotation::Params>(*this));
+         boost::serialization::base_object<PreintegratedRotationParams>(*this));
     ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
     ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
     ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);

matlab/unstable_examples/FlightCameraTransformIMU.m

@@ -1,3 +1,7 @@
+% Simulation for concurrent IMU, camera, IMU-camera transform estimation during flight with known landmarks
+% author: Chris Beall
+% date: July 2014
+
 clear all;
 clf;
 
@@ -24,13 +28,19 @@ if(write_video)
     open(videoObj);
 end
 
-% IMU parameters
+%% IMU parameters
 IMU_metadata.AccelerometerSigma = 1e-2;    
 IMU_metadata.GyroscopeSigma = 1e-2;
 IMU_metadata.AccelerometerBiasSigma = 1e-6;
 IMU_metadata.GyroscopeBiasSigma = 1e-6;
 IMU_metadata.IntegrationSigma = 1e-1;
 
+n_gravity = [0;0;-9.8];
+IMU_params = PreintegrationParams(n_gravity);
+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));
+
 transformKey = 1000;
 calibrationKey = 2000;
 
@@ -52,7 +62,7 @@ K = Cal3_S2(20,1280,960);
 % initialize K incorrectly
 K_corrupt = Cal3_S2(K.fx()+10,K.fy()+10,0,K.px(),K.py());
 
-isamParams = gtsam.ISAM2Params;
+isamParams = ISAM2Params;
 isamParams.setFactorization('QR');
 isam = ISAM2(isamParams);
 
@@ -63,7 +73,6 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
 sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
 sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]);
 sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ];
-g = [0;0;-9.8];
 w_coriolis = [0;0;0];
 
 %% generate trajectory and landmarks
@@ -99,7 +108,9 @@ zlabel('z');
 title('Estimated vs. actual IMU-cam transform');
 axis equal;
 
+%% Main loop
 for i=1:size(trajectory)-1
+    %% Preliminaries
     xKey = symbol('x',i);
     pose = trajectory.atPose3(xKey);     % GT pose
     pose_t = pose.translation();    % GT pose-translation
@@ -134,6 +145,7 @@ for i=1:size(trajectory)-1
     gps_pose = pose.retract([0; 0; 0; normrnd(0,gps_noise,3,1)]);
     fg.add(PoseTranslationPrior3D(xKey, gps_pose, GPS_trans_cov));
     
+    %% First-time initialization
     if i==1
         % camera transform
         if use_camera_transform_noise
@@ -153,12 +165,10 @@ for i=1:size(trajectory)-1
         result = initial;
     end
     
-    % priors on first two poses
+    %% priors on first two poses
     if i < 3        
-%         fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
+        % fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
         fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
-        
-        
     end
    
     %% the 'normal' case
@@ -181,12 +191,14 @@ for i=1:size(trajectory)-1
                 zKey = measurementKeys.at(zz);
                 lKey = symbol('l',symbolIndex(zKey));
 
-                fg.add(TransformCalProjectionFactorCal3_S2(measurements.atPoint2(zKey), ...
+                fg.add(ProjectionFactorPPPCCal3_S2(measurements.atPoint2(zKey), ...
                     z_cov, xKey, transformKey, lKey, calibrationKey, false, true));
 
                 % only add landmark to values if doesn't exist yet
                 if ~result.exists(lKey)
-                    noisy_landmark = landmarks.atPoint3(lKey).compose(Point3(normrnd(0,landmark_noise,3,1)));
+                    p = landmarks.atPoint3(lKey);
+                    n = normrnd(0,landmark_noise,3,1);
+                    noisy_landmark = Point3(p.x()+n(1),p.y()+n(2),p.z()+n(3));
                     initial.insert(lKey, noisy_landmark);
 
                     % and add a prior since its position is known
@@ -195,14 +207,12 @@ for i=1:size(trajectory)-1
             end
         end % end landmark observations 
         
-
         %% IMU
         deltaT = 1;
         logmap = Pose3.Logmap(step);
         omega = logmap(1:3);
         velocity = logmap(4:6);
        
-       
         % Simulate IMU measurements, considering Coriolis effect 
         % (in this simple example we neglect gravity and there are no other forces acting on the body)
         acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
@@ -211,11 +221,9 @@ for i=1:size(trajectory)-1
 %         [ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
 %     currentIMUPoseGlobal, omega, velocity, velocity, deltaT);
 
-        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));
     
-        accMeas = acc_omega(1:3)-g;
+        accMeas = acc_omega(1:3)-n_gravity;
         omegaMeas = acc_omega(4:6);
         currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);
 
@@ -223,13 +231,13 @@ for i=1:size(trajectory)-1
         fg.add(ImuFactor( ...
         xKey_prev, currentVelKey-1, ...
         xKey, currentVelKey, ...
-        currentBiasKey, currentSummarizedMeasurement, g, w_coriolis));
+        currentBiasKey, currentSummarizedMeasurement));
     
         % Bias evolution as given in the IMU metadata
         fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
         noiseModel.Diagonal.Sigmas(sqrt(10) * sigma_between_b)));
     
-        % ISAM update
+        %% ISAM update
         isam.update(fg, initial);
         result = isam.calculateEstimate();
         
@@ -295,10 +303,8 @@ for i=1:size(trajectory)-1
     
 end
 
-% print out final camera transform
+%% print out final camera transform and write video
 result.at(transformKey);
-
-
 if(write_video)
     close(videoObj);
 end