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
*
* 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<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, 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 {

32
gtsam/navigation/ImuFactor.h
View File

@ -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<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, 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 {

10
gtsam/navigation/PreintegrationBase.h
View File

@ -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;