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