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

Added and updated serialization tests to include all IMU factors

release/4.3a0
Varun Agrawal 2020-07-09 00:22:42 -04:00
parent d519d24b67
commit 23e2b29dbe
1 changed files with 49 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
gtsam/navigation/tests/testImuFactorSerialization.cpp
View File

@ -16,15 +16,19 @@
* @author Frank Dellaert
* @author Richard Roberts
* @author Stephen Williams
* @author Varun Agrawal
*/
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <fstream>
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 <typename P>
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<PreintegratedImuMeasurements>();
EXPECT(equalsObj<PreintegratedImuMeasurements>(pim));
EXPECT(equalsXML<PreintegratedImuMeasurements>(pim));
EXPECT(equalsBinary<PreintegratedImuMeasurements>(pim));
ImuFactor factor(1, 2, 3, 4, 5, pim);
EXPECT(equalsObj<ImuFactor>(factor));
@ -69,6 +83,30 @@ TEST(ImuFactor, serialization) {
EXPECT(equalsBinary<ImuFactor>(factor));
}
TEST(ImuFactor2, serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
ImuFactor2 factor(1, 2, 3, pim);
EXPECT(equalsObj<ImuFactor2>(factor));
EXPECT(equalsXML<ImuFactor2>(factor));
EXPECT(equalsBinary<ImuFactor2>(factor));
}
TEST(CombinedImuFactor, Serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedCombinedMeasurements>();
EXPECT(equalsObj<PreintegratedCombinedMeasurements>(pim));
EXPECT(equalsXML<PreintegratedCombinedMeasurements>(pim));
EXPECT(equalsBinary<PreintegratedCombinedMeasurements>(pim));
const CombinedImuFactor factor(1, 2, 3, 4, 5, 6, pim);
EXPECT(equalsObj<CombinedImuFactor>(factor));
EXPECT(equalsXML<CombinedImuFactor>(factor));
EXPECT(equalsBinary<CombinedImuFactor>(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;