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

Cherry-picked imuFixed differences

release/4.3a0
dellaert 2014-12-26 18:23:14 +01:00
parent 19f823a1fa
commit a881e8d3ee
14 changed files with 1688 additions and 1433 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
.cproject
View File

@ -592,7 +592,6 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -600,7 +599,6 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -648,7 +646,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -656,7 +653,6 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -664,7 +660,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -680,7 +675,6 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1136,7 +1130,6 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1366,46 +1359,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1488,6 +1441,7 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1527,6 +1481,7 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1534,6 +1489,7 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1547,6 +1503,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1804,7 +1800,6 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1812,7 +1807,6 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1820,7 +1814,6 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1828,7 +1821,6 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2002,6 +1994,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.navigation" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2 VERBOSE=1</buildArguments>
<buildTarget>check.navigation</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2683,7 +2683,6 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2691,7 +2690,6 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2699,7 +2697,6 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2809,14 +2806,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBasisDecompositions.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testBasisDecompositions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCustomChartExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
@ -3251,6 +3240,7 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>

126
gtsam.h
View File

@ -2404,25 +2404,24 @@ class ConstantBias {
}///\namespace imuBias
#include <gtsam/navigation/ImuFactor.h>
class PoseVelocity{
PoseVelocity(const gtsam::Pose3& pose, Vector velocity);
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
};
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
Matrix measurementCovariance() const;
Matrix deltaRij() const;
double deltaTij() const;
Matrix deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHat() const;
Vector biasHatVector() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
@ -2430,10 +2429,11 @@ class ImuFactorPreintegratedMeasurements {
Matrix delRdelBiasOmega() const;
Matrix preintMeasCov() const;
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
};
virtual class ImuFactor : gtsam::NonlinearFactor {
@ -2444,11 +2444,60 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
};
#include <gtsam/navigation/CombinedImuFactor.h>
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit,
bool use2ndOrderIntegration);
// CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
double deltaTij() const;
Matrix deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
Matrix preintMeasCov() const;
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
};
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
};
#include <gtsam/navigation/AHRSFactor.h>
class AHRSFactorPreintegratedMeasurements {
// Standard Constructor
@ -2461,7 +2510,6 @@ class AHRSFactorPreintegratedMeasurements {
bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
// get Data
Matrix measurementCovariance() const;
Matrix deltaRij() const;
double deltaTij() const;
Vector biasHat() const;
@ -2488,64 +2536,6 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
Vector omegaCoriolis) const;
};
#include <gtsam/navigation/CombinedImuFactor.h>
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
};
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit,
bool use2ndOrderIntegration);
CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
Matrix measurementCovariance() const;
Matrix deltaRij() const;
double deltaTij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHat() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
Matrix PreintMeasCov() const;
};
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis);
};
#include <gtsam/navigation/AttitudeFactor.h>
//virtual class AttitudeFactor : gtsam::NonlinearFactor {
// AttitudeFactor(const Unit3& nZ, const Unit3& bRef);

153
gtsam/navigation/AHRSFactor.cpp
View File

@ -18,9 +18,9 @@
**/
#include <gtsam/navigation/AHRSFactor.h>
#include <iostream>
/* External or standard includes */
#include <ostream>
using namespace std;
namespace gtsam {
@ -29,47 +29,35 @@ namespace gtsam {
//------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
const Vector3& bias, const Matrix3& measuredOmegaCovariance) :
biasHat_(bias), deltaTij_(0.0) {
measurementCovariance_ << measuredOmegaCovariance;
delRdelBiasOmega_.setZero();
PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias)
{
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
biasHat_(Vector3()), deltaTij_(0.0) {
measurementCovariance_.setZero();
delRdelBiasOmega_.setZero();
delRdelBiasOmega_.setZero();
PreintegratedRotation(I_3x3), biasHat_(Vector3())
{
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const {
std::cout << s << std::endl;
std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl;
deltaRij_.print(" deltaRij ");
std::cout << " measurementCovariance [" << measurementCovariance_ << " ]"
<< std::endl;
std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl;
void AHRSFactor::PreintegratedMeasurements::print(const string& s) const {
PreintegratedRotation::print(s);
cout << "biasHat [" << biasHat_.transpose() << "]" << endl;
cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl;
}
//------------------------------------------------------------------------------
bool AHRSFactor::PreintegratedMeasurements::equals(
const PreintegratedMeasurements& other, double tol) const {
return equal_with_abs_tol(biasHat_, other.biasHat_, tol)
&& equal_with_abs_tol(measurementCovariance_,
other.measurementCovariance_, tol)
&& deltaRij_.equals(other.deltaRij_, tol)
&& std::fabs(deltaTij_ - other.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
return PreintegratedRotation::equals(other, tol)
&& equal_with_abs_tol(biasHat_, other.biasHat_, tol);
}
//------------------------------------------------------------------------------
void AHRSFactor::PreintegratedMeasurements::resetIntegration() {
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delRdelBiasOmega_.setZero();
PreintegratedRotation::resetIntegration();
preintMeasCov_.setZero();
}
@ -78,7 +66,6 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor) {
// NOTE: order is important here because each update uses old values.
// First we compensate the measurements for the bias
Vector3 correctedOmega = measuredOmega - biasHat_;
@ -93,64 +80,27 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
// rotation vector describing rotation increment computed from the
// current rotation rate measurement
const Vector3 theta_incr = correctedOmega * deltaT;
Matrix3 D_Rincr_integratedOmega;
const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !!
// rotation increment computed from the current rotation rate measurement
const Rot3 incrR = Rot3::Expmap(theta_incr);
const Matrix3 incrRt = incrR.transpose();
// Update Jacobian
update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
// Right Jacobian computed at theta_incr
const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr);
// Update Jacobians
// ---------------------------------------------------------------------------
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
// Update preintegrated measurements covariance
// ---------------------------------------------------------------------------
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3)
const Matrix3 Jr_theta_i = Rot3::LogmapDerivative(theta_i);
Rot3 Rot_j = deltaRij_ * incrR;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j);
// Update preintegrated measurements covariance: as in [2] we consider a first
// order propagation that can be seen as a prediction phase in an EKF framework
Matrix3 H_angles_angles = Jrinv_theta_j * incrRt * Jr_theta_i;
// analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>
// (boost::bind(&DeltaAngles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrpt preintegrated measurements (df/dx)
const Matrix3& F = H_angles_angles;
// Update rotation and deltaTij.
Matrix3 Fr; // Jacobian of the update
updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr);
// first order uncertainty propagation
// the deltaT allows to pass from continuous time noise to discrete time noise
preintMeasCov_ = F * preintMeasCov_ * F.transpose()
+ measurementCovariance_ * deltaT;
// Update preintegrated measurements
// ---------------------------------------------------------------------------
deltaRij_ = deltaRij_ * incrR;
deltaTij_ += deltaT;
preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose()
+ gyroscopeCovariance() * deltaT;
}
//------------------------------------------------------------------------------
Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias,
boost::optional<Matrix&> H) const {
const Vector3 biasOmegaIncr = bias - biasHat_;
Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr;
const Rot3 deltaRij_biascorrected = deltaRij_.retract(
delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP);
const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
if (H) {
const Matrix3 Jrinv_theta_bc = //
Rot3::LogmapDerivative(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::ExpmapDerivative(delRdelBiasOmega_biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
}
return theta_biascorrected;
return biascorrectedThetaRij(biasOmegaIncr, H);
}
//------------------------------------------------------------------------------
Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
@ -172,7 +122,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
// AHRSFactor methods
//------------------------------------------------------------------------------
AHRSFactor::AHRSFactor() :
preintegratedMeasurements_(Vector3(), Matrix3::Zero()) {
_PIM_(Vector3(), Z_3x3) {
}
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
@ -180,7 +130,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) :
Base(
noiseModel::Gaussian::Covariance(
preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_(
preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_(
preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor) {
}
@ -192,13 +142,12 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const {
}
//------------------------------------------------------------------------------
void AHRSFactor::print(const std::string& s,
void AHRSFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
preintegratedMeasurements_.print(" preintegrated measurements:");
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
<< std::endl;
_PIM_.print(" preintegrated measurements:");
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
noiseModel_->print(" noise model: ");
if (body_P_sensor_)
body_P_sensor_->print(" sensor pose in body frame: ");
@ -207,8 +156,7 @@ void AHRSFactor::print(const std::string& s,
//------------------------------------------------------------------------------
bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
const This *e = dynamic_cast<const This*>(&other);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_)
|| (body_P_sensor_ && e->body_P_sensor_
@ -216,50 +164,49 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
}
//------------------------------------------------------------------------------
Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j,
Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
const Vector3& bias, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3) const {
// Do bias correction, if (H3) will contain 3*3 derivative used below
const Vector3 theta_biascorrected = //
preintegratedMeasurements_.predict(bias, H3);
const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3);
// Coriolis term
const Vector3 coriolis = //
preintegratedMeasurements_.integrateCoriolis(rot_i, omegaCoriolis_);
const Vector3 theta_corrected = theta_biascorrected - coriolis;
const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_);
const Matrix3 coriolisHat = skewSymmetric(coriolis);
const Vector3 correctedOmega = biascorrectedOmega - coriolis;
// Prediction
const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected);
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
// Get error between actual and prediction
const Rot3 actualRij = rot_i.between(rot_j);
const Rot3 fRhat = deltaRij_corrected.between(actualRij);
Vector3 fR = Rot3::Logmap(fRhat);
const Rot3 actualRij = Ri.between(Rj);
const Rot3 fRrot = correctedDeltaRij.between(actualRij);
Vector3 fR = Rot3::Logmap(fRrot);
// Terms common to derivatives
const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_corrected);
const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(fR);
const Matrix3 D_cDeltaRij_cOmega = Rot3::ExpmapDerivative(correctedOmega);
const Matrix3 D_fR_fRrot = Rot3::LogmapDerivative(fR);
if (H1) {
// dfR/dRi
H1->resize(3, 3);
Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis);
Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat;
(*H1)
<< Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta);
<< D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis);
}
if (H2) {
// dfR/dPosej
H2->resize(3, 3);
(*H2) << Jrinv_fRhat * Matrix3::Identity();
(*H2) << D_fR_fRrot * Matrix3::Identity();
}
if (H3) {
// dfR/dBias, note H3 contains derivative of predict
const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3);
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H3);
H3->resize(3, 3);
(*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega);
(*H3) << D_fR_fRrot * (-fRrot.transpose() * JbiasOmega);
}
Vector error(3);
@ -272,16 +219,16 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) {
const Vector3 theta_biascorrected = preintegratedMeasurements.predict(bias);
const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias);
// Coriolis term
const Vector3 coriolis = //
preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis);
const Vector3 theta_corrected = theta_biascorrected - coriolis;
const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected);
const Vector3 correctedOmega = biascorrectedOmega - coriolis;
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
return rot_i.compose(deltaRij_corrected);
return rot_i.compose(correctedDeltaRij);
}
} //namespace gtsam

51
gtsam/navigation/AHRSFactor.h
View File

@ -20,6 +20,7 @@
#pragma once
/* GTSAM includes */
#include <gtsam/navigation/PreintegratedRotation.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
@ -35,17 +36,12 @@ public:
* Can be built incrementally so as to avoid costly integration at time of
* factor construction.
*/
class PreintegratedMeasurements {
class PreintegratedMeasurements : public PreintegratedRotation {
friend class AHRSFactor;
protected:
Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3)
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
public:
@ -61,31 +57,19 @@ public:
PreintegratedMeasurements(const Vector3& bias,
const Matrix3& measuredOmegaCovariance);
Vector3 biasHat() const {
return biasHat_;
}
const Matrix3& preintMeasCov() const {
return preintMeasCov_;
}
/// print
void print(const std::string& s = "Preintegrated Measurements: ") const;
/// equals
bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const;
const Matrix3& measurementCovariance() const {
return measurementCovariance_;
}
Matrix3 deltaRij() const {
return deltaRij_.matrix();
}
double deltaTij() const {
return deltaTij_;
}
Vector3 biasHat() const {
return biasHat_;
}
const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_;
}
const Matrix3& preintMeasCov() const {
return preintMeasCov_;
}
/// TODO: Document
void resetIntegration();
@ -103,12 +87,6 @@ public:
Vector3 predict(const Vector3& bias, boost::optional<Matrix&> H =
boost::none) const;
/// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i,
const Vector3& omegaCoriolis) const {
return rot_i.transpose() * omegaCoriolis * deltaTij_;
}
// This function is only used for test purposes
// (compare numerical derivatives wrt analytic ones)
static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
@ -120,11 +98,8 @@ public:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
}
};
@ -132,7 +107,7 @@ private:
typedef AHRSFactor This;
typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base;
PreintegratedMeasurements preintegratedMeasurements_;
PreintegratedMeasurements _PIM_;
Vector3 gravity_;
Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
@ -178,7 +153,7 @@ public:
/// Access the preintegrated measurements.
const PreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_;
return _PIM_;
}
const Vector3& omegaCoriolis() const {
@ -208,7 +183,7 @@ private:
ar
& boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}

485
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -36,197 +36,136 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) :
biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
deltaRij_(Rot3()), deltaTij_(0.0),
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
integrationErrorCovariance, use2ndOrderIntegration),
biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance),
biasAccOmegaInit_(biasAccOmegaInit)
{
measurementCovariance_.setZero();
measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
measurementCovariance_.block<3,3>(9,9) = biasAccCovariance;
measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance;
measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit;
PreintMeasCov_.setZero();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{
cout << s << endl;
biasHat_.print(" biasHat");
cout << " deltaTij " << deltaTij_ << endl;
cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
deltaRij_.print(" deltaRij ");
cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl;
cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl;
PreintegrationBase::print(s);
cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl;
cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl;
cout << " biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl;
cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
}
//------------------------------------------------------------------------------
bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{
return biasHat_.equals(expected.biasHat_, tol)
&& equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
&& deltaRij_.equals(expected.deltaRij_, tol)
&& fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, tol)
&& equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, tol)
&&equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol)
&& equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
&& PreintegrationBase::equals(expected, tol);
}
//------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){
deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero();
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delPdelBiasAcc_ = Z_3x3;
delPdelBiasOmega_ = Z_3x3;
delVdelBiasAcc_ = Z_3x3;
delVdelBiasOmega_ = Z_3x3;
delRdelBiasOmega_ = Z_3x3;
PreintMeasCov_.setZero();
PreintegrationBase::resetIntegration();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega,
double deltaT, boost::optional<const Pose3&> body_P_sensor) {
double deltaT, boost::optional<const Pose3&> body_P_sensor,
boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
// (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
// First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty
Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
if(body_P_sensor){
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
// linear acceleration vector in the body frame
}
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr
const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
}else{
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
}
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
// can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
// consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i);
Rot3 Rot_j = deltaRij_ * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j);
const Matrix3 R_i = deltaRij(); // store this
// Update preintegrated measurements. TODO Frank moved from end of this function !!!
Matrix9 F_9x9;
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9);
// Single Jacobians to propagate covariance
Matrix3 H_pos_pos = I_3x3;
Matrix3 H_pos_vel = I_3x3 * deltaT;
Matrix3 H_pos_angles = Z_3x3;
Matrix3 H_vel_pos = Z_3x3;
Matrix3 H_vel_vel = I_3x3;
Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT;
Matrix3 H_angles_pos = Z_3x3;
Matrix3 H_angles_vel = Z_3x3;
Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
// analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
Matrix3 H_vel_biasacc = - R_i * deltaT;
Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT;
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15,15);
F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3,
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3,
H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega,
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
// for documentation:
// F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3,
// Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3,
// Z_3x3, Z_3x3, H_angles_angles, Z_3x3, H_angles_biasomega,
// Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
// Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
F.setZero();
F.block<9,9>(0,0) = F_9x9;
F.block<6,6>(9,9) = I_6x6;
F.block<3,3>(3,9) = H_vel_biasacc;
F.block<3,3>(6,12) = H_angles_biasomega;
// first order uncertainty propagation
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
Matrix G_measCov_Gt = Matrix::Zero(15,15);
// BLOCK DIAGONAL TERMS
G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0);
// BLOCK DIAGONAL TERMS
G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance();
G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) *
(measurementCovariance_.block<3,3>(3,3) + measurementCovariance_.block<3,3>(15,15) ) *
(accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) *
(H_vel_biasacc.transpose());
G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) *
(measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) *
(gyroscopeCovariance() + biasAccOmegaInit_.block<3,3>(3,3) ) *
(H_angles_biasomega.transpose());
G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9);
G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12);
// NEW OFF BLOCK DIAGONAL TERMS
Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose();
G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_;
G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_;
// OFF BLOCK DIAGONAL TERMS
Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) * H_angles_biasomega.transpose();
G_measCov_Gt.block<3,3>(3,6) = block23;
G_measCov_Gt.block<3,3>(6,3) = block23.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt;
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){
deltaPij_ += deltaVij_ * deltaT;
}else{
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
// F_test and G_test are used for testing purposes and are not needed by the factor
if(F_test){
F_test->resize(15,15);
(*F_test) << F;
}
if(G_test){
G_test->resize(15,21);
// This is for testing & documentation
///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3,
Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3,
Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega,
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
}
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
deltaRij_ = deltaRij_ * Rincr;
deltaTij_ += deltaT;
}
//------------------------------------------------------------------------------
// CombinedImuFactor methods
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor() :
preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6) {}
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor),
use2ndOrderCoriolis_(use2ndOrderCoriolis){
}
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
_PIM_(preintegratedMeasurements) {}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
@ -243,22 +182,17 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter)
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ","
<< keyFormatter(this->key6()) << ")\n";
preintegratedMeasurements_.print(" preintegrated measurements:");
cout << " gravity: [ " << gravity_.transpose() << " ]" << endl;
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
ImuFactorBase::print("");
_PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: ");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
//------------------------------------------------------------------------------
bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
&& _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol);
}
//------------------------------------------------------------------------------
@ -268,230 +202,69 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
const double& deltaTij = preintegratedMeasurements_.deltaTij_;
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
// if we need the jacobians
if(H1 || H2 || H3 || H4 || H5 || H6){
Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R)
// we give some shorter name to rotations and translations
const Rot3 Rot_i = pose_i.rotation();
const Rot3 Rot_j = pose_j.rotation();
const Vector3 pos_i = pose_i.translation().vector();
const Vector3 pos_j = pose_j.translation().vector();
// error wrt preintegrated measurements
Vector r_pvR(9);
r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
gravity_, omegaCoriolis_, use2ndOrderCoriolis_, //
H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR);
// We compute factor's Jacobians, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
// error wrt bias evolution model (random walk)
Vector6 fbias = bias_j.between(bias_i, Hbias_j, Hbias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr]
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected);
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat));
if(H1) {
H1->resize(15,6);
Matrix3 dfPdPi;
Matrix3 dfVdPi;
if(use2ndOrderCoriolis_){
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
if(H1) {
H1->resize(15,6);
H1->block<9,6>(0,0) = H1_pvR;
// adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
H1->block<6,6>(9,0) = Z_6x6;
}
else{
dfPdPi = - Rot_i.matrix();
dfVdPi = Z_3x3;
if(H2) {
H2->resize(15,3);
H2->block<9,3>(0,0) = H2_pvR;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H2->block<6,3>(9,0) = Matrix::Zero(6,3);
}
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
// dfP/dPi
dfPdPi,
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
// dfV/dPi
dfVdPi,
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Z_3x3,
//dBiasAcc/dPi
Z_3x3, Z_3x3,
//dBiasOmega/dPi
Z_3x3, Z_3x3;
if(H3) {
H3->resize(15,6);
H3->block<9,6>(0,0) = H3_pvR;
// adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
H3->block<6,6>(9,0) = Z_6x6;
}
if(H4) {
H4->resize(15,3);
H4->block<9,3>(0,0) = H4_pvR;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H4->block<6,3>(9,0) = Matrix::Zero(6,3);
}
if(H5) {
H5->resize(15,6);
H5->block<9,6>(0,0) = H5_pvR;
// adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
H5->block<6,6>(9,0) = Hbias_i;
}
if(H6) {
H6->resize(15,6);
H6->block<9,6>(0,0) = Matrix::Zero(9,6);
// adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
H6->block<6,6>(9,0) = Hbias_j;
}
Vector r(15); r << r_pvR, fbias; // vector of size 15
return r;
}
if(H2) {
H2->resize(15,3);
(*H2) <<
// dfP/dVi
- I_3x3 * deltaTij
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
- I_3x3
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Z_3x3,
//dBiasAcc/dVi
Z_3x3,
//dBiasOmega/dVi
Z_3x3;
}
if(H3) {
H3->resize(15,6);
(*H3) <<
// dfP/dPosej
Z_3x3, Rot_j.matrix(),
// dfV/dPosej
Matrix::Zero(3,6),
// dfR/dPosej
Jrinv_fRhat * ( I_3x3 ), Z_3x3,
//dBiasAcc/dPosej
Z_3x3, Z_3x3,
//dBiasOmega/dPosej
Z_3x3, Z_3x3;
}
if(H4) {
H4->resize(15,3);
(*H4) <<
// dfP/dVj
Z_3x3,
// dfV/dVj
I_3x3,
// dfR/dVj
Z_3x3,
//dBiasAcc/dVj
Z_3x3,
//dBiasOmega/dVj
Z_3x3;
}
if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H5->resize(15,6);
(*H5) <<
// dfP/dBias_i
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
// dfV/dBias_i
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
// dfR/dBias_i
Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
//dBiasAcc/dBias_i
-I_3x3, Z_3x3,
//dBiasOmega/dBias_i
Z_3x3, -I_3x3;
}
if(H6) {
H6->resize(15,6);
(*H6) <<
// dfP/dBias_j
Z_3x3, Z_3x3,
// dfV/dBias_j
Z_3x3, Z_3x3,
// dfR/dBias_j
Z_3x3, Z_3x3,
//dBiasAcc/dBias_j
I_3x3, Z_3x3,
//dBiasOmega/dBias_j
Z_3x3, I_3x3;
}
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 fp =
pos_j - pos_i
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
+ preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
- vel_i * deltaTij
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
- 0.5 * gravity_ * deltaTij*deltaTij;
const Vector3 fv =
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
- gravity_ * deltaTij;
const Vector3 fR = Rot3::Logmap(fRhat);
const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer();
const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope();
Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15
// else, only compute the error vector:
// error wrt preintegrated measurements
Vector r_pvR(9);
r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
gravity_, omegaCoriolis_, use2ndOrderCoriolis_, //
boost::none, boost::none, boost::none, boost::none, boost::none);
// error wrt bias evolution model (random walk)
Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr]
// overall error
Vector r(15); r << r_pvR, fbias; // vector of size 15
return r;
}
//------------------------------------------------------------------------------
PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
const double& deltaTij = preintegratedMeasurements.deltaTij_;
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
const Rot3 Rot_i = pose_i.rotation();
const Vector3 pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
+ preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
if(use2ndOrderCoriolis){
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
}
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
return PoseVelocityBias(pose_j, vel_j, bias_i);
}
} /// namespace gtsam

171
gtsam/navigation/CombinedImuFactor.h
View File

@ -23,7 +23,8 @@
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/navigation/ImuFactorBase.h>
#include <gtsam/base/debug.h>
namespace gtsam {
@ -33,78 +34,63 @@ 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.
*/
/**
* Struct to hold all state variables of CombinedImuFactor returned by Predict function
* 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.
*/
struct PoseVelocityBias {
Pose3 pose;
Vector3 velocity;
imuBias::ConstantBias bias;
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
const imuBias::ConstantBias _bias) :
pose(_pose), velocity(_velocity), bias(_bias) {
}
};
/**
* 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.
*/
class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
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 {
class CombinedPreintegratedMeasurements: public PreintegrationBase {
friend class CombinedImuFactor;
protected:
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
Eigen::Matrix<double,21,21> measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j
Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk
Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Eigen::Matrix<double,15,15> PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements
Eigen::Matrix<double,15,15> preintMeasCov_; ///< Covariance matrix of the preintegrated measurements
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation
///< between the preintegrated measurements and the biases
bool use2ndOrderIntegration_; ///< Controls the order of integration
public:
/**
@ -141,60 +127,20 @@ public:
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
*/
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none);
boost::optional<const Pose3&> body_P_sensor = boost::none,
boost::optional<Matrix&> F_test = boost::none, boost::optional<Matrix&> G_test = boost::none);
/// methods to access class variables
Matrix measurementCovariance() const {return measurementCovariance_;}
Matrix deltaRij() const {return deltaRij_.matrix();}
double deltaTij() const{return deltaTij_;}
Vector deltaPij() const {return deltaPij_;}
Vector deltaVij() const {return deltaVij_;}
Vector biasHat() const { return biasHat_.vector();}
Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
Matrix PreintMeasCov() const { return PreintMeasCov_;}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
const Vector3& delta_angles, const Vector& delta_vel_in_t0){
// Note: all delta terms refer to an IMU\sensor system at t0
Vector body_t_a_body = msr_acc_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles){
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Vector body_t_omega_body= msr_gyro_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
return Rot3::Logmap(R_t_to_t0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
Matrix preintMeasCov() const { return preintMeasCov_;}
private:
/** Serialization function */
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
ar & BOOST_SERIALIZATION_NVP(deltaPij_);
ar & BOOST_SERIALIZATION_NVP(deltaVij_);
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};
@ -203,12 +149,7 @@ private:
typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
CombinedPreintegratedMeasurements _PIM_;
public:
@ -257,11 +198,7 @@ public:
/** Access the preintegrated measurements. */
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
return _PIM_; }
/** implement functions needed to derive from Factor */
@ -275,12 +212,6 @@ public:
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> H6 = boost::none) const;
/// predicted states from IMU
static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
private:
/** Serialization function */
@ -289,7 +220,7 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor6",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);

387
gtsam/navigation/ImuFactor.cpp
View File

@ -35,165 +35,82 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration) :
biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
deltaRij_(Rot3()), deltaTij_(0.0),
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
PreintegrationBase(bias,
measuredAccCovariance, measuredOmegaCovariance,
integrationErrorCovariance, use2ndOrderIntegration)
{
measurementCovariance_.setZero();
measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
PreintMeasCov_.setZero(9,9);
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
cout << s << endl;
biasHat_.print(" biasHat");
cout << " deltaTij " << deltaTij_ << endl;
cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
deltaRij_.print(" deltaRij ");
cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl;
cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl;
PreintegrationBase::print(s);
cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl;
}
//------------------------------------------------------------------------------
bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const {
return biasHat_.equals(expected.biasHat_, tol)
&& equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
&& deltaRij_.equals(expected.deltaRij_, tol)
&& fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
&& PreintegrationBase::equals(expected, tol);
}
//------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::resetIntegration(){
deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero();
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delPdelBiasAcc_ = Z_3x3;
delPdelBiasOmega_ = Z_3x3;
delVdelBiasAcc_ = Z_3x3;
delVdelBiasOmega_ = Z_3x3;
delRdelBiasOmega_ = Z_3x3;
PreintMeasCov_.setZero();
PreintegrationBase::resetIntegration();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor) {
boost::optional<const Pose3&> body_P_sensor,
OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
// NOTE: order is important here because each update uses old values (i.e., we have to update
// jacobians and covariances before updating preintegrated measurements).
Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
// First we compensate the measurements for the bias
Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
if(body_P_sensor){
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
// linear acceleration vector in the body frame
}
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr
const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
}else{
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
}
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
// Update preintegrated measurements covariance
// Update preintegrated measurements (also get Jacobian)
const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
// first order covariance propagation:
// as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
/* ----------------------------------------------------------------------------------------------------------------------- */
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i);
Rot3 Rot_j = deltaRij_ * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j);
Matrix H_pos_pos = I_3x3;
Matrix H_pos_vel = I_3x3 * deltaT;
Matrix H_pos_angles = Z_3x3;
Matrix H_vel_pos = Z_3x3;
Matrix H_vel_vel = I_3x3;
Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3;
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
// analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(9,9);
F << H_pos_pos, H_pos_vel, H_pos_angles,
H_vel_pos, H_vel_vel, H_vel_angles,
H_angles_pos, H_angles_vel, H_angles_angles;
// first order uncertainty propagation:
// the deltaT allows to pass from continuous time noise to discrete time noise
// preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
// NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
// Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
// NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise,
// as G and measurementCovariance are blockdiagonal matrices
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT;
preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT;
preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
// This in only kept for documentation.
//
// Matrix G(9,9);
// G << I_3x3 * deltaT, Z_3x3, Z_3x3,
// Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
// Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
//
// PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
// Update preintegrated measurements (this has to be done after the update of covariances and jacobians!)
/* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){
deltaPij_ += deltaVij_ * deltaT;
}else{
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
// F_test and G_test are given as output for testing purposes and are not needed by the factor
if(F_test){ // This in only for testing
(*F_test) << F;
}
if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise
// intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
}
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
deltaRij_ = deltaRij_ * Rincr;
deltaTij_ += deltaT;
}
//------------------------------------------------------------------------------
// ImuFactor methods
//------------------------------------------------------------------------------
ImuFactor::ImuFactor() :
preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){}
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {}
//------------------------------------------------------------------------------
ImuFactor::ImuFactor(
@ -202,13 +119,10 @@ ImuFactor::ImuFactor(
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor,
const bool use2ndOrderCoriolis) :
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor),
use2ndOrderCoriolis_(use2ndOrderCoriolis){
}
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_),
pose_i, vel_i, pose_j, vel_j, bias),
ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
_PIM_(preintegratedMeasurements) {}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
@ -224,215 +138,28 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ")\n";
preintegratedMeasurements_.print(" preintegrated measurements:");
cout << " gravity: [ " << gravity_.transpose() << " ]" << endl;
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
ImuFactorBase::print("");
_PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: ");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
//------------------------------------------------------------------------------
bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
&& _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol);
}
//------------------------------------------------------------------------------
Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
boost::optional<Matrix&> H5) const
{
Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const {
const double& deltaTij = preintegratedMeasurements_.deltaTij_;
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
// we give some shorter name to rotations and translations
const Rot3 Rot_i = pose_i.rotation();
const Rot3 Rot_j = pose_j.rotation();
const Vector3 pos_i = pose_i.translation().vector();
const Vector3 pos_j = pose_j.translation().vector();
// We compute factor's Jacobians
/* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected);
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat));
if(H1) {
H1->resize(9,6);
Matrix3 dfPdPi;
Matrix3 dfVdPi;
if(use2ndOrderCoriolis_){
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
}
else{
dfPdPi = - Rot_i.matrix();
dfVdPi = Z_3x3;
}
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
// dfP/dPi
dfPdPi,
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
// dfV/dPi
dfVdPi,
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Z_3x3;
}
if(H2) {
H2->resize(9,3);
(*H2) <<
// dfP/dVi
- I_3x3 * deltaTij
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
- I_3x3
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Z_3x3;
}
if(H3) {
H3->resize(9,6);
(*H3) <<
// dfP/dPosej
Z_3x3, Rot_j.matrix(),
// dfV/dPosej
Matrix::Zero(3,6),
// dfR/dPosej
Jrinv_fRhat * ( I_3x3 ), Z_3x3;
}
if(H4) {
H4->resize(9,3);
(*H4) <<
// dfP/dVj
Z_3x3,
// dfV/dVj
I_3x3,
// dfR/dVj
Z_3x3;
}
if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H5->resize(9,6);
(*H5) <<
// dfP/dBias
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
// dfV/dBias
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
// dfR/dBias
Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
}
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 fp =
pos_j - pos_i
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
+ preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
- vel_i * deltaTij
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
- 0.5 * gravity_ * deltaTij*deltaTij;
const Vector3 fv =
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
- gravity_ * deltaTij;
const Vector3 fR = Rot3::Logmap(fRhat);
Vector r(9); r << fp, fv, fR;
return r;
}
//------------------------------------------------------------------------------
PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis)
{
const double& deltaTij = preintegratedMeasurements.deltaTij_;
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
const Rot3 Rot_i = pose_i.rotation();
const Vector3 pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
+ preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
if(use2ndOrderCoriolis){
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
}
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
return PoseVelocity(pose_j, vel_j);
return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5);
}
} /// namespace gtsam

302
gtsam/navigation/ImuFactor.h
View File

@ -23,7 +23,8 @@
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/navigation/ImuFactorBase.h>
#include <gtsam/base/debug.h>
namespace gtsam {
@ -38,66 +39,46 @@ 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.
*/
/**
* Struct to hold return variables by the Predict Function
* 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.
*/
struct PoseVelocity {
Pose3 pose;
Vector3 velocity;
PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
pose(_pose), velocity(_velocity) {
}
};
/**
* 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.
*/
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuFactorBase {
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 {
class PreintegratedMeasurements: public PreintegrationBase {
friend class ImuFactor;
protected:
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Eigen::Matrix<double,9,9> PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
Eigen::Matrix<double,9,9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
///< (first-order propagation from *measurementCovariance*).
bool use2ndOrderIntegration_; ///< Controls the order of integration
public:
public:
/**
* Default constructor, initializes the class with no measurements
@ -127,160 +108,107 @@ public:
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param deltaT Time interval between two consecutive IMU measurements
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
* @param Fout, Gout Jacobians used internally (only needed for testing)
*/
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none);
boost::optional<const Pose3&> body_P_sensor = boost::none,
OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none);
/// methods to access class variables
Matrix measurementCovariance() const {return measurementCovariance_;}
Matrix deltaRij() const {return deltaRij_.matrix();}
double deltaTij() const{return deltaTij_;}
Vector deltaPij() const {return deltaPij_;}
Vector deltaVij() const {return deltaVij_;}
Vector biasHat() const { return biasHat_.vector();}
Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
Matrix preintMeasCov() const { return PreintMeasCov_;}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
const Vector3& delta_angles, const Vector& delta_vel_in_t0){
// Note: all delta terms refer to an IMU\sensor system at t0
Vector body_t_a_body = msr_acc_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles){
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Vector body_t_omega_body= msr_gyro_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
return Rot3::Logmap(R_t_to_t0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
ar & BOOST_SERIALIZATION_NVP(deltaPij_);
ar & BOOST_SERIALIZATION_NVP(deltaVij_);
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
}
};
Matrix preintMeasCov() const { return preintMeasCov_;}
private:
typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
PreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
public:
/** Shorthand for a smart pointer to a factor */
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<ImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
ImuFactor();
/**
* Constructor
* @param pose_i Previous pose key
* @param vel_i Previous velocity key
* @param pose_j Current pose key
* @param vel_j Current velocity key
* @param bias Previous bias key
* @param preintegratedMeasurements Preintegrated IMU measurements
* @param gravity Gravity vector expressed in the global frame
* @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
* @param body_P_sensor Optional pose of the sensor frame in the body frame
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
*/
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false);
virtual ~ImuFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
/** implement functions needed for Testable */
/// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// equals
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
/** Access the preintegrated measurements. */
const PreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
/** implement functions needed to derive from Factor */
/// vector of errors
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const;
/// predicted states from IMU
static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
private:
/** Serialization function */
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
}; // class ImuFactor
};
typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;
private:
typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
PreintegratedMeasurements _PIM_;
public:
/** Shorthand for a smart pointer to a factor */
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<ImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
ImuFactor();
/**
* Constructor
* @param pose_i Previous pose key
* @param vel_i Previous velocity key
* @param pose_j Current pose key
* @param vel_j Current velocity key
* @param bias Previous bias key
* @param preintegratedMeasurements Preintegrated IMU measurements
* @param gravity Gravity vector expressed in the global frame
* @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
* @param body_P_sensor Optional pose of the sensor frame in the body frame
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
*/
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false);
virtual ~ImuFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
/** implement functions needed for Testable */
/// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// equals
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
/** Access the preintegrated measurements. */
const PreintegratedMeasurements& preintegratedMeasurements() const {
return _PIM_; }
/** implement functions needed to derive from Factor */
/// vector of errors
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // class ImuFactor
typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;
} /// namespace gtsam

84
gtsam/navigation/ImuFactorBase.h Normal file
View File

@ -0,0 +1,84 @@
/* ----------------------------------------------------------------------------
* 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 PreintegrationBase.h
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#pragma once
/* GTSAM includes */
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/PreintegrationBase.h>
namespace gtsam {
class ImuFactorBase {
protected:
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
public:
/** Default constructor - only use for serialization */
ImuFactorBase() :
gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)),
body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {}
/**
* Default constructor, stores basic quantities required by the Imu factors
* @param gravity Gravity vector expressed in the global frame
* @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
* @param body_P_sensor Optional pose of the sensor frame in the body frame
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
*/
ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) :
gravity_(gravity), omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {}
/// Methods to access class variables
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
/// Needed for testable
//------------------------------------------------------------------------------
void print(const std::string& s) const {
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl;
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
/// Needed for testable
//------------------------------------------------------------------------------
bool equals(const ImuFactorBase& expected, double tol) const {
return equal_with_abs_tol(gravity_, expected.gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol)
&& (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_)
&& ((!body_P_sensor_ && !expected.body_P_sensor_) ||
(body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_)));
}
};
} /// namespace gtsam

141
gtsam/navigation/PreintegratedRotation.h Normal file
View File

@ -0,0 +1,141 @@
/* ----------------------------------------------------------------------------
* 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 PreintegratedRotation.h
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/geometry/Rot3.h>
namespace gtsam {
/**
* PreintegratedRotation is the base class for all PreintegratedMeasurements
* classes (in AHRSFactor, ImuFactor, and CombinedImuFactor).
* It includes the definitions of the preintegrated rotation.
*/
class PreintegratedRotation {
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j
/// Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix3 delRdelBiasOmega_;
Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements
public:
/**
* Default constructor, initializes the variables in the base class
*/
PreintegratedRotation(const Matrix3& measuredOmegaCovariance) :
deltaRij_(Rot3()), deltaTij_(0.0),
delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {}
/// methods to access class variables
Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive
Vector3 thetaRij(boost::optional<Matrix3&> H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive
const double& deltaTij() const{return deltaTij_;}
const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;}
const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;}
/// Needed for testable
void print(const std::string& s) const {
std::cout << s << std::endl;
std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl;
deltaRij_.print(" deltaRij ");
std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl;
std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl;
}
/// Needed for testable
bool equals(const PreintegratedRotation& expected, double tol) const {
return deltaRij_.equals(expected.deltaRij_, tol)
&& fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol)
&& equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol);
}
/// Re-initialize PreintegratedMeasurements
void resetIntegration(){
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delRdelBiasOmega_ = Z_3x3;
}
/// Update preintegrated measurements
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
OptionalJacobian<3, 3> H = boost::none){
deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
deltaTij_ += deltaT;
}
/**
* Update Jacobians to be used during preintegration
* TODO: explain arguments
*/
void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
double deltaT) {
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT;
}
/// Return a bias corrected version of the integrated rotation - expensive
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const {
return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr);
}
/// Get so<3> version of bias corrected rotation, with optional Jacobian
// Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) )
Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H = boost::none) const {
// First, we correct deltaRij using the biasOmegaIncr, rotated
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
if (H) {
Matrix3 Jrinv_theta_bc;
// This was done via an expmap, now we go *back* to so<3>
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc);
const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
return biascorrectedOmega;
}
//else
return Rot3::Logmap(deltaRij_biascorrected);
}
/// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i,
const Vector3& omegaCoriolis) const {
return rot_i.transpose() * omegaCoriolis * deltaTij();
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
}
};
} /// namespace gtsam

425
gtsam/navigation/PreintegrationBase.h Normal file
View File

@ -0,0 +1,425 @@
/* ----------------------------------------------------------------------------
* 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 PreintegrationBase.h
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/navigation/PreintegratedRotation.h>
#include <gtsam/navigation/ImuBias.h>
namespace gtsam {
/**
* Struct to hold all state variables of returned by Predict function
*/
struct PoseVelocityBias {
Pose3 pose;
Vector3 velocity;
imuBias::ConstantBias bias;
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
const imuBias::ConstantBias _bias) :
pose(_pose), velocity(_velocity), bias(_bias) {
}
};
/**
* 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 : public PreintegratedRotation {
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
bool use2ndOrderIntegration_; ///< Controls the order of integration
Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements
Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty
/// (to compensate errors in Euler integration)
public:
/**
* Default constructor, initializes the variables in the base class
* @param bias Current estimate of acceleration and rotation rate biases
* @param use2ndOrderIntegration Controls the order of integration
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
*/
PreintegrationBase(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance,
const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration) :
PreintegratedRotation(measuredOmegaCovariance),
biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration),
deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
accelerometerCovariance_(measuredAccCovariance),
integrationCovariance_(integrationErrorCovariance) {}
/// methods to access class variables
const Vector3& deltaPij() const {return deltaPij_;}
const Vector3& deltaVij() const {return deltaVij_;}
const imuBias::ConstantBias& biasHat() const { return biasHat_;}
Vector6 biasHatVector() const { return biasHat_.vector();} // expensive
const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;}
const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;}
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;}
const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;}
const Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;}
const Matrix3& integrationCovariance() const { return integrationCovariance_;}
/// Needed for testable
void print(const std::string& s) const {
PreintegratedRotation::print(s);
std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl;
std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl;
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
biasHat_.print(" biasHat");
}
/// Needed for testable
bool equals(const PreintegrationBase& expected, double tol) const {
return PreintegratedRotation::equals(expected, tol)
&& biasHat_.equals(expected.biasHat_, tol)
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
&& equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol)
&& equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol);
}
/// Re-initialize PreintegratedMeasurements
void resetIntegration(){
PreintegratedRotation::resetIntegration();
deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero();
delPdelBiasAcc_ = Z_3x3;
delPdelBiasOmega_ = Z_3x3;
delVdelBiasAcc_ = Z_3x3;
delVdelBiasOmega_ = Z_3x3;
}
/// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc,
const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) {
Matrix3 dRij = deltaRij(); // expensive
Vector3 temp = dRij * correctedAcc * deltaT;
if(!use2ndOrderIntegration_){
deltaPij_ += deltaVij_ * deltaT;
}else{
deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
}
deltaVij_ += temp;
Matrix3 R_i, F_angles_angles;
if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0);
if(F){
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
// pos vel angle
*F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos
Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles;// angle
}
}
/// Update Jacobians to be used during preintegration
void updatePreintegratedJacobians(const Vector3& correctedAcc,
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){
Matrix3 dRij = deltaRij(); // expensive
Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega();
if (!use2ndOrderIntegration_) {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
} else {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5);
}
delVdelBiasAcc_ += -dRij * deltaT;
delVdelBiasOmega_ += temp;
update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT);
}
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
const Vector3& measuredOmega, Vector3& correctedAcc,
Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) {
correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
correctedOmega = biasHat_.correctGyroscope(measuredOmega);
// Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame
if(body_P_sensor){
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
// linear acceleration vector in the body frame
}
}
/// Predict state at time j
//------------------------------------------------------------------------------
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const {
const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
const Rot3& Rot_i = pose_i.rotation();
const Vector3& pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr;
if(deltaPij_biascorrected_out)// if desired, store this
*deltaPij_biascorrected_out = deltaPij_biascorrected;
Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected
+ vel_i * deltaTij()
- omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij()*deltaTij();
Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr;
if(deltaVij_biascorrected_out)// if desired, store this
*deltaVij_biascorrected_out = deltaVij_biascorrected;
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected
- 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term
+ gravity * deltaTij());
if(use2ndOrderCoriolis){
pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position
vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity
}
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
// deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
Vector3 correctedOmega = biascorrectedOmega -
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
const Rot3 correctedDeltaRij =
Rot3::Expmap( correctedOmega );
const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij );
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
}
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
//------------------------------------------------------------------------------
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
OptionalJacobian<9, 6> H1 = boost::none,
OptionalJacobian<9, 3> H2 = boost::none,
OptionalJacobian<9, 6> H3 = boost::none,
OptionalJacobian<9, 3> H4 = boost::none,
OptionalJacobian<9, 6> H5 = boost::none) const {
// We need the mismatch w.r.t. the biases used for preintegration
// const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
// we give some shorter name to rotations and translations
const Rot3& Ri = pose_i.rotation();
const Rot3& Rj = pose_j.rotation();
const Vector3& pos_j = pose_j.translation().vector();
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected);
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() );
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity );
// fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
// Jacobian computation
/* ---------------------------------------------------------------------------------------------------- */
// Get Get so<3> version of bias corrected rotation
// If H5 is asked for, we will need the Jacobian, which we store in H5
// H5 will then be corrected below to take into account the Coriolis effect
Matrix3 D_cThetaRij_biasOmegaIncr;
Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0);
// Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis);
const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
Vector3 correctedOmega = biascorrectedOmega - coriolis;
Rot3 correctedDeltaRij, fRrot;
Vector3 fR;
// Accessory matrix, used to build the jacobians
Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot;
// This is done to save computation: we ask for the jacobians only when they are needed
if(H1 || H2 || H3 || H4 || H5){
correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega);
// Residual rotation error
fRrot = correctedDeltaRij.between(Ri.between(Rj));
fR = Rot3::Logmap(fRrot, D_fR_fRrot);
D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
}else{
correctedDeltaRij = Rot3::Expmap( correctedOmega);
// Residual rotation error
fRrot = correctedDeltaRij.between(Ri.between(Rj));
fR = Rot3::Logmap(fRrot);
}
if(H1) {
H1->resize(9,6);
Matrix3 dfPdPi = -I_3x3;
Matrix3 dfVdPi = Z_3x3;
if(use2ndOrderCoriolis){
// this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose());
dfPdPi += 0.5 * temp * deltaTij()*deltaTij();
dfVdPi += temp * deltaTij();
}
(*H1) <<
// dfP/dRi
skewSymmetric(fp + deltaPij_biascorrected),
// dfP/dPi
dfPdPi,
// dfV/dRi
skewSymmetric(fv + deltaVij_biascorrected),
// dfV/dPi
dfVdPi,
// dfR/dRi
D_fR_fRrot * (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis),
// dfR/dPi
Z_3x3;
}
if(H2) {
H2->resize(9,3);
(*H2) <<
// dfP/dVi
- Ri.transpose() * deltaTij()
+ Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
- Ri.transpose()
+ 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term
// dfR/dVi
Z_3x3;
}
if(H3) {
H3->resize(9,6);
(*H3) <<
// dfP/dPosej
Z_3x3, Ri.transpose() * Rj.matrix(),
// dfV/dPosej
Matrix::Zero(3,6),
// dfR/dPosej
D_fR_fRrot * ( I_3x3 ), Z_3x3;
}
if(H4) {
H4->resize(9,3);
(*H4) <<
// dfP/dVj
Z_3x3,
// dfV/dVj
Ri.transpose(),
// dfR/dVj
Z_3x3;
}
if(H5) {
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
H5->resize(9,6);
(*H5) <<
// dfP/dBias
- delPdelBiasAcc(), - delPdelBiasOmega(),
// dfV/dBias
- delVdelBiasAcc(), - delVdelBiasOmega(),
// dfR/dBias
Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega);
}
Vector9 r; r << fp, fv, fR;
return r;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaPij_);
ar & BOOST_SERIALIZATION_NVP(deltaVij_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
}
};
class ImuBase {
protected:
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
public:
ImuBase() :
gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)),
body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {}
ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) :
gravity_(gravity), omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {}
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
};
} /// namespace gtsam

2
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -116,7 +116,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
/* ************************************************************************* */
TEST(AHRSFactor, Error) {
// Linearization point
Vector3 bias; // Bias
Vector3 bias(0.,0.,0.); // Bias
Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));

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

@ -39,19 +39,55 @@ using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
/* ************************************************************************* */
namespace {
/* ************************************************************************* */
// Auxiliary functions to test Jacobians F and G used for
// covariance propagation during preintegration
/* ************************************************************************* */
Vector updatePreintegratedMeasurementsTest(
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old,
const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration) {
ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT;
Vector3 deltaPij_new;
if(!use2ndOrderIntegration){
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
}else{
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
}
Vector3 deltaVij_new = deltaVij_old + temp;
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT);
Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more
imuBias::ConstantBias bias_new(bias_old);
Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector();
return result;
}
Rot3 updatePreintegratedMeasurementsRot(
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old,
const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration){
Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, deltaVij_old, deltaRij_old,
bias_old, correctedAcc, correctedOmega, deltaT, use2ndOrderIntegration);
return Rot3::Expmap(result.segment<3>(6));
}
// Auxiliary functions to test preintegrated Jacobians
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
/* ************************************************************************* */
CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
)
{
ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
Matrix3::Identity(), Matrix3::Identity());
const list<double>& deltaTs){
CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false);
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -59,7 +95,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
}
return result;
}
@ -67,20 +102,16 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
const list<double>& deltaTs){
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij();
measuredAccs, measuredOmegas, deltaTs).deltaPij();
}
Vector3 evaluatePreintegratedMeasurementsVelocity(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
const list<double>& deltaTs){
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij();
}
@ -89,9 +120,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
const list<double>& deltaTs){
return Rot3(evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaRij());
}
@ -101,7 +130,6 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
/* ************************************************************************* */
TEST( CombinedImuFactor, PreintegratedMeasurements )
{
//cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
// Linearization point
imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases
@ -120,28 +148,17 @@ TEST( CombinedImuFactor, PreintegratedMeasurements )
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6));
// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol));
// EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol));
// EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol));
// DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol);
EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol));
EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol));
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
}
/* ************************************************************************* */
TEST( CombinedImuFactor, ErrorWithBiases )
{
//cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl;
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
@ -157,50 +174,37 @@ TEST( CombinedImuFactor, ErrorWithBiases )
double deltaT = 1.0;
double tol = 1e-6;
// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
Matrix I6x6(6,6);
I6x6 = Matrix::Identity(6,6);
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 );
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 );
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2);
Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2);
// Expected Jacobians
Matrix H1e, H2e, H3e, H4e, H5e;
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
// Expected Jacobians
Matrix H1e, H2e, H3e, H4e, H5e;
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
// Actual Jacobians
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a, H6a;
(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a);
@ -214,7 +218,6 @@ TEST( CombinedImuFactor, ErrorWithBiases )
/* ************************************************************************* */
TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
{
//cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
@ -237,22 +240,22 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
}
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs);
// Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
@ -265,6 +268,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
/* ************************************************************************* */
TEST(CombinedImuFactor, PredictPositionAndVelocity){
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
@ -283,22 +287,21 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){
for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; expectedVelocity<<0,1,0;
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
// Create factor
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; expectedVelocity<<0,1,0;
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
}
/* ************************************************************************* */
TEST(CombinedImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
Matrix I6x6(6,6);
@ -319,14 +322,152 @@ TEST(CombinedImuFactor, PredictRotation) {
// Predict
Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0));
Vector3 v(0,0,0);
PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis);
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0));
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol));
}
#include <gtsam/linear/GaussianFactorGraph.h>
/* ************************************************************************* */
TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
{
// Linearization point
imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3();
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for(int i=1;i<100;i++)
{
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
deltaTs.push_back(0.01);
}
// Actual preintegrated values
CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, deltaTs);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
body_P_sensor, Factual, Gactual);
bool use2ndOrderIntegration = false;
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected F wrt positions (15,3)
Matrix df_dpos =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
_1, deltaVij_old, deltaRij_old, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
// rotation part has to be done properly, on manifold
df_dpos.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
_1, deltaVij_old, deltaRij_old, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
// Compute expected F wrt velocities (15,3)
Matrix df_dvel =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, _1, deltaRij_old, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
// rotation part has to be done properly, on manifold
df_dvel.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, _1, deltaRij_old, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
// Compute expected F wrt angles (15,3)
Matrix df_dangle = numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, _1, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
// rotation part has to be done properly, on manifold
df_dangle.block<3,3>(6,0) = numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, deltaVij_old, _1, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
// Compute expected F wrt biases (15,6)
Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, deltaRij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
// rotation part has to be done properly, on manifold
df_dbias.block<3,6>(6,0) = numericalDerivative11<Rot3, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, deltaVij_old, deltaRij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
Matrix Fexpected(15,15);
Fexpected << df_dpos, df_dvel, df_dangle, df_dbias;
EXPECT(assert_equal(Fexpected, Factual));
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR G
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected G wrt integration noise
Matrix df_dintNoise(15,3);
df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3;
// Compute expected G wrt acc noise (15,3)
Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
// rotation part has to be done properly, on manifold
df_daccNoise.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
// Compute expected G wrt gyro noise (15,3)
Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
// rotation part has to be done properly, on manifold
df_domegaNoise.block<3,3>(6,0) = numericalDerivative11< Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
// Compute expected G wrt bias random walk noise (15,6)
Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries
df_rwBias.setZero();
df_rwBias.block<6,6>(9,0) = eye(6);
// Compute expected G wrt gyro noise (15,6)
Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, deltaRij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
// rotation part has to be done properly, on manifold
df_dinitBias.block<3,6>(6,0) = numericalDerivative11<Rot3, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, deltaVij_old, deltaRij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows
Matrix Gexpected(15,21);
Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() +
(1/newDeltaT) * Gactual * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

369
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -37,6 +37,8 @@ using symbol_shorthand::B;
/* ************************************************************************* */
namespace {
// Auxiliary functions to test evaluate error in ImuFactor
/* ************************************************************************* */
Vector callEvaluateError(const ImuFactor& factor,
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias){
@ -49,14 +51,48 @@ Rot3 evaluateRotationError(const ImuFactor& factor,
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
}
// Auxiliary functions to test Jacobians F and G used for
// covariance propagation during preintegration
/* ************************************************************************* */
Vector updatePreintegratedPosVel(
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration_) {
Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * correctedAcc * deltaT;
Vector3 deltaPij_new;
if(!use2ndOrderIntegration_){
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
}else{
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
}
Vector3 deltaVij_new = deltaVij_old + temp;
Vector result(6); result << deltaPij_new, deltaVij_new;
return result;
}
Rot3 updatePreintegratedRot(const Rot3& deltaRij_old,
const Vector3& correctedOmega, const double deltaT) {
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT);
return deltaRij_new;
}
// Auxiliary functions to test preintegrated Jacobians
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
/* ************************************************************************* */
double accNoiseVar = 0.01;
double omegaNoiseVar = 0.03;
double intNoiseVar = 0.0001;
ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
Matrix3::Identity(), Matrix3::Identity());
ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(),
omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity());
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -152,7 +188,7 @@ TEST( ImuFactor, PreintegratedMeasurements )
}
/* ************************************************************************* */
TEST( ImuFactor, Error )
TEST( ImuFactor, ErrorAndJacobians )
{
// Linearization point
imuBias::ConstantBias bias; // Bias
@ -180,6 +216,77 @@ TEST( ImuFactor, Error )
Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a;
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
// Expected Jacobians
/////////////////// H1 ///////////////////////////
Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
// Jacobians are around zero, so the rotation part is the same as:
Matrix H1Rot3 = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
EXPECT(assert_equal(H1Rot3, H1e.bottomRows(3)));
EXPECT(assert_equal(H1e, H1a));
/////////////////// H2 ///////////////////////////
Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
EXPECT(assert_equal(H2e, H2a));
/////////////////// H3 ///////////////////////////
Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
// Jacobians are around zero, so the rotation part is the same as:
Matrix H3Rot3 = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
EXPECT(assert_equal(H3Rot3, H3e.bottomRows(3)));
EXPECT(assert_equal(H3e, H3a));
/////////////////// H4 ///////////////////////////
Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
EXPECT(assert_equal(H4e, H4a));
/////////////////// H5 ///////////////////////////
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
EXPECT(assert_equal(H5e, H5a));
}
/* ************************************************************************* */
TEST( ImuFactor, ErrorAndJacobianWithBiases )
{
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0));
Vector3 v1(Vector3(0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
Vector3 v2(Vector3(0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
SETDEBUG("ImuFactor evaluateError", false);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
SETDEBUG("ImuFactor evaluateError", false);
// Expected error (should not be zero in this test, as we want to evaluate Jacobians
// at a nontrivial linearization point)
// Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
@ -197,45 +304,27 @@ TEST( ImuFactor, Error )
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a;
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
// positions and velocities
Matrix H1etop6 = H1e.topRows(6);
Matrix H1atop6 = H1a.topRows(6);
EXPECT(assert_equal(H1etop6, H1atop6));
// rotations
EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a));
// positions and velocities
Matrix H3etop6 = H3e.topRows(6);
Matrix H3atop6 = H3a.topRows(6);
EXPECT(assert_equal(H3etop6, H3atop6));
// rotations
EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(H3e, H3a));
EXPECT(assert_equal(H4e, H4a));
// EXPECT(assert_equal(H5e, H5a));
EXPECT(assert_equal(H5e, H5a));
}
/* ************************************************************************* */
TEST( ImuFactor, ErrorWithBiases )
TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis )
{
// Linearization point
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// Vector3 v1(Vector3(0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// Vector3 v2(Vector3(0.5, 0.0, 0.0));
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0));
Vector3 v1(Vector3(0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
Vector3 v2(Vector3(0.5, 0.0, 0.0));
// Measurements
@ -245,56 +334,57 @@ TEST( ImuFactor, ErrorWithBiases )
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
Pose3 bodyPsensor = Pose3();
bool use2ndOrderCoriolis = true;
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
SETDEBUG("ImuFactor evaluateError", false);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
SETDEBUG("ImuFactor evaluateError", false);
SETDEBUG("ImuFactor evaluateError", false);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
SETDEBUG("ImuFactor evaluateError", false);
// Expected error (should not be zero in this test, as we want to evaluate Jacobians
// at a nontrivial linearization point)
// Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected error
Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a;
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a;
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a));
EXPECT(assert_equal(H3e, H3a));
EXPECT(assert_equal(H4e, H4a));
EXPECT(assert_equal(H5e, H5a));
EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a));
EXPECT(assert_equal(H3e, H3a));
EXPECT(assert_equal(H4e, H4a));
EXPECT(assert_equal(H5e, H5a));
}
/* ************************************************************************* */
TEST( ImuFactor, PartialDerivativeExpmap )
TEST( ImuFactor, PartialDerivative_wrt_Bias )
{
// Linearization point
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
@ -324,20 +414,14 @@ TEST( ImuFactor, PartialDerivativeLogmap )
// Measurements
Vector3 deltatheta; deltatheta << 0, 0, 0;
// Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<Vector,Vector3>(boost::bind(
&evaluateLogRotation, thetahat, _1), Vector3(deltatheta));
const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
double normx = norm_2(x);
const Matrix3 actualDelFdeltheta = Matrix3::Identity() +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat);
// Compare Jacobians
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
}
/* ************************************************************************* */
@ -354,7 +438,6 @@ TEST( ImuFactor, fistOrderExponential )
double alpha = 0.0;
Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;
const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
@ -366,7 +449,7 @@ TEST( ImuFactor, fistOrderExponential )
hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
//hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
// Compare Jacobians
// This is a first order expansion so the equality is only an approximation
EXPECT(assert_equal(expectedRot, actualRot));
}
@ -423,6 +506,128 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
/* ************************************************************************* */
TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
{
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for(int i=1;i<100;i++)
{
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
deltaTs.push_back(0.01);
}
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
body_P_sensor, Factual, Gactual);
bool use2ndOrderIntegration = false;
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
_1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
// Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
// Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle =
numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
// Compute expected f_rot wrt angles
Matrix dfr_dangle =
numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedRot,
_1, newMeasuredOmega, newDeltaT), deltaRij_old);
Matrix FexpectedBottom3(3,9);
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3;
EXPECT(assert_equal(Fexpected, Factual));
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR G
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute jacobian wrt integration noise
Matrix dgpv_dintNoise(6,3);
dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
// Compute jacobian wrt acc noise
Matrix dgpv_daccNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, deltaRij_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, deltaRij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
Matrix GexpectedTop6(6,9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
// Compute expected f_rot wrt gyro noise
Matrix dgr_dangle =
numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedRot,
deltaRij_old, _1, newDeltaT), newMeasuredOmega);
Matrix GexpectedBottom3(3,9);
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix9 measurementCovariance;
measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3,
Z_3x3, accNoiseVar*I_3x3, Z_3x3,
Z_3x3, Z_3x3, omegaNoiseVar*I_3x3;
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() +
(1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
}
//#include <gtsam/linear/GaussianFactorGraph.h>
///* ************************************************************************* */
//TEST( ImuFactor, LinearizeTiming)
@ -561,13 +766,11 @@ TEST(ImuFactor, PredictPositionAndVelocity){
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; expectedVelocity<<0,1,0;
EXPECT(assert_equal(expectedPose, poseVelocity.pose));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
}
/* ************************************************************************* */
@ -595,7 +798,7 @@ TEST(ImuFactor, PredictRotation) {
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0));
Vector3 expectedVelocity; expectedVelocity<<0,0,0;
EXPECT(assert_equal(expectedPose, poseVelocity.pose));