Merge pull request #1943 from borglab/improvements
						commit
						0c44f9f451
					
				|  | @ -101,7 +101,7 @@ std::pair<double, double> approximateDiscreteMarginal( | |||
|   // Do importance sampling
 | ||||
|   double w0 = 0.0, w1 = 0.0; | ||||
|   std::mt19937_64 rng(42); | ||||
|   for (int i = 0; i < N; i++) { | ||||
|   for (size_t i = 0; i < N; i++) { | ||||
|     HybridValues sample = q.sample(&rng); | ||||
|     sample.insert(given); | ||||
|     double weight = hbn.evaluate(sample) / q.evaluate(sample); | ||||
|  |  | |||
|  | @ -84,7 +84,7 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements | |||
|   /// @{
 | ||||
| 
 | ||||
|   /// Default constructor only for serialization and wrappers
 | ||||
|   PreintegratedCombinedMeasurements() { preintMeasCov_.setZero(); } | ||||
|   PreintegratedCombinedMeasurements() { resetIntegration(); } | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Default constructor, initializes the class with no measurements | ||||
|  | @ -97,7 +97,9 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements | |||
|       const imuBias::ConstantBias& biasHat = imuBias::ConstantBias(), | ||||
|       const Eigen::Matrix<double, 15, 15>& preintMeasCov = | ||||
|           Eigen::Matrix<double, 15, 15>::Zero()) | ||||
|       : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {} | ||||
|       : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) { | ||||
|     PreintegrationType::resetIntegration(); | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Construct preintegrated directly from members: base class and | ||||
|  | @ -108,7 +110,9 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements | |||
|   PreintegratedCombinedMeasurements( | ||||
|       const PreintegrationType& base, | ||||
|       const Eigen::Matrix<double, 15, 15>& preintMeasCov) | ||||
|       : PreintegrationType(base), preintMeasCov_(preintMeasCov) {} | ||||
|       : PreintegrationType(base), preintMeasCov_(preintMeasCov) { | ||||
|     PreintegrationType::resetIntegration(); | ||||
|   } | ||||
| 
 | ||||
|   /// Virtual destructor
 | ||||
|   ~PreintegratedCombinedMeasurements() override {} | ||||
|  |  | |||
|  | @ -79,7 +79,7 @@ public: | |||
| 
 | ||||
|   /// Default constructor for serialization and wrappers
 | ||||
|   PreintegratedImuMeasurements() { | ||||
|     preintMeasCov_.setZero(); | ||||
|     resetIntegration(); | ||||
|   } | ||||
| 
 | ||||
|  /**
 | ||||
|  | @ -90,7 +90,7 @@ public: | |||
|   PreintegratedImuMeasurements(const std::shared_ptr<PreintegrationParams>& p, | ||||
|       const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : | ||||
|       PreintegrationType(p, biasHat) { | ||||
|     preintMeasCov_.setZero(); | ||||
|     resetIntegration(); | ||||
|   } | ||||
| 
 | ||||
| /**
 | ||||
|  | @ -101,6 +101,7 @@ public: | |||
|   PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov) | ||||
|      : PreintegrationType(base), | ||||
|        preintMeasCov_(preintMeasCov) { | ||||
|     PreintegrationType::resetIntegration(); | ||||
|   } | ||||
| 
 | ||||
|   /// Virtual destructor
 | ||||
|  | @ -113,7 +114,7 @@ public: | |||
|   /// equals
 | ||||
|   bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const; | ||||
| 
 | ||||
|   /// Re-initialize PreintegratedIMUMeasurements
 | ||||
|   /// Re-initialize PreintegratedImuMeasurements
 | ||||
|   void resetIntegration() override; | ||||
| 
 | ||||
|   /**
 | ||||
|  | @ -159,7 +160,7 @@ public: | |||
|  * the vehicle at previous time step), current state (pose and velocity at | ||||
|  * current time step), and the bias estimate. Following the preintegration | ||||
|  * scheme proposed in [2], the ImuFactor includes many IMU measurements, which | ||||
|  * are "summarized" using the PreintegratedIMUMeasurements class. | ||||
|  * are "summarized" using the PreintegratedImuMeasurements class. | ||||
|  * Note that this factor does not model "temporal consistency" of the biases | ||||
|  * (which are usually slowly varying quantities), which is up to the caller. | ||||
|  * See also CombinedImuFactor for a class that does this for you. | ||||
|  |  | |||
|  | @ -156,6 +156,22 @@ virtual class ImuFactor: gtsam::NonlinearFactor { | |||
|   void serialize() const; | ||||
| }; | ||||
| 
 | ||||
| virtual class ImuFactor2: gtsam::NonlinearFactor { | ||||
|   ImuFactor2(); | ||||
|   ImuFactor2(size_t state_i, size_t state_j, | ||||
|       size_t bias, | ||||
|       const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); | ||||
| 
 | ||||
|   // Standard Interface | ||||
|   gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; | ||||
|   gtsam::Vector evaluateError(const gtsam::NavState& state_i, | ||||
|                               gtsam::NavState& state_j, | ||||
|                               const gtsam::imuBias::ConstantBias& bias_i); | ||||
| 
 | ||||
|   // enable serialization functionality | ||||
|   void serialize() const; | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam/navigation/CombinedImuFactor.h> | ||||
| virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { | ||||
|   PreintegrationCombinedParams(gtsam::Vector n_gravity); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue