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

15
gtsam/navigation/CombinedImuFactor.h
View File

@ -66,12 +66,12 @@ public:
struct Params : PreintegrationParams { struct Params : 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 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 /// See two named constructors below for good values of n_gravity in body frame
Params(const Vector3& n_gravity) : Params(const Vector3& n_gravity) :
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( 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 // 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_BASE_OBJECT_NVP(PreintegratedRotation::Params);
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);
} }
}; };
@ -166,9 +166,7 @@ public:
/// @} /// @}
/// @name Deprecated #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @{
/// deprecated constructor /// deprecated constructor
/// NOTE(frank): assumes Z-Down convention, only second order integration supported /// NOTE(frank): assumes Z-Down convention, only second order integration supported
PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat, PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
@ -176,9 +174,8 @@ public:
const Matrix3& measuredOmegaCovariance, const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true); const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration = true);
#endif
/// @}
private: private:
/// Serialization function /// Serialization function

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

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