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

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

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

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_);
}

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

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_);

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

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

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

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

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

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

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);}
/* ************************************************************************* */

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