Added tests in testAHRSFactor and corrected AHRSFactor so that it works.
added target in .cproject. Note that not all tests work. In particular the IMUbias jacobian fails because the dimensions of expected and actual are different.release/4.3a0
krunalchande
parent
f3882cd0d7
commit
b721a7ce1f
122
.cproject
122
.cproject
|
|
@ -1,19 +1,17 @@
|
|||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<?fileVersion 4.0.0?>
|
||||
|
||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<storageModule moduleId="org.eclipse.cdt.core.settings">
|
||||
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
|
||||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
|
||||
<externalSettings/>
|
||||
<extensions>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
|
@ -62,13 +60,13 @@
|
|||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing">
|
||||
<externalSettings/>
|
||||
<extensions>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
|
@ -118,13 +116,13 @@
|
|||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast">
|
||||
<externalSettings/>
|
||||
<extensions>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
|
@ -584,7 +582,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>
|
||||
|
|
@ -592,7 +589,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>
|
||||
|
|
@ -640,7 +636,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>
|
||||
|
|
@ -648,7 +643,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>
|
||||
|
|
@ -656,7 +650,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>
|
||||
|
|
@ -672,7 +665,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>
|
||||
|
|
@ -902,6 +894,14 @@
|
|||
<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="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1008,7 +1008,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>
|
||||
|
|
@ -1238,46 +1237,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>
|
||||
|
|
@ -1360,6 +1319,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>
|
||||
|
|
@ -1399,6 +1359,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>
|
||||
|
|
@ -1406,6 +1367,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>
|
||||
|
|
@ -1419,6 +1381,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>
|
||||
|
|
@ -1676,7 +1678,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>
|
||||
|
|
@ -1684,7 +1685,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>
|
||||
|
|
@ -1692,7 +1692,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>
|
||||
|
|
@ -1700,7 +1699,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>
|
||||
|
|
@ -2427,7 +2425,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>
|
||||
|
|
@ -2435,7 +2432,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>
|
||||
|
|
@ -2443,7 +2439,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>
|
||||
|
|
@ -2907,6 +2902,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>
|
||||
|
|
|
|||
|
|
@ -17,9 +17,9 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias>{
|
||||
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias> {
|
||||
public:
|
||||
class PreintegratedMeasurements{
|
||||
class PreintegratedMeasurements {
|
||||
public:
|
||||
imuBias::ConstantBias biasHat;
|
||||
Matrix measurementCovariance;
|
||||
|
|
@ -30,87 +30,121 @@ public:
|
|||
Matrix PreintMeasCov;
|
||||
bool use2ndOrderIntegration_;
|
||||
|
||||
|
||||
PreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias,
|
||||
PreintegratedMeasurements(const imuBias::ConstantBias& bias,
|
||||
const Matrix3& measurementAccCovariance,
|
||||
const Matrix3& measurementGyroCovariance,
|
||||
const Matrix3& measuredOmegaCovariance,
|
||||
const Matrix3& integrationErrorCovariance,
|
||||
const bool use2ndOrderIntegration =false
|
||||
): biasHat(bias), measurementCovariance(9,9), delRdelBiasOmega(Matirx3::Zero()), PreintMeasCov(9,9),
|
||||
use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
|
||||
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(),
|
||||
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
|
||||
PreintMeasCov = Matrix::Zero(9,9);
|
||||
const bool use2ndOrderIntegration = false) :
|
||||
biasHat(bias), measurementCovariance(9, 9), delRdelBiasOmega(
|
||||
Matrix3::Zero()), PreintMeasCov(9, 9), use2ndOrderIntegration_(
|
||||
use2ndOrderIntegration) {
|
||||
measurementCovariance << integrationErrorCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measurementAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
|
||||
PreintMeasCov = Matrix::Zero(9, 9);
|
||||
}
|
||||
|
||||
PreintegratedMeasurements():
|
||||
biasHat(imuBias::ConstantBias()), measurementCovariance(9,9),
|
||||
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9)
|
||||
{
|
||||
measurementCovariance = Matrix::Zero(9,9);
|
||||
PreintMeasCov = Matrix::Zero(9,9);
|
||||
PreintegratedMeasurements() :
|
||||
biasHat(imuBias::ConstantBias()), measurementCovariance(9, 9), delRdelBiasOmega(
|
||||
Matrix3::Zero()), PreintMeasCov(9, 9) {
|
||||
measurementCovariance = Matrix::Zero(9, 9);
|
||||
PreintMeasCov = Matrix::Zero(9, 9);
|
||||
}
|
||||
|
||||
void print (const std::string& s = "Preintegrated Measurements: ") const {
|
||||
std::cout<<s<<std::endl;
|
||||
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;
|
||||
std::cout << " measurementCovariance [" << measurementCovariance << " ]"
|
||||
<< std::endl;
|
||||
std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
|
||||
}
|
||||
|
||||
bool equals (const PreintegratedMeasurements& expected, double tol = 1e-9) const {
|
||||
bool equals(const PreintegratedMeasurements& expected,
|
||||
double tol = 1e-9) const {
|
||||
return biasHat.equals(expected.biasHat, tol)
|
||||
&& equal_with_abs_tol(measurementCovariance, expected.measurementCovariancem, tol)
|
||||
&& deltaRij.equals(expected.deltaRij, tol)
|
||||
&& std::fabs(deltaTij - expected.deltaTij)<tol
|
||||
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, 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);
|
||||
}
|
||||
|
||||
void resetIntegration(){
|
||||
void resetIntegration() {
|
||||
deltaRij = Rot3();
|
||||
deltaTij = 0.0;
|
||||
delRdelBiasOmega = Matrix3::Zero();
|
||||
PreintMeasCov = Matrix::Zero(9,9);
|
||||
PreintMeasCov = Matrix::Zero(9, 9);
|
||||
}
|
||||
|
||||
void integrateMeasurement (
|
||||
const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega,
|
||||
double deltaT,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none
|
||||
){
|
||||
void integrateMeasurement(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, double deltaT,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none) {
|
||||
Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc);
|
||||
Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega);
|
||||
|
||||
if(body_P_sensor) {
|
||||
if (body_P_sensor) {
|
||||
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
|
||||
correctedOmega = body_R_sensor * correctedOmega;
|
||||
Matrix3 body_omega_body_cross = skewSymmetric(correctedOmega);
|
||||
correctedAcc = body_R_sensor * correctedAcc - body_omega_body_cross * body_omega_body_cross * body_P_sensor->translation.vector();
|
||||
correctedAcc = body_R_sensor * correctedAcc
|
||||
- body_omega_body_cross * body_omega_body_cross
|
||||
* body_P_sensor->translation().vector();
|
||||
}
|
||||
const Vector3 theta_incr = correctedOmega * deltaT;
|
||||
const Rot3 Rincr = Rot3::Expmap(theta_incr);
|
||||
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr);
|
||||
|
||||
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT;
|
||||
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega
|
||||
- Jr_theta_incr * deltaT;
|
||||
|
||||
Matrix3 Z_3x3 = Matrix::Zero();
|
||||
Matrix3 I_3x3 = Matrix::Identity();
|
||||
// Matrix3 Z_3x3 = Matrix::Zero();
|
||||
// Matrix3 I_3x3 = Matrix::Identity();
|
||||
const Vector3 theta_i = Rot3::Logmap(deltaRij);
|
||||
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_j);
|
||||
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i);
|
||||
|
||||
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
|
||||
Matrix F(3,3);
|
||||
F<<H_angles_angles;
|
||||
Rot3 Rot_j = deltaRij * Rincr;
|
||||
const Vector3 theta_j = Rot3::Logmap(Rot_j);
|
||||
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(
|
||||
theta_j);
|
||||
|
||||
PreintMeasCov = F* PreintMeasCov * F.transpose() + measurementCovariance * deltaT;
|
||||
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix()
|
||||
* Jr_theta_i;
|
||||
Matrix F(3, 3);
|
||||
F << H_angles_angles;
|
||||
|
||||
PreintMeasCov = F * PreintMeasCov * F.transpose()
|
||||
+ measurementCovariance * deltaT;
|
||||
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:
|
||||
|
|
@ -127,117 +161,208 @@ public:
|
|||
|
||||
/** Shorthand for a smart pointer to a factor */
|
||||
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
|
||||
typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
|
||||
typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
|
||||
#else
|
||||
typedef boost::shared_ptr<ImuFactor> shared_ptr;
|
||||
typedef boost::shared_ptr<AHRSFactor> shared_ptr;
|
||||
#endif
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
AHRSFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {}
|
||||
AHRSFactor() :
|
||||
preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(),
|
||||
Matrix3::Zero(), Matrix3::Zero()) {
|
||||
}
|
||||
|
||||
AHRSFactor(
|
||||
Key rot_i,
|
||||
Key rot_j,
|
||||
Key bias,
|
||||
AHRSFactor(Key rot_i, Key rot_j, Key bias,
|
||||
const PreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity,
|
||||
const Vector3& omegaCoriolis,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none,
|
||||
const bool use2ndOrderCoriolis = false
|
||||
):
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), Rot_i, bias),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){}
|
||||
const bool use2ndOrderCoriolis = false) :
|
||||
Base(
|
||||
noiseModel::Gaussian::Covariance(
|
||||
preintegratedMeasurements.PreintMeasCov), rot_i, rot_j, bias), preintegratedMeasurements_(
|
||||
preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(
|
||||
omegaCoriolis), body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(
|
||||
use2ndOrderCoriolis) {
|
||||
}
|
||||
|
||||
virtual ~AHRSFactor() {}
|
||||
|
||||
|
||||
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()) << ",";
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(
|
||||
new This(*this)
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
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 << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
|
||||
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
|
||||
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
|
||||
<< std::endl;
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
if(this->body_P_sensor_)
|
||||
if (this->body_P_sensor_)
|
||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||
}
|
||||
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
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(gravity_, e->gravity_, tol)
|
||||
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
|
||||
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||
&& ((!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_; }
|
||||
return preintegratedMeasurements_;
|
||||
}
|
||||
|
||||
const Vector3& gravity() const { return gravity_; }
|
||||
const Vector3& gravity() const {
|
||||
return gravity_;
|
||||
}
|
||||
|
||||
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
|
||||
const Vector3& omegaCoriolis() const {
|
||||
return omegaCoriolis_;
|
||||
}
|
||||
|
||||
|
||||
Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j, const imuBias::ConstantBias& bias,
|
||||
boost::optional<Matrix&> H1 = boost::none) const
|
||||
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
|
||||
{
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements_.deltaTij;
|
||||
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope();
|
||||
// const Vector3 biasAccIncr = bias.accelerometer()
|
||||
- preintegratedMeasurements_.biasHat.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias.gyroscope()
|
||||
- preintegratedMeasurements_.biasHat.gyroscope();
|
||||
|
||||
// we give some shorter name to rotations and translations
|
||||
const Rot3 Rot_i = pose_i.rotation();
|
||||
const Rot3 Rot_j = pose_j.rotation();
|
||||
const Rot3 Rot_i = rot_i;
|
||||
const Rot3 Rot_j = rot_j;
|
||||
// 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);
|
||||
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
|
||||
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 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap(
|
||||
theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
|
||||
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(
|
||||
Rot_i.between(Rot_j));
|
||||
|
||||
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
|
||||
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(
|
||||
theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc
|
||||
* skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
|
||||
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
|
||||
const 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 (H1) {
|
||||
H1->resize(3, 3);
|
||||
(*H1) << // dfR/dRi
|
||||
Jrinv_fRhat
|
||||
* (-Rot_j.between(Rot_i).matrix()
|
||||
- fRhat.inverse().matrix() * Jtheta);
|
||||
}
|
||||
|
||||
if(H2) {
|
||||
|
||||
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;
|
||||
|
||||
H5->resize(3,3);
|
||||
(*H5) <<
|
||||
// dfR/dBias
|
||||
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
|
||||
H2->resize(3,3);
|
||||
(*H2) <<
|
||||
// dfR/dPosej
|
||||
Jrinv_fRhat * ( Matrix3::Identity() );
|
||||
}
|
||||
|
||||
if (H3) {
|
||||
|
||||
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;
|
||||
|
||||
H3->resize(3, 3);
|
||||
(*H3) <<
|
||||
// dfR/dBias
|
||||
Jrinv_fRhat * (-fRhat.inverse().matrix() * JbiasOmega);
|
||||
}
|
||||
|
||||
const Vector3 fR = Rot3::Logmap(fRhat);
|
||||
|
||||
Vector r(3); r << fR;
|
||||
Vector r(3);
|
||||
r << fR;
|
||||
return r;
|
||||
}
|
||||
}; // AHRSFactor
|
||||
|
||||
/** predicted states from IMU */
|
||||
static void Predict(const Rot3& rot_i, const Rot3& rot_j,
|
||||
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 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 = 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(gravity_);
|
||||
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
|
||||
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
|
||||
}
|
||||
};
|
||||
// AHRSFactor
|
||||
typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements;
|
||||
} //namespace gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,439 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Luca Carlone, Stephen Williams, Richard Roberts
|
||||
*/
|
||||
|
||||
#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>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
|
||||
)
|
||||
{
|
||||
AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
|
||||
Matrix3::Identity(), Matrix3::Identity());
|
||||
|
||||
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
|
||||
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
|
||||
list<double>::const_iterator itDeltaT = deltaTs.begin();
|
||||
for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
|
||||
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
Rot3 evaluatePreintegratedMeasurementsRotation(
|
||||
const imuBias::ConstantBias& bias,
|
||||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, 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 measuredAcc(0.1, 0.0, 0.0);
|
||||
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);
|
||||
|
||||
bool use2ndOrderIntegration = true;
|
||||
// Actual preintegrated values
|
||||
AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
|
||||
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
EXPECT(assert_equal(expectedDeltaR1, 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(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
EXPECT(assert_equal(expectedDeltaR2, 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;
|
||||
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector();
|
||||
double deltaT = 1.0;
|
||||
bool use2ndOrderIntegration = true;
|
||||
AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, 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<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
|
||||
Matrix H2e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
|
||||
Matrix H3e = numericalDerivative11<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
|
||||
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
|
||||
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
|
||||
// LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
|
||||
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
|
||||
// LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
|
||||
|
||||
|
||||
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;
|
||||
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
|
||||
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(), Matrix3::Zero(), Matrix3::Zero());
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
|
||||
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, 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<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
|
||||
Matrix H2e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
|
||||
Matrix H3e = numericalDerivative11<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, LieVector>(boost::bind(
|
||||
&evaluateRotation, measuredOmega, _1, deltaT), LieVector(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<LieVector>(boost::bind(
|
||||
&evaluateLogRotation, thetahat, _1), LieVector(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;
|
||||
|
||||
// std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl;
|
||||
// std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl;
|
||||
|
||||
// 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();
|
||||
//hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
|
||||
|
||||
// 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> measuredAccs, measuredOmegas;
|
||||
list<double> deltaTs;
|
||||
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
|
||||
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
|
||||
deltaTs.push_back(0.01);
|
||||
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
|
||||
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
|
||||
deltaTs.push_back(0.01);
|
||||
for(int i=1;i<100;i++)
|
||||
{
|
||||
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
|
||||
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
|
||||
deltaTs.push_back(0.01);
|
||||
}
|
||||
|
||||
// Actual preintegrated values
|
||||
AHRSFactor::PreintegratedMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, 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
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
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;
|
||||
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
|
||||
double deltaT = 1.0;
|
||||
|
||||
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0));
|
||||
|
||||
// ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
|
||||
// Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega);
|
||||
|
||||
|
||||
AHRSFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
|
||||
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
|
||||
|
||||
|
||||
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
|
||||
Matrix H2e = numericalDerivative11<Rot3>(
|
||||
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
|
||||
Matrix H3e = numericalDerivative11<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));
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue