Skip to content

Commit

Permalink
revert name change, save for another PR
Browse files Browse the repository at this point in the history
  • Loading branch information
varunagrawal committed Aug 10, 2022
1 parent ac28b0e commit 103c78b
Show file tree
Hide file tree
Showing 7 changed files with 15 additions and 15 deletions.
2 changes: 1 addition & 1 deletion examples/CombinedImuFactorsExample.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
2 changes: 1 addition & 1 deletion examples/ImuFactorsExample.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
6 changes: 3 additions & 3 deletions gtsam/navigation/CombinedImuFactor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -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);
}

//------------------------------------------------------------------------------
Expand Down Expand Up @@ -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()
Expand Down
12 changes: 6 additions & 6 deletions gtsam/navigation/CombinedImuFactor.h
Original file line number Diff line number Diff line change
Expand Up @@ -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) {

}

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

Expand All @@ -109,7 +109,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
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:
Expand Down
2 changes: 1 addition & 1 deletion gtsam/navigation/tests/testCombinedImuFactor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
4 changes: 2 additions & 2 deletions tests/testImuPreintegration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 103c78b

Please sign in to comment.