12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

undo name change from 984a90

release/4.3a0
Varun Agrawal 2021-09-28 13:43:04 -04:00
parent 6bc9b50a46
commit af714cdb5a
6 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/CombinedImuFactorsExample.cpp
View File

@ -107,7 +107,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
I_3x3 * 1e-8; // error committed in integrating position from velocities I_3x3 * 1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
Matrix66 bias_acc_omega_int = Matrix66 bias_acc_omega_init =
I_6x6 * 1e-5; // error in the bias used for preintegration I_6x6 * 1e-5; // error in the bias used for preintegration
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@ -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->biasAccOmegaInt = bias_acc_omega_int; p->biasAccOmegaInit = bias_acc_omega_init;
return p; return p;
} }

4
examples/ImuFactorsExample.cpp
View File

@ -94,7 +94,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
I_3x3 * 1e-8; // error committed in integrating position from velocities I_3x3 * 1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
Matrix66 bias_acc_omega_int = Matrix66 bias_acc_omega_init =
I_6x6 * 1e-5; // error in the bias used for preintegration I_6x6 * 1e-5; // error in the bias used for preintegration
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@ -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->biasAccOmegaInt = bias_acc_omega_int; p->biasAccOmegaInit = bias_acc_omega_init;
return p; return p;
} }

10
gtsam/navigation/CombinedImuFactor.cpp
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 << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]" cout << "biasAccOmegaInit:\n[\n" << biasAccOmegaInit << "\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(biasAccOmegaInt, e->biasAccOmegaInt, tol); equal_with_abs_tol(biasAccOmegaInit, e->biasAccOmegaInit, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -137,8 +137,8 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
Eigen::Matrix<double, 15, 15> G_measCov_Gt; Eigen::Matrix<double, 15, 15> G_measCov_Gt;
G_measCov_Gt.setZero(15, 15); G_measCov_Gt.setZero(15, 15);
Matrix3 aCov_updated = aCov + p().biasAccOmegaInt.block<3, 3>(0, 0); Matrix3 aCov_updated = aCov + p().biasAccOmegaInit.block<3, 3>(0, 0);
Matrix3 wCov_updated = wCov + p().biasAccOmegaInt.block<3, 3>(3, 3); Matrix3 wCov_updated = wCov + p().biasAccOmegaInit.block<3, 3>(3, 3);
// BLOCK DIAGONAL TERMS // BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = (pos_H_biasAcc // D_t_t(&G_measCov_Gt) = (pos_H_biasAcc //
@ -156,7 +156,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS // OFF BLOCK DIAGONAL TERMS
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInit.block<3, 3>(3, 0)
* theta_H_biasOmega.transpose(); * theta_H_biasOmega.transpose();
D_v_R(&G_measCov_Gt) = temp; D_v_R(&G_measCov_Gt) = temp;
D_v_t(&G_measCov_Gt) = vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose(); D_v_t(&G_measCov_Gt) = vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose();

12
gtsam/navigation/CombinedImuFactor.h
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 biasAccOmegaInt; ///< covariance of bias used for pre-integration Matrix6 biasAccOmegaInit; ///< 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),
biasAccOmegaInt(I_6x6) {} biasAccOmegaInit(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), biasAccOmegaInt(I_6x6) { biasOmegaCovariance(I_3x3), biasAccOmegaInit(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 setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; } void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInit=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& getBiasAccOmegaInt() const { return biasAccOmegaInt; } const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInit; }
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(biasAccOmegaInt); ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
} }
public: public:

2
gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp
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->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5; p->biasAccOmegaInit = 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,

4
tests/testImuPreintegration.cpp
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 biasAccOmegaInt = 1e-5; double biasAccOmegaInit = 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->biasAccOmegaInt = I_6x6 * biasAccOmegaInt; imuPreintegratedParams->biasAccOmegaInit = I_6x6 * biasAccOmegaInit;
// Initial state // Initial state
Pose3 priorPose; Pose3 priorPose;