revert name change, save for another PR
parent
ac28b0e969
commit
103c78b0de
|
@ -123,7 +123,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
|
||||||
// PreintegrationCombinedMeasurements params:
|
// PreintegrationCombinedMeasurements params:
|
||||||
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
|
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
|
||||||
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
||||||
p->biasAccOmegaInit = bias_acc_omega_init;
|
p->biasAccOmegaInt = bias_acc_omega_init;
|
||||||
|
|
||||||
return p;
|
return p;
|
||||||
}
|
}
|
||||||
|
|
|
@ -110,7 +110,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
|
||||||
// PreintegrationCombinedMeasurements params:
|
// PreintegrationCombinedMeasurements params:
|
||||||
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
|
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
|
||||||
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
||||||
p->biasAccOmegaInit = bias_acc_omega_init;
|
p->biasAccOmegaInt = bias_acc_omega_init;
|
||||||
|
|
||||||
return p;
|
return p;
|
||||||
}
|
}
|
||||||
|
|
|
@ -39,7 +39,7 @@ void PreintegrationCombinedParams::print(const string& s) const {
|
||||||
<< endl;
|
<< endl;
|
||||||
cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
|
cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
|
||||||
<< endl;
|
<< endl;
|
||||||
cout << "biasAccOmegaInit:\n[\n" << biasAccOmegaInit << "\n]"
|
cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
|
||||||
<< endl;
|
<< endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -52,7 +52,7 @@ bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& oth
|
||||||
tol) &&
|
tol) &&
|
||||||
equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
|
equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
|
||||||
tol) &&
|
tol) &&
|
||||||
equal_with_abs_tol(biasAccOmegaInit, e->biasAccOmegaInit, tol);
|
equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol);
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
@ -135,7 +135,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
||||||
const Matrix3& aCov = p().accelerometerCovariance;
|
const Matrix3& aCov = p().accelerometerCovariance;
|
||||||
const Matrix3& wCov = p().gyroscopeCovariance;
|
const Matrix3& wCov = p().gyroscopeCovariance;
|
||||||
const Matrix3& iCov = p().integrationCovariance;
|
const Matrix3& iCov = p().integrationCovariance;
|
||||||
const Matrix6& bInitCov = p().biasAccOmegaInit;
|
const Matrix6& bInitCov = p().biasAccOmegaInt;
|
||||||
|
|
||||||
// first order uncertainty propagation
|
// first order uncertainty propagation
|
||||||
// Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
|
// Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
|
||||||
|
|
|
@ -62,19 +62,19 @@ typedef ManifoldPreintegration PreintegrationType;
|
||||||
struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
|
struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
|
||||||
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
|
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
|
||||||
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
|
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
|
||||||
Matrix6 biasAccOmegaInit; ///< covariance of bias used as initial estimate.
|
Matrix6 biasAccOmegaInt; ///< covariance of bias used as initial estimate.
|
||||||
|
|
||||||
/// Default constructor makes uninitialized params struct.
|
/// Default constructor makes uninitialized params struct.
|
||||||
/// Used for serialization.
|
/// Used for serialization.
|
||||||
PreintegrationCombinedParams()
|
PreintegrationCombinedParams()
|
||||||
: biasAccCovariance(I_3x3),
|
: biasAccCovariance(I_3x3),
|
||||||
biasOmegaCovariance(I_3x3),
|
biasOmegaCovariance(I_3x3),
|
||||||
biasAccOmegaInit(I_6x6) {}
|
biasAccOmegaInt(I_6x6) {}
|
||||||
|
|
||||||
/// See two named constructors below for good values of n_gravity in body frame
|
/// See two named constructors below for good values of n_gravity in body frame
|
||||||
PreintegrationCombinedParams(const Vector3& n_gravity) :
|
PreintegrationCombinedParams(const Vector3& n_gravity) :
|
||||||
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
|
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
|
||||||
biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) {
|
biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -93,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
|
||||||
|
|
||||||
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
|
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
|
||||||
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
|
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
|
||||||
void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInit=cov; }
|
void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; }
|
||||||
|
|
||||||
const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
|
const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
|
||||||
const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
|
const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
|
||||||
const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInit; }
|
const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
@ -109,7 +109,7 @@ private:
|
||||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
|
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
|
||||||
ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
|
ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
|
||||||
ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
|
ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
|
||||||
ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
|
ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
|
||||||
}
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
|
@ -43,7 +43,7 @@ static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
|
||||||
p->integrationCovariance = 0.0001 * I_3x3;
|
p->integrationCovariance = 0.0001 * I_3x3;
|
||||||
p->biasAccCovariance = Z_3x3;
|
p->biasAccCovariance = Z_3x3;
|
||||||
p->biasOmegaCovariance = Z_3x3;
|
p->biasOmegaCovariance = Z_3x3;
|
||||||
p->biasAccOmegaInit = Z_6x6;
|
p->biasAccOmegaInt = Z_6x6;
|
||||||
return p;
|
return p;
|
||||||
}
|
}
|
||||||
} // namespace testing
|
} // namespace testing
|
||||||
|
|
|
@ -57,7 +57,7 @@ struct IMUHelper {
|
||||||
p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous
|
p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous
|
||||||
p->biasOmegaCovariance =
|
p->biasOmegaCovariance =
|
||||||
I_3x3 * pow(0.001, 2.0); // gyro bias in continuous
|
I_3x3 * pow(0.001, 2.0); // gyro bias in continuous
|
||||||
p->biasAccOmegaInit = Matrix::Identity(6, 6) * 1e-5;
|
p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5;
|
||||||
|
|
||||||
// body to IMU rotation
|
// body to IMU rotation
|
||||||
Rot3 iRb(0.036129, -0.998727, 0.035207,
|
Rot3 iRb(0.036129, -0.998727, 0.035207,
|
||||||
|
|
|
@ -43,7 +43,7 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
|
||||||
double gyrNoiseSigma = 0.000208;
|
double gyrNoiseSigma = 0.000208;
|
||||||
double gyrBiasRwSigma = 0.000004;
|
double gyrBiasRwSigma = 0.000004;
|
||||||
double integrationCovariance = 1e-8;
|
double integrationCovariance = 1e-8;
|
||||||
double biasAccOmegaInit = 1e-5;
|
double biasAccOmegaInt = 1e-5;
|
||||||
|
|
||||||
double gravity = 9.81;
|
double gravity = 9.81;
|
||||||
double rate = 400.0; // Hz
|
double rate = 400.0; // Hz
|
||||||
|
@ -76,7 +76,7 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
|
||||||
imuPreintegratedParams->gyroscopeCovariance = I_3x3 * pow(gyrNoiseSigma, 2);
|
imuPreintegratedParams->gyroscopeCovariance = I_3x3 * pow(gyrNoiseSigma, 2);
|
||||||
imuPreintegratedParams->biasOmegaCovariance = I_3x3 * pow(gyrBiasRwSigma, 2);
|
imuPreintegratedParams->biasOmegaCovariance = I_3x3 * pow(gyrBiasRwSigma, 2);
|
||||||
imuPreintegratedParams->integrationCovariance = I_3x3 * integrationCovariance;
|
imuPreintegratedParams->integrationCovariance = I_3x3 * integrationCovariance;
|
||||||
imuPreintegratedParams->biasAccOmegaInit = I_6x6 * biasAccOmegaInit;
|
imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt;
|
||||||
|
|
||||||
// Initial state
|
// Initial state
|
||||||
Pose3 priorPose;
|
Pose3 priorPose;
|
||||||
|
|
Loading…
Reference in New Issue