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

Slightly edited and re-formatted comments

release/4.3a0
dellaert 2014-12-05 09:29:00 +01:00
parent cc03a13f5f
commit 37e6b796ec
3 changed files with 58 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
gtsam/navigation/CombinedImuFactor.h
View File

@ -34,36 +34,47 @@ namespace gtsam {
* @addtogroup SLAM * @addtogroup SLAM
* *
* If you are using the factor, please cite: * If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
* independent sets in factor graphs: a unifying perspective based on smart factors, * conditionally independent sets in factor graphs: a unifying perspective based
* Int. Conf. on Robotics and Automation (ICRA), 2014. * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
* *
* REFERENCES: ** REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built * Volume 2, 2008.
* Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. * 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 * CombinedImuFactor is a 6-ways factor involving previous state (pose and
* at previous time step), and current state (pose, velocity, bias at current time step). According to the * velocity of the vehicle, as well as bias at previous time step), and current
* preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are * state (pose, velocity, bias at current time step). Following the pre-
* "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor: * integration scheme proposed in [2], the CombinedImuFactor includes many IMU
* 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step). * measurements, which are "summarized" using the CombinedPreintegratedMeasurements
* Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices * class. There are 3 main differences wrpt the ImuFactor class:
* "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution. * 1) The factor is 6-ways, meaning that it also involves both biases (previous
* 2) The preintegration covariance takes into account the noise in the bias estimate used for integration. * and current time step).Therefore, the factor internally imposes the biases
* 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty * to be slowly varying; in particular, the matrices "biasAccCovariance" and
* and the preintegrated measurements uncertainty. * "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<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, public ImuFactorBase{ class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, public ImuFactorBase{
public: 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). * CombinedPreintegratedMeasurements integrates the IMU measurements
* Integration is done incrementally (ideally, one integrates the measurement as soon as it is received * (rotation rates and accelerations) and the corresponding covariance matrix.
* from the IMU) so as to avoid costly integration at time of factor construction. * 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 { class CombinedPreintegratedMeasurements: public PreintegrationBase {

32
gtsam/navigation/ImuFactor.h
View File

@ -39,19 +39,24 @@ namespace gtsam {
* Int. Conf. on Robotics and Automation (ICRA), 2014. * Int. Conf. on Robotics and Automation (ICRA), 2014.
* *
** REFERENCES: ** REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built * Volume 2, 2008.
* Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. * 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), * ImuFactor is a 5-ways factor involving previous state (pose and velocity of
* current state (pose and velocity at current time step), and the bias estimate. According to the * the vehicle at previous time step), current state (pose and velocity at
* preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are * current time step), and the bias estimate. Following the preintegration
* "summarized" using the PreintegratedMeasurements class. * scheme proposed in [2], the ImuFactor includes many IMU measurements, which
* Note that this factor does not force "temporal consistency" of the biases (which are usually * are "summarized" using the PreintegratedMeasurements class.
* slowly varying quantities), see also CombinedImuFactor for more details. * 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<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuFactorBase { class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuFactorBase {
public: public:
@ -59,9 +64,10 @@ public:
/** /**
* PreintegratedMeasurements accumulates (integrates) the IMU measurements * PreintegratedMeasurements accumulates (integrates) the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix. * (rotation rates and accelerations) and the corresponding covariance matrix.
* The measurements are then used to build the Preintegrated IMU factor (ImuFactor). * 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 * Integration is done incrementally (ideally, one integrates the measurement
* from the IMU) so as to avoid costly integration at time of factor construction. * as soon as it is received from the IMU) so as to avoid costly integration
* at time of factor construction.
*/ */
class PreintegratedMeasurements: public PreintegrationBase { class PreintegratedMeasurements: public PreintegrationBase {

10
gtsam/navigation/PreintegrationBase.h
View File

@ -28,9 +28,10 @@ namespace gtsam {
/** /**
* PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor.h) and * PreintegrationBase is the base class for PreintegratedMeasurements
* CombinedPreintegratedMeasurements (in CombinedImuFactor.h). It includes the definitions of the * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor).
* preintegrated variables and the methods to access, print, and compare them. * It includes the definitions of the preintegrated variables and the methods
* to access, print, and compare them.
*/ */
class PreintegrationBase { class PreintegrationBase {
@ -119,7 +120,8 @@ public:
} }
/// Update Jacobians to be used during preintegration /// 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_){ if(!use2ndOrderIntegration_){
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;