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

Deprecated constructor + fixed parameter name

release/4.3a0
Frank Dellaert 2016-01-30 10:19:04 -08:00
parent 3a2c2e23be
commit 984a90672f
3 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -108,16 +108,16 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = dt * iCov;
D_v_v(&G_measCov_Gt) = (1 / dt) * H_vel_biasacc *
(aCov + p().biasAccOmegaInit.block<3, 3>(0, 0)) *
(aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) *
(H_vel_biasacc.transpose());
D_R_R(&G_measCov_Gt) = (1 / dt) * H_angles_biasomega *
(wCov + p().biasAccOmegaInit.block<3, 3>(3, 3)) *
(wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) *
(H_angles_biasomega.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS
Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) *
Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInt.block<3, 3>(3, 0) *
H_angles_biasomega.transpose();
D_v_R(&G_measCov_Gt) = temp;
D_R_v(&G_measCov_Gt) = temp.transpose();
@ -125,11 +125,12 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
}
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit,
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt,
const bool use2ndOrderIntegration) {
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
@ -140,11 +141,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
p->integrationCovariance = integrationErrorCovariance;
p->biasAccCovariance = biasAccCovariance;
p->biasOmegaCovariance = biasOmegaCovariance;
p->biasAccOmegaInit = biasAccOmegaInit;
p->biasAccOmegaInt = biasAccOmegaInt;
p_ = p;
resetIntegration();
preintMeasCov_.setZero();
}
#endif
//------------------------------------------------------------------------------
// CombinedImuFactor methods
//------------------------------------------------------------------------------

15
gtsam/navigation/CombinedImuFactor.h
View File

@ -66,12 +66,12 @@ public:
struct Params : 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 for pre-integration
Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration
/// See two named constructors below for good values of n_gravity in body frame
Params(const Vector3& n_gravity) :
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
I_3x3), biasAccOmegaInit(I_6x6) {
I_3x3), biasAccOmegaInt(I_6x6) {
}
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
@ -95,7 +95,7 @@ public:
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
}
};
@ -166,9 +166,7 @@ public:
/// @}
/// @name Deprecated
/// @{
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// deprecated constructor
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
@ -176,9 +174,8 @@ public:
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true);
/// @}
const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration = true);
#endif
private:
/// Serialization function

7
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -13,8 +13,9 @@
* @file testCombinedImuFactor.cpp
* @brief Unit test for Lupton-style combined IMU factor
* @author Luca Carlone
* @author Stephen Williams
* @author Frank Dellaert
* @author Richard Roberts
* @author Stephen Williams
*/
#include <gtsam/navigation/ImuFactor.h>
@ -51,8 +52,8 @@ namespace {
PreintegratedCombinedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
PreintegratedCombinedMeasurements result(bias, I_3x3,
I_3x3, I_3x3, I_3x3, I_3x3, I_6x6);
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
PreintegratedCombinedMeasurements result(p, bias);
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();