From 19f7da62dd1b072f390c2f875524581eaceaba7f Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 12 Jun 2013 19:30:20 +0000 Subject: [PATCH] Refactored existing serialization functionality, added exposed interface for serialization --- .cproject | 72 ++----- gtsam/base/serialization.h | 92 +++++++++ gtsam/base/serializationTestHelpers.h | 74 +------ gtsam/slam/serialization.cpp | 257 +++++++++++++++++++++++++ gtsam/slam/serialization.h | 40 ++++ gtsam/slam/tests/testSerialization.cpp | 75 ++++++++ tests/testSerializationSLAM.cpp | 25 ++- 7 files changed, 503 insertions(+), 132 deletions(-) create mode 100644 gtsam/base/serialization.h create mode 100644 gtsam/slam/serialization.cpp create mode 100644 gtsam/slam/serialization.h create mode 100644 gtsam/slam/tests/testSerialization.cpp diff --git a/.cproject b/.cproject index 2145af336..46f9a60fb 100644 --- a/.cproject +++ b/.cproject @@ -619,54 +619,6 @@ true true - - make - -j5 - testPlanarSLAM.run - true - true - true - - - make - -j5 - testPose2SLAM.run - true - true - true - - - make - -j5 - testPose3SLAM.run - true - true - true - - - make - -j5 - testSimulated2D.run - true - true - true - - - make - -j5 - testSimulated2DOriented.run - true - true - true - - - make - -j5 - testVisualSLAM.run - true - true - true - make -j5 @@ -675,14 +627,6 @@ true true - - make - -j5 - testSerializationSLAM.run - true - true - true - make -j5 @@ -691,6 +635,14 @@ true true + + make + -j5 + testSerialization.run + true + true + true + make -j5 @@ -1163,6 +1115,14 @@ true true + + make + -j5 + testSerializationSLAM.run + true + true + true + make -j5 diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h new file mode 100644 index 000000000..8089de814 --- /dev/null +++ b/gtsam/base/serialization.h @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * 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 serialization.h + * @brief Convenience functions for serializing data structures via boost.serialization + * @author Alex Cunningham + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#pragma once + +#include +#include + +// includes for standard serialization types +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +// Serialization directly to strings in compressed format +template +std::string serialize(const T& input) { + std::ostringstream out_archive_stream; + boost::archive::text_oarchive out_archive(out_archive_stream); + out_archive << input; + return out_archive_stream.str(); +} + +template +void deserialize(const std::string& serialized, T& output) { + std::istringstream in_archive_stream(serialized); + boost::archive::text_iarchive in_archive(in_archive_stream); + in_archive >> output; +} + +// Serialization to XML format with named structures +template +std::string serializeXML(const T& input, const std::string& name="data") { + std::ostringstream out_archive_stream; + boost::archive::xml_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp(name.c_str(), input); + return out_archive_stream.str(); +} + +template +void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") { + std::istringstream in_archive_stream(serialized); + boost::archive::xml_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp(name.c_str(), output); +} + +// Serialization to binary format with named structures +template +std::string serializeBinary(const T& input, const std::string& name="data") { + std::ostringstream out_archive_stream; + boost::archive::binary_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp(name.c_str(), input); + return out_archive_stream.str(); +} + +template +void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") { + std::istringstream in_archive_stream(serialized); + boost::archive::binary_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp(name.c_str(), output); +} + +} // \namespace gtsam diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 3d5bf4d71..b6593bd9f 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -22,46 +22,14 @@ #include #include -// includes for standard serialization types -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include +#include // whether to print the serialized text to stdout const bool verbose = false; -namespace gtsam { namespace serializationTestHelpers { +namespace gtsam { +namespace serializationTestHelpers { - /* ************************************************************************* */ - // Serialization testing code. - /* ************************************************************************* */ - -template -std::string serialize(const T& input) { - std::ostringstream out_archive_stream; - boost::archive::text_oarchive out_archive(out_archive_stream); - out_archive << input; - return out_archive_stream.str(); -} - -template -void deserialize(const std::string& serialized, T& output) { - std::istringstream in_archive_stream(serialized); - boost::archive::text_iarchive in_archive(in_archive_stream); - in_archive >> output; -} // Templated round-trip serialization template @@ -97,22 +65,6 @@ bool equalsDereferenced(const T& input) { return input->equals(*output); } -/* ************************************************************************* */ -template -std::string serializeXML(const T& input) { - std::ostringstream out_archive_stream; - boost::archive::xml_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp("data", input); - return out_archive_stream.str(); -} - -template -void deserializeXML(const std::string& serialized, T& output) { - std::istringstream in_archive_stream(serialized); - boost::archive::xml_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp("data", output); -} - // Templated round-trip serialization using XML template void roundtripXML(const T& input, T& output) { @@ -148,22 +100,6 @@ bool equalsDereferencedXML(const T& input = T()) { return input->equals(*output); } -/* ************************************************************************* */ -template -std::string serializeBinary(const T& input) { - std::ostringstream out_archive_stream; - boost::archive::binary_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp("data", input); - return out_archive_stream.str(); -} - -template -void deserializeBinary(const std::string& serialized, T& output) { - std::istringstream in_archive_stream(serialized); - boost::archive::binary_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp("data", output); -} - // Templated round-trip serialization using XML template void roundtripBinary(const T& input, T& output) { @@ -199,4 +135,6 @@ bool equalsDereferencedBinary(const T& input = T()) { return input->equals(*output); } -} } +} // \namespace serializationTestHelpers +} // \namespace gtsam + diff --git a/gtsam/slam/serialization.cpp b/gtsam/slam/serialization.cpp new file mode 100644 index 000000000..a57d82add --- /dev/null +++ b/gtsam/slam/serialization.cpp @@ -0,0 +1,257 @@ +/** + * @file serialization.cpp + * + * @date Jun 12, 2013 + * @author Alex Cunningham + */ + +#include +#include + +//#include +#include +#include +#include +//#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include +#include +#include +//#include + +using namespace gtsam; + +// Creating as many permutations of factors as possible +typedef PriorFactor PriorFactorLieVector; +typedef PriorFactor PriorFactorLieMatrix; +typedef PriorFactor PriorFactorPoint2; +typedef PriorFactor PriorFactorStereoPoint2; +typedef PriorFactor PriorFactorPoint3; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorStereoCamera; + +typedef BetweenFactor BetweenFactorLieVector; +typedef BetweenFactor BetweenFactorLieMatrix; +typedef BetweenFactor BetweenFactorPoint2; +typedef BetweenFactor BetweenFactorPoint3; +typedef BetweenFactor BetweenFactorRot2; +typedef BetweenFactor BetweenFactorRot3; +typedef BetweenFactor BetweenFactorPose2; +typedef BetweenFactor BetweenFactorPose3; + +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityStereoCamera; + +typedef RangeFactor RangeFactorPosePoint2; +typedef RangeFactor RangeFactorPosePoint3; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; + +typedef BearingFactor BearingFactor2D; +typedef BearingFactor BearingFactor3D; + +typedef BearingRangeFactor BearingRangeFactor2D; +typedef BearingRangeFactor BearingRangeFactor3D; + +typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; +typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; + +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + +typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; + +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + + +/* Create GUIDs for Noisemodels */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); + +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); + +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +/* Create GUIDs for geometry */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT(gtsam::LieVector); +BOOST_CLASS_EXPORT(gtsam::LieMatrix); +BOOST_CLASS_EXPORT(gtsam::Point2); +BOOST_CLASS_EXPORT(gtsam::StereoPoint2); +BOOST_CLASS_EXPORT(gtsam::Point3); +BOOST_CLASS_EXPORT(gtsam::Rot2); +BOOST_CLASS_EXPORT(gtsam::Rot3); +BOOST_CLASS_EXPORT(gtsam::Pose2); +BOOST_CLASS_EXPORT(gtsam::Pose3); +BOOST_CLASS_EXPORT(gtsam::Cal3_S2); +BOOST_CLASS_EXPORT(gtsam::Cal3DS2); +BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo); +BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); +BOOST_CLASS_EXPORT(gtsam::SimpleCamera); +BOOST_CLASS_EXPORT(gtsam::StereoCamera); + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); + +BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector"); +BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); + +BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3"); + +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); + +BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); + +BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D"); + +BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); + +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); + +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); +//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); + +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); + +BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); + +/* ************************************************************************* */ +// Actual implementations of functions +/* ************************************************************************* */ +std::string gtsam::serializeGraph(const NonlinearFactorGraph& graph) { + return serialize(graph); +} + +/* ************************************************************************* */ +NonlinearFactorGraph gtsam::deserializeGraph(const std::string& serialized_graph) { + NonlinearFactorGraph result; + deserialize(serialized_graph, result); + return result; +} + +/* ************************************************************************* */ +std::string gtsam::serializeGraphXML(const NonlinearFactorGraph& graph, const std::string& name) { + return serializeXML(graph, name); +} + +/* ************************************************************************* */ +NonlinearFactorGraph gtsam::deserializeGraphXML(const std::string& serialized_graph, + const std::string& name) { + NonlinearFactorGraph result; + deserializeXML(serialized_graph, result, name); + return result; +} + +/* ************************************************************************* */ +std::string gtsam::serializeValues(const Values& values) { + return serialize(values); +} + +/* ************************************************************************* */ +Values gtsam::deserializeValues(const std::string& serialized_values) { + Values result; + deserialize(serialized_values, result); + return result; +} + +/* ************************************************************************* */ +std::string gtsam::serializeValuesXML(const Values& values, const std::string& name) { + return serializeXML(values, name); +} + +/* ************************************************************************* */ +Values gtsam::deserializeValuesXML(const std::string& serialized_values, + const std::string& name) { + Values result; + deserializeXML(serialized_values, result, name); + return result; +} + +/* ************************************************************************* */ + + diff --git a/gtsam/slam/serialization.h b/gtsam/slam/serialization.h new file mode 100644 index 000000000..ceb237686 --- /dev/null +++ b/gtsam/slam/serialization.h @@ -0,0 +1,40 @@ +/** + * @file serialization.h + * + * @brief Global functions for performing serialization, designed for use with matlab + * + * @date Jun 12, 2013 + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { + +// Serialize/Deserialize a NonlinearFactorGraph +std::string serializeGraph(const NonlinearFactorGraph& graph); + +NonlinearFactorGraph deserializeGraph(const std::string& serialized_graph); + +std::string serializeGraphXML(const NonlinearFactorGraph& graph, + const std::string& name = "graph"); + +NonlinearFactorGraph deserializeGraphXML(const std::string& serialized_graph, + const std::string& name = "graph"); + + +// Serialize/Deserialize a Values +std::string serializeValues(const Values& values); + +Values deserializeValues(const std::string& serialized_values); + +std::string serializeValuesXML(const Values& values, const std::string& name = "values"); + +Values deserializeValuesXML(const std::string& serialized_values, + const std::string& name = "values"); + +} // \namespace gtsam + + diff --git a/gtsam/slam/tests/testSerialization.cpp b/gtsam/slam/tests/testSerialization.cpp new file mode 100644 index 000000000..64d5504da --- /dev/null +++ b/gtsam/slam/tests/testSerialization.cpp @@ -0,0 +1,75 @@ +/** + * @file testSerialization.cpp + * + * @brief Tests for serialization global functions using boost.serialization + * + * @date Jun 12, 2013 + * @author Alex Cunningham + */ + +#include + +#include + +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +Values exampleValues() { + Values result; + result.insert(234, gtsam::Rot2::fromAngle(0.1)); + result.insert(123, gtsam::Point2(1.0, 2.0)); + result.insert(254, gtsam::Pose2(1.0, 2.0, 0.3)); + result.insert(678, gtsam::Rot3::Rx(0.1)); + result.insert(498, gtsam::Point3(1.0, 2.0, 3.0)); + result.insert(345, gtsam::Pose3(Rot3::Rx(0.1), Point3(1.0, 2.0, 3.0))); + return result; +} + +NonlinearFactorGraph exampleGraph() { + NonlinearFactorGraph graph; + graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); + graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); + return graph; +} + +/* ************************************************************************* */ +TEST( testSerialization, text_graph_serialization ) { + NonlinearFactorGraph graph = exampleGraph(); + string serialized = serializeGraph(graph); + NonlinearFactorGraph actGraph = deserializeGraph(serialized); + EXPECT(assert_equal(graph, actGraph)); +} + +/* ************************************************************************* */ +TEST( testSerialization, xml_graph_serialization ) { + NonlinearFactorGraph graph = exampleGraph(); + string serialized = serializeGraphXML(graph, "graph1"); + NonlinearFactorGraph actGraph = deserializeGraphXML(serialized, "graph1"); + EXPECT(assert_equal(graph, actGraph)); +} + +/* ************************************************************************* */ +TEST( testSerialization, text_values_serialization ) { + Values values = exampleValues(); + string serialized = serializeValues(values); + Values actValues = deserializeValues(serialized); + EXPECT(assert_equal(values, actValues)); +} + +/* ************************************************************************* */ +TEST( testSerialization, xml_values_serialization ) { + Values values = exampleValues(); + string serialized = serializeValuesXML(values, "values1"); + Values actValues = deserializeValuesXML(serialized, "values1"); + EXPECT(assert_equal(values, actValues, 1e-5)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index f9229eac6..57a1e5cb3 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -133,6 +133,13 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal" BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust"); + +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); @@ -155,7 +162,6 @@ BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); BOOST_CLASS_EXPORT(gtsam::SimpleCamera); BOOST_CLASS_EXPORT(gtsam::StereoCamera); - /* Create GUIDs for factors */ /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); @@ -225,7 +231,7 @@ BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); /* ************************************************************************* */ -TEST (Serialization, smallExample_linear) { +TEST (testSerializationSLAM, smallExample_linear) { using namespace example; Ordering ordering; ordering += X(1),X(2),L(1); @@ -245,7 +251,7 @@ TEST (Serialization, smallExample_linear) { } /* ************************************************************************* */ -TEST (Serialization, gaussianISAM) { +TEST (testSerializationSLAM, gaussianISAM) { using namespace example; Ordering ordering; GaussianFactorGraph smoother; @@ -265,7 +271,7 @@ BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement") /* ************************************************************************* */ -TEST (Serialization, smallExample_nonlinear) { +TEST (testSerializationSLAM, smallExample_nonlinear) { using namespace example; NonlinearFactorGraph nfg = createNonlinearFactorGraph(); Values c1 = createValues(); @@ -279,7 +285,7 @@ TEST (Serialization, smallExample_nonlinear) { } /* ************************************************************************* */ -TEST (Serialization, factors) { +TEST (testSerializationSLAM, factors) { LieVector lieVector(4, 1.0, 2.0, 3.0, 4.0); LieMatrix lieMatrix(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0); @@ -331,7 +337,13 @@ TEST (Serialization, factors) { SharedNoiseModel model9 = noiseModel::Isotropic::Sigma(9, 0.3); SharedNoiseModel model11 = noiseModel::Isotropic::Sigma(11, 0.3); + SharedNoiseModel robust1 = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(10.0, noiseModel::mEstimator::Huber::Scalar), + noiseModel::Unit::Create(2)); + EXPECT(equalsDereferenced(robust1)); + EXPECT(equalsDereferencedXML(robust1)); + EXPECT(equalsDereferencedBinary(robust1)); PriorFactorLieVector priorFactorLieVector(a01, lieVector, model4); PriorFactorLieMatrix priorFactorLieMatrix(a02, lieMatrix, model6); @@ -660,11 +672,8 @@ TEST (Serialization, factors) { EXPECT(equalsBinary(generalSFMFactor2Cal3_S2)); EXPECT(equalsBinary(genericStereoFactor3D)); - } - - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */