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

Actually use displacement in test

release/4.3a0
Frank Dellaert 2024-06-09 00:25:33 -07:00
parent c18191d98d
commit d72f31fbc6
1 changed files with 28 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -30,6 +30,7 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/factorTesting.h> #include <gtsam/nonlinear/factorTesting.h>
#include <cmath>
#include <list> #include <list>
using namespace std::placeholders; using namespace std::placeholders;
@ -300,6 +301,29 @@ TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) {
// 1e-3 needs to be added only when using quaternions for rotations // 1e-3 needs to be added only when using quaternions for rotations
} }
//******************************************************************************
// TEST(AHRSFactor, PimWithBodyDisplacement) {
// Vector3 bias(0, 0, 0.3);
// Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
// double deltaT = 1.0;
// auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
// p->gyroscopeCovariance = kMeasuredOmegaCovariance;
// p->body_P_sensor = Pose3(Rot3::Roll(M_PI_2), Point3(0, 0, 0));
// PreintegratedAhrsMeasurements pim(p, Vector3::Zero());
// pim.integrateMeasurement(measuredOmega, deltaT);
// Vector3 biasOmegaIncr(0.01, 0.0, 0.0);
// Matrix3 actualH;
// pim.biascorrectedDeltaRij(biasOmegaIncr, actualH);
// // Numerical derivative using a lambda function:
// auto f = [&](const Vector3& bias) { return pim.biascorrectedDeltaRij(bias); };
// Matrix3 expectedH = numericalDerivative11<Rot3, Vector3>(f, bias);
// EXPECT(assert_equal(expectedH, actualH));
// }
//****************************************************************************** //******************************************************************************
TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
Vector3 bias(0, 0, 0.3); Vector3 bias(0, 0, 0.3);
@ -312,10 +336,10 @@ TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
double deltaT = 1.0; double deltaT = 1.0;
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
Point3(1, 0, 0)); p->gyroscopeCovariance = kMeasuredOmegaCovariance;
p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(1, 2, 3)), Point3(1, 0, 0));
PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredOmegaCovariance); PreintegratedAhrsMeasurements pim(p, Vector3::Zero());
pim.integrateMeasurement(measuredOmega, deltaT); pim.integrateMeasurement(measuredOmega, deltaT);