From a12be48af09bc43ee82be9b7a9aa330738805ae8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 12:32:06 +0100 Subject: [PATCH] Now use Matrix.h constants --- gtsam/navigation/CombinedImuFactor.cpp | 25 -------- gtsam/navigation/CombinedImuFactor.h | 3 - gtsam/navigation/ImuFactor.h | 59 ++++++++++--------- .../tests/testCombinedImuFactor.cpp | 13 ++-- 4 files changed, 39 insertions(+), 61 deletions(-) delete mode 100644 gtsam/navigation/CombinedImuFactor.cpp diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp deleted file mode 100644 index 9448517f2..000000000 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ /dev/null @@ -1,25 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file CombinedImuFactor.cpp - * @author Frank Dellaert - * @date Nov 28, 2014 - **/ - -#include - -namespace gtsam { - -const Matrix3 CombinedImuFactor::Z_3x3 = Matrix3::Zero(); -const Matrix3 CombinedImuFactor::I_3x3 = Matrix3::Identity(); - -} /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index e48a0f053..a60b9ba07 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -67,9 +67,6 @@ namespace gtsam { class CombinedImuFactor: public NoiseModelFactor6 { - static const Matrix3 Z_3x3; - static const Matrix3 I_3x3; - public: /** Struct to store results of preintegrating IMU measurements. Can be build diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index d1b8a9d80..b6205edf8 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -11,7 +11,12 @@ /** * @file ImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert **/ #pragma once @@ -90,21 +95,21 @@ struct PoseVelocity { const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors const bool use2ndOrderIntegration = false ///< Controls the order of integration ) : biasHat_(bias), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration) + delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), + delRdelBiasOmega_(Z_3x3), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration) { - measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; + measurementCovariance_ << integrationErrorCovariance , Z_3x3, Z_3x3, + Z_3x3, measuredAccCovariance, Z_3x3, + Z_3x3, Z_3x3, measuredOmegaCovariance; PreintMeasCov_ = Matrix::Zero(9,9); } PreintegratedMeasurements() : biasHat_(imuBias::ConstantBias()), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(false) + delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), + delRdelBiasOmega_(Z_3x3), PreintMeasCov_(9,9), use2ndOrderIntegration_(false) { measurementCovariance_ = Matrix::Zero(9,9); PreintMeasCov_ = Matrix::Zero(9,9); @@ -155,11 +160,11 @@ struct PoseVelocity { deltaVij_ = Vector3::Zero(); deltaRij_ = Rot3(); deltaTij_ = 0.0; - delPdelBiasAcc_ = Matrix3::Zero(); - delPdelBiasOmega_ = Matrix3::Zero(); - delVdelBiasAcc_ = Matrix3::Zero(); - delVdelBiasOmega_ = Matrix3::Zero(); - delRdelBiasOmega_ = Matrix3::Zero(); + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; + delRdelBiasOmega_ = Z_3x3; PreintMeasCov_ = Matrix::Zero(9,9); } @@ -206,8 +211,6 @@ struct PoseVelocity { // Update preintegrated measurements covariance /* ----------------------------------------------------------------------------------------------------------------------- */ - Matrix3 Z_3x3 = Matrix3::Zero(); - Matrix3 I_3x3 = Matrix3::Identity(); const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); @@ -336,7 +339,7 @@ struct PoseVelocity { #endif /** Default constructor - only use for serialization */ - ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()), use2ndOrderCoriolis_(false){} + ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){} /** Constructor */ ImuFactor( @@ -456,7 +459,7 @@ struct PoseVelocity { } else{ dfPdPi = - Rot_i.matrix(); - dfVdPi = Matrix3::Zero(); + dfVdPi = Z_3x3; } (*H1) << @@ -473,20 +476,20 @@ struct PoseVelocity { // dfR/dRi Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), // dfR/dPi - Matrix3::Zero(); + Z_3x3; } if(H2) { H2->resize(9,3); (*H2) << // dfP/dVi - - Matrix3::Identity() * deltaTij + - I_3x3 * deltaTij + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - - Matrix3::Identity() + - I_3x3 + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term // dfR/dVi - Matrix3::Zero(); + Z_3x3; } if(H3) { @@ -494,22 +497,22 @@ struct PoseVelocity { H3->resize(9,6); (*H3) << // dfP/dPosej - Matrix3::Zero(), Rot_j.matrix(), + Z_3x3, Rot_j.matrix(), // dfV/dPosej Matrix::Zero(3,6), // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); + Jrinv_fRhat * ( I_3x3 ), Z_3x3; } if(H4) { H4->resize(9,3); (*H4) << // dfP/dVj - Matrix3::Zero(), + Z_3x3, // dfV/dVj - Matrix3::Identity(), + I_3x3, // dfR/dVj - Matrix3::Zero(); + Z_3x3; } if(H5) { diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index fda82eca9..aab38f258 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -10,19 +10,22 @@ * -------------------------------------------------------------------------- */ /** - * @file testImuFactor.cpp - * @brief Unit test for ImuFactor - * @author Luca Carlone, Stephen Williams, Richard Roberts + * @file testCombinedImuFactor.cpp + * @brief Unit test for Lupton-style combined IMU factor + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts */ -#include -#include #include #include #include #include +#include +#include #include #include + #include #include