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

Merged in feature/AHRSFactor (pull request #36) 

AHRS, Range and Drone Dynamics Factors
release/4.3a0
Krunal Chande 2014-11-22 18:10:02 -05:00
parent db3126cd14 50b1f78b6a
commit b70528cca7
14 changed files with 1557 additions and 354 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
.cproject
View File

@ -21,7 +21,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="-j8" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<option id="macosx.cpp.link.option.paths.451252615" name="Library search path (-L)" superClass="macosx.cpp.link.option.paths"/>
@ -132,7 +132,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216." name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.1052998998" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.1735323246" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build-fast}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.1127775962" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build-fast}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.1127775962" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.1922513433" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1097733515" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<option id="macosx.cpp.link.option.paths.2077171343" name="Library search path (-L)" superClass="macosx.cpp.link.option.paths" valueType="libPaths">
@ -600,7 +600,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>
@ -608,7 +607,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>
@ -656,7 +654,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>
@ -664,7 +661,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>
@ -672,7 +668,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>
@ -688,7 +683,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>
@ -1014,6 +1008,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAHRSFactor.run" path="build/gtsam/navigation/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testAHRSFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAttitudeFactor.run" path="build/gtsam/navigation/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j8</buildArguments>
<buildTarget>testAttitudeFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1120,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>
@ -1350,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>
@ -1472,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>
@ -1511,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>
@ -1518,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>
@ -1531,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>
@ -1788,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>
@ -1796,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>
@ -1804,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>
@ -1812,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>
@ -2627,7 +2635,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>
@ -2635,7 +2642,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>
@ -2643,7 +2649,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>
@ -3171,6 +3176,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>

114
gtsam.h
View File

@ -1737,6 +1737,7 @@ class Values {
void insert(size_t j, const gtsam::Cal3DS2& t);
void insert(size_t j, const gtsam::Cal3Bundler& t);
void insert(size_t j, const gtsam::EssentialMatrix& t);
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
@ -1748,9 +1749,10 @@ class Values {
void update(size_t j, const gtsam::Cal3DS2& t);
void update(size_t j, const gtsam::Cal3Bundler& t);
void update(size_t j, const gtsam::EssentialMatrix& t);
void update(size_t j, const gtsam::imuBias::ConstantBias& t);
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2}>
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::imuBias::ConstantBias}>
T at(size_t j);
};
@ -1849,6 +1851,7 @@ class KeyGroupMap {
class Marginals {
Marginals(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& solution);
void print(string s) const;
Matrix marginalCovariance(size_t variable) const;
Matrix marginalInformation(size_t variable) const;
@ -2168,6 +2171,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
};
#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
@ -2386,6 +2390,9 @@ class ConstantBias {
}///\namespace imuBias
#include <gtsam/navigation/ImuFactor.h>
class PoseVelocity{
PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity);
};
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
@ -2396,6 +2403,20 @@ class ImuFactorPreintegratedMeasurements {
void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
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;
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
@ -2407,16 +2428,56 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j,
const gtsam::imuBias::ConstantBias& bias,
gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
#include <gtsam/navigation/AHRSFactor.h>
class AHRSFactorPreintegratedMeasurements {
// Standard Constructor
AHRSFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
// get Data
Matrix measurementCovariance() const;
Matrix deltaRij() const;
double deltaTij() const;
Vector biasHat() const;
// Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
void resetIntegration() ;
};
virtual class AHRSFactor : gtsam::NonlinearFactor {
AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
const gtsam::imuBias::ConstantBias& bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, const gtsam::imuBias::ConstantBias& bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const;
};
#include <gtsam/navigation/CombinedImuFactor.h>
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias);
};
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
@ -2445,6 +2506,19 @@ class CombinedImuFactorPreintegratedMeasurements {
// 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 {
@ -2453,10 +2527,36 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
Vector gravity, Vector omegaCoriolis);
};
#include <gtsam/navigation/AttitudeFactor.h>
//virtual class AttitudeFactor : gtsam::NonlinearFactor {
// AttitudeFactor(const Unit3& nZ, const Unit3& bRef);
// AttitudeFactor();
//};
virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
const gtsam::Unit3& bRef);
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Rot3AttitudeFactor();
void print(string s) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const;
};
virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
const gtsam::Unit3& bRef);
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Pose3AttitudeFactor();
void print(string s) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const;
};
//*************************************************************************

13
gtsam/geometry/Point3.h
View File

@ -162,8 +162,17 @@ namespace gtsam {
Point3 operator / (double s) const;
/** distance between two points */
inline double distance(const Point3& p2) const {
return (p2 - *this).norm();
inline double distance(const Point3& p2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
double d = (p2 - *this).norm();
if (H1) {
*H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z())*(1./d);
}
if (H2) {
*H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z())*(1./d);
}
return d;
}
/** @deprecated The following function has been deprecated, use distance above */

17
gtsam/geometry/tests/testPoint3.cpp
View File

@ -102,6 +102,23 @@ TEST (Point3, norm) {
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
double testFunc(const Point3& P, const Point3& Q) {
return P.distance(Q);
}
TEST (Point3, distance) {
Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3);
Matrix H1, H2;
double d = P.distance(Q, H1, H2);
double expectedDistance = 55.542686;
Matrix numH1 = numericalDerivative21(testFunc, P, Q);
Matrix numH2 = numericalDerivative22(testFunc, P, Q);
DOUBLES_EQUAL(expectedDistance, d, 1e-5);
EXPECT(assert_equal(numH1, H1, 1e-8));
EXPECT(assert_equal(numH2, H2, 1e-8));
}
/* ************************************************************************* */
int main() {
TestResult tr;

1
gtsam/linear/GaussianConditional.cpp
View File

@ -208,4 +208,3 @@ namespace gtsam {
}
}

399
gtsam/navigation/AHRSFactor.h Normal file
View File

@ -0,0 +1,399 @@
/* ----------------------------------------------------------------------------
* 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 AHRSFactor.h
* @author Krunal Chande, Luca Carlone
**/
#pragma once
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/debug.h>
/* External or standard includes */
#include <ostream>
namespace gtsam {
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias> {
public:
/** Struct to store results of preintegrating IMU measurements. Can be build
* incrementally so as to avoid costly integration at time of factor construction. */
/** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates)
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/
class PreintegratedMeasurements {
friend class AHRSFactor;
protected:
imuBias::ConstantBias biasHat_;///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
Matrix 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
Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
public:
/** Default constructor, initialize with no measurements */
PreintegratedMeasurements(
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
const Matrix3& measuredOmegaCovariance ///< Covariance matrix of measured angular rate
) : biasHat_(bias), measurementCovariance_(3,3), deltaTij_(0.0),
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(3,3) {
measurementCovariance_ <<measuredOmegaCovariance;
PreintMeasCov_ = Matrix::Zero(3,3);
}
PreintegratedMeasurements() :
biasHat_(imuBias::ConstantBias()), measurementCovariance_(Matrix::Zero(3,3)), deltaTij_(0.0),
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(3,3)) {}
/** print */
void print(const std::string& s = "Preintegrated Measurements: ") const {
std::cout << s << std::endl;
biasHat_.print(" biasHat");
deltaRij_.print(" deltaRij ");
std::cout << " measurementCovariance [" << measurementCovariance_ << " ]"
<< std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
}
/** equals */
bool equals(const PreintegratedMeasurements& expected,
double tol = 1e-9) const {
return biasHat_.equals(expected.biasHat_, tol)
&& equal_with_abs_tol(measurementCovariance_,
expected.measurementCovariance_, tol)
&& deltaRij_.equals(expected.deltaRij_, tol)
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_,
tol);
}
Matrix measurementCovariance() const {return measurementCovariance_;}
Matrix deltaRij() const {return deltaRij_.matrix();}
double deltaTij() const {return deltaTij_;}
Vector biasHat() const {return biasHat_.vector();}
Matrix3 delRdelBiasOmega() {return delRdelBiasOmega_;}
Matrix PreintMeasCov() {return PreintMeasCov_;}
void resetIntegration() {
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delRdelBiasOmega_ = Matrix3::Zero();
PreintMeasCov_ = Matrix::Zero(9, 9);
}
/** Add a single Gyroscope measurement to the preintegration. */
void integrateMeasurement(
const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
double deltaT, ///< Time step
boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame
) {
// NOTE: order is important here because each update uses old values.
// First we compensate the measurements for the bias
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
// 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::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_
- Jr_theta_incr * deltaT;
// Update preintegrated measurements covariance
/* ----------------------------------------------------------------------------------------------------------------------- */
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(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::rightJacobianExpMapSO3inverse(
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
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<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(3, 3);
F << 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()
+ measurementCovariance_ * deltaT;
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
deltaRij_ = deltaRij_ * Rincr;
deltaTij_ += deltaT;
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// 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(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
}
};
private:
typedef AHRSFactor This;
typedef NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias> Base;
PreintegratedMeasurements preintegratedMeasurements_;
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
public:
/** Shorthand for a smart pointer to a factor */
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
#else
typedef boost::shared_ptr<AHRSFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
AHRSFactor() :
preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero()) {}
AHRSFactor(
Key rot_i, ///< previous rot key
Key rot_j, ///< current rot key
Key bias,///< previous bias key
const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated measurements
const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
boost::optional<const Pose3&> body_P_sensor = boost::none ///< The Pose of the sensor frame in the body frame
) :
Base(
noiseModel::Gaussian::Covariance(
preintegratedMeasurements.PreintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_(
preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor) {
}
virtual ~AHRSFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(
new This(*this)
)
);
}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
<< ",";
preintegratedMeasurements_.print(" preintegrated measurements:");
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
<< std::endl;
this->noiseModel_->print(" noise model: ");
if (this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected,
double tol = 1e-9) 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(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_)));
}
/** Access the preintegrated measurements. */
const PreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_;
}
const Vector3& omegaCoriolis() const {
return omegaCoriolis_;
}
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
const imuBias::ConstantBias& bias,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const
{
double deltaTij = preintegratedMeasurements_.deltaTij_;
Vector3 biasOmegaIncr = bias.gyroscope()
- preintegratedMeasurements_.biasHat_.gyroscope();
// We compute factor's Jacobians
/* ---------------------------------------------------------------------------------------------------- */
Rot3 deltaRij_biascorrected =
preintegratedMeasurements_.deltaRij_.retract(
preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr,
Rot3::EXPMAP);
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected
- rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
Rot3 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap(
theta_biascorrected_corioliscorrected);
Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(
rot_i.between(rot_j));
Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(
theta_biascorrected_corioliscorrected);
Matrix3 Jtheta = -Jr_theta_bcc
* skewSymmetric(rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(
Rot3::Logmap(fRhat));
if (H1) {
H1->resize(3, 3);
(*H1) << // dfR/dRi
Jrinv_fRhat
* (-rot_j.between(rot_i).matrix()
- fRhat.inverse().matrix() * Jtheta);
}
if(H2) {
H2->resize(3,3);
(*H2) <<
// dfR/dPosej
Jrinv_fRhat * ( Matrix3::Identity() );
}
if (H3) {
Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(
theta_biascorrected);
Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(
preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc
* Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H3->resize(3, 6);
(*H3) <<
// dfR/dBias
Matrix::Zero(3,3),
Jrinv_fRhat * (-fRhat.inverse().matrix() * JbiasOmega);
}
Vector3 fR = Rot3::Logmap(fRhat);
Vector r(3);
r << fR;
return r;
}
/** predicted states from IMU */
static Rot3 predict(const Rot3& rot_i,
const imuBias::ConstantBias& bias,
const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none
) {
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 = rot_i;
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
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 =
return (Rot_i.compose(deltaRij_biascorrected_corioliscorrected));
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // AHRSFactor
typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements;
} //namespace gtsam

12
gtsam/navigation/AttitudeFactor.h
View File

@ -110,6 +110,12 @@ public:
boost::optional<Matrix&> H = boost::none) const {
return attitudeError(nRb, H);
}
Unit3 nZ() const {
return nZ_;
}
Unit3 bRef() const {
return bRef_;
}
private:
@ -185,6 +191,12 @@ public:
}
return e;
}
Unit3 nZ() const {
return nZ_;
}
Unit3 bRef() const {
return bRef_;
}
private:

288
gtsam/navigation/CombinedImuFactor.h
View File

@ -29,6 +29,21 @@
namespace gtsam {
/**
* Struct to hold all state variables of CombinedImuFactor
* 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) {
}
};
/**
*
* @addtogroup SLAM
@ -55,27 +70,29 @@ namespace gtsam {
/** CombinedPreintegratedMeasurements 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*/
class CombinedPreintegratedMeasurements {
public:
imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector
friend class CombinedImuFactor;
protected:
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
Matrix 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
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
Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
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
Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
bool use2ndOrderIntegration_; ///< Controls the order of integration
// public:
///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
/** Default constructor, initialize with no IMU measurements */
public:
CombinedPreintegratedMeasurements(
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
@ -86,14 +103,14 @@ namespace gtsam {
const Matrix& biasAccOmegaInit, ///< Covariance of biasAcc & biasOmega when preintegrating measurements
const bool use2ndOrderIntegration = false ///< Controls the order of integration
///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements)
) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)),
) : biasHat_(bias), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)),
use2ndOrderIntegration_(use2ndOrderIntegration)
{
// COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
@ -104,51 +121,52 @@ namespace gtsam {
}
CombinedPreintegratedMeasurements() :
biasHat(imuBias::ConstantBias()), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15))
biasHat_(imuBias::ConstantBias()), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)),
use2ndOrderIntegration_(false) ///< Controls the order of integration
{
}
/** print */
void print(const std::string& s = "Preintegrated Measurements:") const {
std::cout << s << std::endl;
biasHat.print(" biasHat");
std::cout << " deltaTij " << deltaTij << std::endl;
std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl;
deltaRij.print(" deltaRij ");
std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
biasHat_.print(" biasHat");
std::cout << " deltaTij " << deltaTij_ << std::endl;
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
deltaRij_.print(" deltaRij ");
std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
}
/** equals */
bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) 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)
&& std::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 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)
&& std::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);
}
void resetIntegration(){
deltaPij = Vector3::Zero();
deltaVij = Vector3::Zero();
deltaRij = Rot3();
deltaTij = 0.0;
delPdelBiasAcc = Matrix3::Zero();
delPdelBiasOmega = Matrix3::Zero();
delVdelBiasAcc = Matrix3::Zero();
delVdelBiasOmega = Matrix3::Zero();
delRdelBiasOmega = Matrix3::Zero();
PreintMeasCov = Matrix::Zero(15,15);
deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero();
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delPdelBiasAcc_ = Matrix3::Zero();
delPdelBiasOmega_ = Matrix3::Zero();
delVdelBiasAcc_ = Matrix3::Zero();
delVdelBiasOmega_ = Matrix3::Zero();
delRdelBiasOmega_ = Matrix3::Zero();
PreintMeasCov_ = Matrix::Zero(15,15);
}
/** Add a single IMU measurement to the preintegration. */
@ -160,8 +178,8 @@ namespace gtsam {
) {
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
// 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 = 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){
@ -179,17 +197,17 @@ namespace gtsam {
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){
delPdelBiasAcc += delVdelBiasAcc * deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT;
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;
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;
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * 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
@ -197,10 +215,10 @@ namespace gtsam {
/* ----------------------------------------------------------------------------------------------------------------------- */
Matrix3 Z_3x3 = Matrix3::Zero();
Matrix3 I_3x3 = Matrix3::Identity();
const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3)
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
Rot3 Rot_j = deltaRij * Rincr;
Rot3 Rot_j = deltaRij_ * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
@ -211,10 +229,10 @@ namespace gtsam {
Matrix3 H_vel_pos = Z_3x3;
Matrix3 H_vel_vel = I_3x3;
Matrix3 H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
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<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix3 H_vel_biasacc = - deltaRij.matrix() * deltaT;
// 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;
@ -237,37 +255,37 @@ namespace gtsam {
Matrix G_measCov_Gt = Matrix::Zero(15,15);
// BLOCK DIAGONAL TERMS
G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3);
G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance_.block(0,0,3,3);
G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) *
(measurementCovariance.block(3,3,3,3) + measurementCovariance.block(15,15,3,3) ) *
(measurementCovariance_.block(3,3,3,3) + measurementCovariance_.block(15,15,3,3) ) *
(H_vel_biasacc.transpose());
G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) *
(measurementCovariance.block(6,6,3,3) + measurementCovariance.block(18,18,3,3) ) *
(measurementCovariance_.block(6,6,3,3) + measurementCovariance_.block(18,18,3,3) ) *
(H_angles_biasomega.transpose());
G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3);
G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance_.block(9,9,3,3);
G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3);
G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance_.block(12,12,3,3);
// NEW OFF BLOCK DIAGONAL TERMS
Matrix3 block23 = H_vel_biasacc * measurementCovariance.block(18,15,3,3) * H_angles_biasomega.transpose();
Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block(18,15,3,3) * H_angles_biasomega.transpose();
G_measCov_Gt.block(3,6,3,3) = block23;
G_measCov_Gt.block(6,3,3,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;
deltaPij_ += deltaVij_ * deltaT;
}else{
deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
}
deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
deltaRij = deltaRij * Rincr;
deltaTij += deltaT;
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
deltaRij_ = deltaRij_ * Rincr;
deltaTij_ += deltaT;
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
@ -298,23 +316,35 @@ namespace gtsam {
return Rot3::Logmap(R_t_to_t0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
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_;}
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);
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_);
}
};
@ -356,7 +386,7 @@ namespace gtsam {
boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
) :
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
@ -421,9 +451,9 @@ namespace gtsam {
boost::optional<Matrix&> H6 = boost::none) const
{
const double& deltaTij = preintegratedMeasurements_.deltaTij;
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope();
const double& deltaTij = preintegratedMeasurements_.deltaTij_;
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
// we give some shorter name to rotations and translations
const Rot3 Rot_i = pose_i.rotation();
@ -433,7 +463,7 @@ namespace gtsam {
// We compute factor's Jacobians, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
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);
@ -489,13 +519,13 @@ namespace gtsam {
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
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),
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
// dfV/dPi
dfVdPi,
// dfR/dRi
@ -558,17 +588,17 @@ namespace gtsam {
if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(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,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
// dfV/dBias_i
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
// dfR/dBias_i
Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
@ -598,17 +628,17 @@ namespace gtsam {
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 fp =
pos_j - pos_i
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr)
- 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)
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;
@ -625,41 +655,41 @@ namespace gtsam {
/** predicted states from IMU */
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j,
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, boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false)
{
const double& deltaTij = preintegratedMeasurements.deltaTij;
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat.gyroscope();
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 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;
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);
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);
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 -
@ -668,9 +698,9 @@ namespace gtsam {
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
pose_j = Pose3( Rot_j, Point3(pos_j) );
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
bias_j = bias_i;
return PoseVelocityBias(pose_j, vel_j, bias_i);
}

266
gtsam/navigation/ImuFactor.h
View File

@ -28,6 +28,16 @@
namespace gtsam {
/**
* Struct to hold return variables by the Predict Function
*/
struct PoseVelocity {
Pose3 pose;
Vector3 velocity;
PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
pose(_pose), velocity(_velocity) {
}
};
/**
*
@ -46,7 +56,6 @@ namespace gtsam {
*/
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
public:
/** Struct to store results of preintegrating IMU measurements. Can be build
@ -55,89 +64,103 @@ namespace gtsam {
/** CombinedPreintegratedMeasurements 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*/
class PreintegratedMeasurements {
public:
imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
Matrix measurementCovariance; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
friend class ImuFactor;
protected:
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
Matrix 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
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
Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
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
Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
bool use2ndOrderIntegration_; ///< Controls the order of integration
public:
/** Default constructor, initialize with no IMU measurements */
PreintegratedMeasurements(
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& measuredOmegaCovariance, ///< Covariance matrix of measured Angular Rate
const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors
const bool use2ndOrderIntegration = false ///< Controls the order of integration
) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), use2ndOrderIntegration_(use2ndOrderIntegration)
) : biasHat_(bias), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration)
{
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
PreintMeasCov = Matrix::Zero(9,9);
PreintMeasCov_ = Matrix::Zero(9,9);
}
PreintegratedMeasurements() :
biasHat(imuBias::ConstantBias()), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9)
biasHat_(imuBias::ConstantBias()), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(false)
{
measurementCovariance = Matrix::Zero(9,9);
PreintMeasCov = Matrix::Zero(9,9);
measurementCovariance_ = Matrix::Zero(9,9);
PreintMeasCov_ = Matrix::Zero(9,9);
}
/** print */
void print(const std::string& s = "Preintegrated Measurements:") const {
std::cout << s << std::endl;
biasHat.print(" biasHat");
std::cout << " deltaTij " << deltaTij << std::endl;
std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl;
deltaRij.print(" deltaRij ");
std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
biasHat_.print(" biasHat");
std::cout << " deltaTij " << deltaTij_ << std::endl;
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
deltaRij_.print(" deltaRij ");
std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
}
/** equals */
bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) 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)
&& std::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 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)
&& std::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);
}
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_;}
void resetIntegration(){
deltaPij = Vector3::Zero();
deltaVij = Vector3::Zero();
deltaRij = Rot3();
deltaTij = 0.0;
delPdelBiasAcc = Matrix3::Zero();
delPdelBiasOmega = Matrix3::Zero();
delVdelBiasAcc = Matrix3::Zero();
delVdelBiasOmega = Matrix3::Zero();
delRdelBiasOmega = Matrix3::Zero();
PreintMeasCov = Matrix::Zero(9,9);
deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero();
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delPdelBiasAcc_ = Matrix3::Zero();
delPdelBiasOmega_ = Matrix3::Zero();
delVdelBiasAcc_ = Matrix3::Zero();
delVdelBiasOmega_ = Matrix3::Zero();
delRdelBiasOmega_ = Matrix3::Zero();
PreintMeasCov_ = Matrix::Zero(9,9);
}
/** Add a single IMU measurement to the preintegration. */
@ -150,8 +173,8 @@ namespace gtsam {
// NOTE: order is important here because each update uses old values.
// First we compensate the measurements for the bias
Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc);
Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega);
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){
@ -170,25 +193,25 @@ namespace gtsam {
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){
delPdelBiasAcc += delVdelBiasAcc * deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT;
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;
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;
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
// Update preintegrated measurements covariance
/* ----------------------------------------------------------------------------------------------------------------------- */
Matrix3 Z_3x3 = Matrix3::Zero();
Matrix3 I_3x3 = Matrix3::Identity();
const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3)
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
Rot3 Rot_j = deltaRij * Rincr;
Rot3 Rot_j = deltaRij_ * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
@ -200,7 +223,7 @@ namespace gtsam {
Matrix H_vel_pos = Z_3x3;
Matrix H_vel_vel = I_3x3;
Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
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);
@ -220,7 +243,7 @@ namespace gtsam {
// the 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 ;
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
//
@ -234,13 +257,13 @@ namespace gtsam {
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){
deltaPij += deltaVij * deltaT;
deltaPij_ += deltaVij_ * deltaT;
}else{
deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
}
deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
deltaRij = deltaRij * Rincr;
deltaTij += deltaT;
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
deltaRij_ = deltaRij_ * Rincr;
deltaTij_ += deltaT;
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
@ -277,17 +300,17 @@ namespace gtsam {
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_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_);
}
};
@ -313,7 +336,7 @@ namespace gtsam {
#endif
/** Default constructor - only use for serialization */
ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {}
ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()), use2ndOrderCoriolis_(false){}
/** Constructor */
ImuFactor(
@ -328,7 +351,7 @@ namespace gtsam {
boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
) :
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias),
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
@ -391,9 +414,9 @@ namespace gtsam {
boost::optional<Matrix&> H5 = boost::none) const
{
const double& deltaTij = preintegratedMeasurements_.deltaTij;
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope();
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();
@ -403,7 +426,7 @@ namespace gtsam {
// We compute factor's Jacobians
/* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
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);
@ -438,13 +461,13 @@ namespace gtsam {
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
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),
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
// dfV/dPi
dfVdPi,
// dfR/dRi
@ -492,17 +515,17 @@ namespace gtsam {
if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(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,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
// dfV/dBias
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
// dfR/dBias
Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
@ -512,17 +535,17 @@ namespace gtsam {
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 fp =
pos_j - pos_i
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr)
- 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)
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;
@ -534,31 +557,31 @@ namespace gtsam {
/** predicted states from IMU */
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false)
{
const double& deltaTij = preintegratedMeasurements.deltaTij;
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat.gyroscope();
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)
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;
vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
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);
@ -567,7 +590,7 @@ namespace gtsam {
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);
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 -
@ -576,7 +599,8 @@ namespace gtsam {
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
pose_j = Pose3( Rot_j, Point3(pos_j) );
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
return PoseVelocity(pose_j, vel_j);
}

8
gtsam/navigation/MagFactor.h
View File

@ -74,7 +74,7 @@ public:
*/
Vector evaluateError(const Rot2& nRb,
boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRbÕ * nM + b
// measured bM = nRb<EFBFBD> * nM + b
Point3 hx = unrotate(nRb, nM_, H) + bias_;
return (hx - measured_).vector();
}
@ -112,7 +112,7 @@ public:
*/
Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRbÕ * nM + b
// measured bM = nRb<EFBFBD> * nM + b
Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
return (hx - measured_).vector();
}
@ -151,7 +151,7 @@ public:
Vector evaluateError(const Point3& nM, const Point3& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
// measured bM = nRbÕ * nM + b, where b is unknown bias
// measured bM = nRb<EFBFBD> * nM + b, where b is unknown bias
Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
if (H2)
*H2 = eye(3);
@ -193,7 +193,7 @@ public:
const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const {
// measured bM = nRbÕ * nM + b, where b is unknown bias
// measured bM = nRb<EFBFBD> * nM + b, where b is unknown bias
Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
Point3 hx = scale * rotated.point3() + bias;
if (H1)

483
gtsam/navigation/tests/testAHRSFactor.cpp Normal file
View File

@ -0,0 +1,483 @@
/* ----------------------------------------------------------------------------
* 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 testImuFactor.cpp
* @brief Unit test for ImuFactor
* @author Krunal Chande, Luca Carlone
*/
#include <gtsam/navigation/AHRSFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <list>
using namespace std;
using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
/* ************************************************************************* */
namespace {
Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i,
const Rot3 rot_j, const imuBias::ConstantBias& bias) {
return factor.evaluateError(rot_i, rot_j, bias);
}
Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
const Rot3 rot_j, const imuBias::ConstantBias& bias) {
return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3));
}
AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity());
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
list<double>::const_iterator itDeltaT = deltaTs.begin();
for (; itOmega != measuredOmegas.end(); ++itOmega, ++itDeltaT) {
result.integrateMeasurement(*itOmega, *itDeltaT);
}
return result;
}
Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
return Rot3(evaluatePreintegratedMeasurements(bias, measuredOmegas,
deltaTs, initialRotationRate).deltaRij());
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
const double deltaT) {
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
}
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
}
}
/* ************************************************************************* */
TEST( AHRSFactor, PreintegratedMeasurements ) {
// Linearization point
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
// Measurements
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
double deltaT = 0.5;
// Expected preintegrated values
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT1(0.5);
// Actual preintegrated values
AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero());
actual1.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
// Integrate again
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT2(1);
// Actual preintegrated values
AHRSFactor::PreintegratedMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
}
/* ************************************************************************* */
TEST( ImuFactor, Error ) {
// Linearization point
imuBias::ConstantBias bias; // 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));
// Measurements
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega;
measuredOmega << M_PI / 100, 0, 0;
double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis,
false);
Vector errorActual = factor.evaluateError(x1, x2, bias);
// Expected error
Vector errorExpected(3);
errorExpected << 0, 0, 0;
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
// Actual Jacobians
Matrix H1a, H2a, H3a;
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
// rotations
EXPECT(assert_equal(RH1e, H1a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(H2e, H2a, 1e-5));
// rotations
EXPECT(assert_equal(RH2e, H2a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(H3e, H3a, 1e-5)); // FIXME!! DOes not work. Different matrix dimensions.
}
/* ************************************************************************* */
TEST( ImuFactor, ErrorWithBiases ) {
// Linearization point
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
// Measurements
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0.0, 0.0;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
SETDEBUG("ImuFactor evaluateError", false);
Vector errorActual = factor.evaluateError(x1, x2, bias);
SETDEBUG("ImuFactor evaluateError", false);
// Expected error
Vector errorExpected(3);
errorExpected << 0, 0, 0;
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a;
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a));
EXPECT(assert_equal(H3e, H3a));
}
/* ************************************************************************* */
TEST( AHRSFactor, PartialDerivativeExpmap ) {
// Linearization point
Vector3 biasOmega;
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
// Measurements
Vector3 measuredOmega;
measuredOmega << 0.1, 0, 0;
double deltaT = 0.5;
// Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT),
biasOmega);
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3(
(measuredOmega - biasOmega) * deltaT);
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
// Compare Jacobians
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
/* ************************************************************************* */
TEST( AHRSFactor, PartialDerivativeLogmap ) {
// Linearization point
Vector3 thetahat;
thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias
// Measurements
Vector3 deltatheta;
deltatheta << 0, 0, 0;
// Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<Vector3, Vector3>(
boost::bind(&evaluateLogRotation, thetahat, _1), 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;
// Compare Jacobians
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
}
/* ************************************************************************* */
TEST( AHRSFactor, fistOrderExponential ) {
// Linearization point
Vector3 biasOmega;
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
// Measurements
Vector3 measuredOmega;
measuredOmega << 0.1, 0, 0;
double deltaT = 1.0;
// change w.r.t. linearization point
double alpha = 0.0;
Vector3 deltabiasOmega;
deltabiasOmega << alpha, alpha, alpha;
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3(
(measuredOmega - biasOmega) * deltaT);
Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
const Matrix expectedRot = Rot3::Expmap(
(measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix3 hatRot =
Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 actualRot = hatRot
* Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
// Compare Jacobians
EXPECT(assert_equal(expectedRot, actualRot));
}
/* ************************************************************************* */
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
// Measurements
list<Vector3> measuredOmegas;
list<double> deltaTs;
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for (int i = 1; i < 100; i++) {
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
AHRSFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredOmegas,
deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0));
// Compute numerical derivatives
Matrix expectedDelRdelBias =
numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
measuredOmegas, deltaTs,
Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
/* ************************************************************************* */
TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.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;
double deltaT = 1.0;
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0));
AHRSFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a;
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a));
EXPECT(assert_equal(H3e, H3a));
}
/* ************************************************************************* */
TEST (AHRSFactor, predictTest) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.0, 0.0;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.001;
AHRSFactor::PreintegratedMeasurements pre_int_data(bias,Matrix3::Zero());
for (int i = 0; i < 1000; ++i){
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
}
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
// Predict
Rot3 x;
Rot3 expectedRot = Rot3().ypr(M_PI/10,0,0);
Rot3 actualRot = factor.predict(x,bias, pre_int_data, omegaCoriolis);
EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
}
/* ************************************************************************* */
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Marginals.h>
TEST (AHRSFactor, graphTest) {
// linearization point
Rot3 x1(Rot3::RzRyRx(0, 0, 0));
Rot3 x2(Rot3::RzRyRx(0, M_PI/4, 0));
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
// PreIntegrator
imuBias::ConstantBias biasHat(Vector3(0, 0, 0), Vector3(0, 0, 0));
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
AHRSFactor::PreintegratedMeasurements pre_int_data(biasHat,
Matrix3::Identity());
// Pre-integrate measurements
Vector3 measuredAcc(0.0, 0.0, 0.0);
Vector3 measuredOmega(0, M_PI/20, 0);
double deltaT = 1;
// Create Factor
noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov());
NonlinearFactorGraph graph;
Values values;
for(size_t i = 0; i < 5; ++i) {
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
}
// pre_int_data.print("Pre integrated measurementes");
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
values.insert(X(1), x1);
values.insert(X(2), x2);
values.insert(B(1), bias);
graph.push_back(factor);
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
Rot3 expectedRot(Rot3::RzRyRx(0, M_PI/4, 0));
EXPECT(assert_equal(expectedRot, result.at<Rot3>(X(2))));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -68,7 +68,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij;
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij();
}
Vector3 evaluatePreintegratedMeasurementsVelocity(
@ -79,7 +79,7 @@ Vector3 evaluatePreintegratedMeasurementsVelocity(
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij;
measuredAccs, measuredOmegas, deltaTs).deltaVij();
}
Rot3 evaluatePreintegratedMeasurementsRotation(
@ -89,8 +89,8 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaRij;
return Rot3(evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaRij());
}
}
@ -127,10 +127,10 @@ TEST( CombinedImuFactor, PreintegratedMeasurements )
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.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);
}
@ -181,7 +181,7 @@ TEST( CombinedImuFactor, ErrorWithBiases )
// 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);
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);
@ -254,12 +254,72 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
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)
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3;
Vector3 measuredAcc; measuredAcc << 0,1,-9.81;
double deltaT = 0.001;
double tol = 1e-6;
Matrix I6x6(6,6);
I6x6 = Matrix::Identity(6,6);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true );
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)));
}
TEST(CombinedImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
Matrix I6x6(6,6);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true );
Vector3 measuredAcc; measuredAcc << 0, 0, -9.81;
Vector3 gravity; gravity<<0,0,9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0,0,0;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.001;
double tol = 1e-4;
for (int i = 0; i < 1000; ++i)
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
deltaT);
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
Combined_pre_int_data, gravity, omegaCoriolis);
// 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);
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>

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

@ -80,7 +80,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaPij;
measuredAccs, measuredOmegas, deltaTs).deltaPij();
}
Vector3 evaluatePreintegratedMeasurementsVelocity(
@ -91,7 +91,7 @@ Vector3 evaluatePreintegratedMeasurementsVelocity(
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij;
measuredAccs, measuredOmegas, deltaTs).deltaVij();
}
Rot3 evaluatePreintegratedMeasurementsRotation(
@ -101,8 +101,8 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij;
return Rot3(evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij());
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
@ -140,10 +140,10 @@ TEST( ImuFactor, PreintegratedMeasurements )
ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij), 1e-6));
EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij, 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij, 1e-6);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
// Integrate again
Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0;
@ -155,10 +155,10 @@ TEST( ImuFactor, PreintegratedMeasurements )
ImuFactor::PreintegratedMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij), 1e-6));
EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij, 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij, 1e-6);
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
}
/* ************************************************************************* */
@ -432,12 +432,12 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
//#include <gtsam/linear/GaussianFactorGraph.h>
@ -466,8 +466,8 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
// }
//
// // Create factor
// noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.preintegratedMeasurementsCovariance());
// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
// noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.MeasurementCovariance());
// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
//
// Values values;
// values.insert(X(1), x1);
@ -560,6 +560,71 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
EXPECT(assert_equal(H5e, H5a));
}
TEST(ImuFactor, PredictPositionAndVelocity){
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3;
Vector3 measuredAcc; measuredAcc << 0,1,-9.81;
double deltaT = 0.001;
double tol = 1e-6;
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::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
for (int i = 0; i<1000; ++i) 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);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, 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)));
}
TEST(ImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3;
Vector3 measuredAcc; measuredAcc << 0,0,-9.81;
double deltaT = 0.001;
double tol = 1e-6;
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::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
for (int i = 0; i<1000; ++i) 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);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, 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));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}

1
gtsam/nonlinear/ISAM2-inl.h
View File

@ -303,4 +303,3 @@ int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique) {
}