revert name change, save for another PR

release/4.3a0
Varun Agrawal 2022-08-10 10:31:10 -04:00
parent ac28b0e969
commit 103c78b0de
7 changed files with 15 additions and 15 deletions

View File

@ -123,7 +123,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc 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;
}

View File

@ -110,7 +110,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc 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;
}

View File

@ -39,7 +39,7 @@ void PreintegrationCombinedParams::print(const string& s) const {
<< endl;
cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
<< endl;
cout << "biasAccOmegaInit:\n[\n" << biasAccOmegaInit << "\n]"
cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
<< endl;
}
@ -52,7 +52,7 @@ bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& oth
tol) &&
equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
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& wCov = p().gyroscopeCovariance;
const Matrix3& iCov = p().integrationCovariance;
const Matrix6& bInitCov = p().biasAccOmegaInit;
const Matrix6& bInitCov = p().biasAccOmegaInt;
// first order uncertainty propagation
// Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()

View File

@ -62,19 +62,19 @@ typedef ManifoldPreintegration PreintegrationType;
struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer 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.
/// Used for serialization.
PreintegrationCombinedParams()
: biasAccCovariance(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
PreintegrationCombinedParams(const Vector3& n_gravity) :
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 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& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInit; }
const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
private:
@ -109,7 +109,7 @@ private:
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
}
public:

View File

@ -43,7 +43,7 @@ static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
p->integrationCovariance = 0.0001 * I_3x3;
p->biasAccCovariance = Z_3x3;
p->biasOmegaCovariance = Z_3x3;
p->biasAccOmegaInit = Z_6x6;
p->biasAccOmegaInt = Z_6x6;
return p;
}
} // namespace testing

View File

@ -57,7 +57,7 @@ struct IMUHelper {
p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous
p->biasOmegaCovariance =
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
Rot3 iRb(0.036129, -0.998727, 0.035207,

View File

@ -43,7 +43,7 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
double gyrNoiseSigma = 0.000208;
double gyrBiasRwSigma = 0.000004;
double integrationCovariance = 1e-8;
double biasAccOmegaInit = 1e-5;
double biasAccOmegaInt = 1e-5;
double gravity = 9.81;
double rate = 400.0; // Hz
@ -76,7 +76,7 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
imuPreintegratedParams->gyroscopeCovariance = I_3x3 * pow(gyrNoiseSigma, 2);
imuPreintegratedParams->biasOmegaCovariance = I_3x3 * pow(gyrBiasRwSigma, 2);
imuPreintegratedParams->integrationCovariance = I_3x3 * integrationCovariance;
imuPreintegratedParams->biasAccOmegaInit = I_6x6 * biasAccOmegaInit;
imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt;
// Initial state
Pose3 priorPose;