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: // 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;
} }

View File

@ -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;
} }

View File

@ -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()

View File

@ -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:

View File

@ -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

View File

@ -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,

View File

@ -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;