diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index 59d0ac199..ed72e18e9 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -16,15 +16,19 @@ * @author Frank Dellaert * @author Richard Roberts * @author Stephen Williams + * @author Varun Agrawal */ -#include -#include #include +#include +#include +#include + #include using namespace std; using namespace gtsam; +using namespace gtsam::serializationTestHelpers; BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); @@ -38,23 +42,23 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); -TEST(ImuFactor, serialization) { - using namespace gtsam::serializationTestHelpers; - +template +P getPreintegratedMeasurements() { // Create default parameters with Z-down and above noise paramaters - auto p = PreintegrationParams::MakeSharedD(9.81); - p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0)); + auto p = P::Params::MakeSharedD(9.81); + p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0)); p->accelerometerCovariance = 1e-7 * I_3x3; p->gyroscopeCovariance = 1e-8 * I_3x3; p->integrationCovariance = 1e-9 * I_3x3; const double deltaT = 0.005; - const imuBias::ConstantBias priorBias( - Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) - PreintegratedImuMeasurements pim(p, priorBias); + // Biases (acc, rot) + const imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); - // measurements are needed for non-inf noise model, otherwise will throw err + P pim(p, priorBias); + + // measurements are needed for non-inf noise model, otherwise will throw error // when deserialize const Vector3 measuredOmega(0, 0.01, 0); const Vector3 measuredAcc(0, 0, -9.81); @@ -62,6 +66,16 @@ TEST(ImuFactor, serialization) { for (int j = 0; j < 200; ++j) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + return pim; +} + +TEST(ImuFactor, serialization) { + auto pim = getPreintegratedMeasurements(); + + EXPECT(equalsObj(pim)); + EXPECT(equalsXML(pim)); + EXPECT(equalsBinary(pim)); + ImuFactor factor(1, 2, 3, 4, 5, pim); EXPECT(equalsObj(factor)); @@ -69,6 +83,30 @@ TEST(ImuFactor, serialization) { EXPECT(equalsBinary(factor)); } +TEST(ImuFactor2, serialization) { + auto pim = getPreintegratedMeasurements(); + + ImuFactor2 factor(1, 2, 3, pim); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +TEST(CombinedImuFactor, Serialization) { + auto pim = getPreintegratedMeasurements(); + + EXPECT(equalsObj(pim)); + EXPECT(equalsXML(pim)); + EXPECT(equalsBinary(pim)); + + const CombinedImuFactor factor(1, 2, 3, 4, 5, 6, pim); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr;