From 37e6b796ec8eb87fb70c6c079fd62aebe7a2f899 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 5 Dec 2014 09:29:00 +0100 Subject: [PATCH] Slightly edited and re-formatted comments --- gtsam/navigation/CombinedImuFactor.h | 55 ++++++++++++++++----------- gtsam/navigation/ImuFactor.h | 32 +++++++++------- gtsam/navigation/PreintegrationBase.h | 10 +++-- 3 files changed, 58 insertions(+), 39 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index d1b27d338..98a248024 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -34,36 +34,47 @@ namespace gtsam { * @addtogroup SLAM * * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating + * conditionally independent sets in factor graphs: a unifying perspective based + * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014. * - * REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + ** REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + * Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + * High-Dynamic Motion in Built Environments Without Initial Conditions", + * TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + * Computation of the Jacobian Matrices", Tech. Report, 2013. */ /** - * CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias - * at previous time step), and current state (pose, velocity, bias at current time step). According to the - * preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are - * "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor: - * 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step). - * Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices - * "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution. - * 2) The preintegration covariance takes into account the noise in the bias estimate used for integration. - * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty - * and the preintegrated measurements uncertainty. + * CombinedImuFactor is a 6-ways factor involving previous state (pose and + * velocity of the vehicle, as well as bias at previous time step), and current + * state (pose, velocity, bias at current time step). Following the pre- + * integration scheme proposed in [2], the CombinedImuFactor includes many IMU + * measurements, which are "summarized" using the CombinedPreintegratedMeasurements + * class. There are 3 main differences wrpt the ImuFactor class: + * 1) The factor is 6-ways, meaning that it also involves both biases (previous + * and current time step).Therefore, the factor internally imposes the biases + * to be slowly varying; in particular, the matrices "biasAccCovariance" and + * "biasOmegaCovariance" described the random walk that models bias evolution. + * 2) The preintegration covariance takes into account the noise in the bias + * estimate used for integration. + * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves + * the correlation between the bias uncertainty and the preintegrated + * measurements uncertainty. */ class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase{ public: - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the CombinedPreintegrated IMU factor (CombinedImuFactor). - * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received - * from the IMU) so as to avoid costly integration at time of factor construction. + /** + * CombinedPreintegratedMeasurements integrates the IMU measurements + * (rotation rates and accelerations) and the corresponding covariance matrix. + * The measurements are then used to build the CombinedImuFactor. Integration + * is done incrementally (ideally, one integrates the measurement as soon as + * it is received from the IMU) so as to avoid costly integration at time of + * factor construction. */ class CombinedPreintegratedMeasurements: public PreintegrationBase { diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 98cc5e642..89344f1a1 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -39,19 +39,24 @@ namespace gtsam { * Int. Conf. on Robotics and Automation (ICRA), 2014. * ** REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + * Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + * High-Dynamic Motion in Built Environments Without Initial Conditions", + * TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + * Computation of the Jacobian Matrices", Tech. Report, 2013. */ /** - * ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step), - * current state (pose and velocity at current time step), and the bias estimate. According to the - * preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are - * "summarized" using the PreintegratedMeasurements class. - * Note that this factor does not force "temporal consistency" of the biases (which are usually - * slowly varying quantities), see also CombinedImuFactor for more details. + * ImuFactor is a 5-ways factor involving previous state (pose and velocity of + * the vehicle at previous time step), current state (pose and velocity at + * current time step), and the bias estimate. Following the preintegration + * scheme proposed in [2], the ImuFactor includes many IMU measurements, which + * are "summarized" using the PreintegratedMeasurements class. + * Note that this factor does not model "temporal consistency" of the biases + * (which are usually slowly varying quantities), which is up to the caller. + * See also CombinedImuFactor for a class that does this for you. */ class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { public: @@ -59,9 +64,10 @@ public: /** * PreintegratedMeasurements accumulates (integrates) the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. - * The measurements are then used to build the Preintegrated IMU factor (ImuFactor). - * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received - * from the IMU) so as to avoid costly integration at time of factor construction. + * The measurements are then used to build the Preintegrated IMU factor. + * Integration is done incrementally (ideally, one integrates the measurement + * as soon as it is received from the IMU) so as to avoid costly integration + * at time of factor construction. */ class PreintegratedMeasurements: public PreintegrationBase { diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 55a793004..3ce3336df 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -28,9 +28,10 @@ namespace gtsam { /** - * PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor.h) and - * CombinedPreintegratedMeasurements (in CombinedImuFactor.h). It includes the definitions of the - * preintegrated variables and the methods to access, print, and compare them. + * PreintegrationBase is the base class for PreintegratedMeasurements + * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). + * It includes the definitions of the preintegrated variables and the methods + * to access, print, and compare them. */ class PreintegrationBase { @@ -119,7 +120,8 @@ public: } /// Update Jacobians to be used during preintegration - void updatePreintegratedJacobians(const Vector3& correctedAcc, const Matrix3& Jr_theta_incr, const Rot3& Rincr, double deltaT){ + void updatePreintegratedJacobians(const Vector3& correctedAcc, + const Matrix3& Jr_theta_incr, const Rot3& Rincr, double deltaT){ if(!use2ndOrderIntegration_){ delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;