diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am index aa02ffec9..25abacbaf 100644 --- a/gtsam/base/Makefile.am +++ b/gtsam/base/Makefile.am @@ -36,6 +36,11 @@ headers += BTree.h DSF.h FastMap.h FastSet.h FastList.h FastVector.h sources += DSFVector.cpp check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationBase +endif + # Timing tests noinst_PROGRAMS = tests/timeMatrix tests/timeVirtual tests/timeVirtual2 tests/timeTest noinst_PROGRAMS += tests/timeMatrixOps @@ -56,6 +61,11 @@ AM_CPPFLAGS = AM_CPPFLAGS += $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationBase_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h new file mode 100644 index 000000000..ef2e9393d --- /dev/null +++ b/gtsam/base/serializationTestHelpers.h @@ -0,0 +1,148 @@ +/* ---------------------------------------------------------------------------- + + * 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 serializationTestHelpers.h + * @brief + * @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 + +// whether to print the serialized text to stdout +const bool verbose = false; + +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 +void roundtrip(const T& input, T& output) { + // Serialize + std::string serialized = serialize(input); + if (verbose) std::cout << serialized << std::endl << std::endl; + + deserialize(serialized, output); +} + +// This version requires equality operator +template +bool equality(const T& input = T()) { + T output; + roundtrip(input,output); + return input==output; +} + +// This version requires equals +template +bool equalsObj(const T& input = T()) { + T output; + roundtrip(input,output); + return input.equals(output); +} + +// De-referenced version for pointers +template +bool equalsDereferenced(const T& input) { + T output; + roundtrip(input,output); + 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) { + // Serialize + std::string serialized = serializeXML(input); + if (verbose) std::cout << serialized << std::endl << std::endl; + + // De-serialize + deserializeXML(serialized, output); +} + +// This version requires equality operator +template +bool equalityXML(const T& input = T()) { + T output; + roundtripXML(input,output); + return input==output; +} + +// This version requires equals +template +bool equalsXML(const T& input = T()) { + T output; + roundtripXML(input,output); + return input.equals(output); +} + +// This version is for pointers +template +bool equalsDereferencedXML(const T& input = T()) { + T output; + roundtripXML(input,output); + return input->equals(*output); +} + +} } diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp new file mode 100644 index 000000000..211a1c319 --- /dev/null +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -0,0 +1,41 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSerializationBase.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + + +/* ************************************************************************* */ +TEST (Serialization, matrix_vector) { + EXPECT(equality(Vector_(4, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equality(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); + + EXPECT(equalityXML(Vector_(4, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityXML(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am index 5f9b34610..b87ba8d07 100644 --- a/gtsam/geometry/Makefile.am +++ b/gtsam/geometry/Makefile.am @@ -32,6 +32,11 @@ headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Ex sources += projectiveGeometry.cpp tensorInterface.cpp check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationGeometry +endif + # Timing tests noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera tests/timeStereoCamera @@ -53,6 +58,11 @@ libgeometry_la_SOURCES = $(allsources) AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationGeometry_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp new file mode 100644 index 000000000..c1fba6b2c --- /dev/null +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSerializationGeometry.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Export all classes derived from Value +BOOST_CLASS_EXPORT(gtsam::Cal3_S2) +BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo) +BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) +BOOST_CLASS_EXPORT(gtsam::CalibratedCamera) +BOOST_CLASS_EXPORT(gtsam::Point2) +BOOST_CLASS_EXPORT(gtsam::Point3) +BOOST_CLASS_EXPORT(gtsam::Pose2) +BOOST_CLASS_EXPORT(gtsam::Pose3) +BOOST_CLASS_EXPORT(gtsam::Rot2) +BOOST_CLASS_EXPORT(gtsam::Rot3) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::StereoPoint2) + +/* ************************************************************************* */ +Point3 pt3(1.0, 2.0, 3.0); +Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +Pose3 pose3(rt3, pt3); + +Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); +Cal3Bundler cal3(1.0, 2.0, 3.0); +Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); +Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); +CalibratedCamera cal5(Pose3(rt3, pt3)); + +PinholeCamera cam1(pose3, cal1); +StereoCamera cam2(pose3, cal4ptr); +StereoPoint2 spt(1.0, 2.0, 3.0); + +/* ************************************************************************* */ +TEST (Serialization, text_geometry) { + EXPECT(equalsObj(Point2(1.0, 2.0))); + EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); + EXPECT(equalsObj(Rot2::fromDegrees(30.0))); + + EXPECT(equalsObj(pt3)); + EXPECT(equalsObj(rt3)); + EXPECT(equalsObj(Pose3(rt3, pt3))); + + EXPECT(equalsObj(cal1)); + EXPECT(equalsObj(cal2)); + EXPECT(equalsObj(cal3)); + EXPECT(equalsObj(cal4)); + EXPECT(equalsObj(cal5)); + + EXPECT(equalsObj(cam1)); + EXPECT(equalsObj(cam2)); + EXPECT(equalsObj(spt)); +} + +/* ************************************************************************* */ +TEST (Serialization, xml_geometry) { + EXPECT(equalsXML(Point2(1.0, 2.0))); + EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); + EXPECT(equalsXML(Rot2::fromDegrees(30.0))); + + EXPECT(equalsXML(pt3)); + EXPECT(equalsXML(rt3)); + EXPECT(equalsXML(Pose3(rt3, pt3))); + + EXPECT(equalsXML(cal1)); + EXPECT(equalsXML(cal2)); + EXPECT(equalsXML(cal3)); + EXPECT(equalsXML(cal4)); + EXPECT(equalsXML(cal5)); + + EXPECT(equalsXML(cam1)); + EXPECT(equalsXML(cam2)); + EXPECT(equalsXML(spt)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/inference/Makefile.am b/gtsam/inference/Makefile.am index da84df61b..cab5c5cbb 100644 --- a/gtsam/inference/Makefile.am +++ b/gtsam/inference/Makefile.am @@ -45,6 +45,11 @@ check_PROGRAMS += tests/testClusterTree check_PROGRAMS += tests/testJunctionTree check_PROGRAMS += tests/testPermutation +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationInference +endif + #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am @@ -64,6 +69,11 @@ AM_CPPFLAGS = $(ccolamd_inc) $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) AM_CXXFLAGS = +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationInference_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/inference/tests/testSerializationInference.cpp b/gtsam/inference/tests/testSerializationInference.cpp new file mode 100644 index 000000000..2db7014f1 --- /dev/null +++ b/gtsam/inference/tests/testSerializationInference.cpp @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSerializationInference.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#define GTSAM_MAGIC_KEY + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +TEST (Serialization, symbolic_graph) { + Ordering o; o += "x1","l1","x2"; + // construct expected symbolic graph + SymbolicFactorGraph sfg; + sfg.push_factor(o["x1"]); + sfg.push_factor(o["x1"],o["x2"]); + sfg.push_factor(o["x1"],o["l1"]); + sfg.push_factor(o["l1"],o["x2"]); + + EXPECT(equalsObj(sfg)); + EXPECT(equalsXML(sfg)); +} + +/* ************************************************************************* */ +TEST (Serialization, symbolic_bn) { + Ordering o; o += "x2","l1","x1"; + + IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"])); + IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"])); + IndexConditional::shared_ptr x1(new IndexConditional(o["x1"])); + + SymbolicBayesNet sbn; + sbn.push_back(x2); + sbn.push_back(l1); + sbn.push_back(x1); + + EXPECT(equalsObj(sbn)); + EXPECT(equalsXML(sbn)); +} + +/* ************************************************************************* */ +TEST (Serialization, symbolic_bayes_tree ) { + typedef BayesTree SymbolicBayesTree; + static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; + IndexConditional::shared_ptr + B(new IndexConditional(_B_)), + L(new IndexConditional(_L_, _B_)), + E(new IndexConditional(_E_, _L_, _B_)), + S(new IndexConditional(_S_, _L_, _B_)), + T(new IndexConditional(_T_, _E_, _L_)), + X(new IndexConditional(_X_, _E_)); + + // Bayes Tree for Asia example + SymbolicBayesTree bayesTree; + SymbolicBayesTree::insert(bayesTree, B); + SymbolicBayesTree::insert(bayesTree, L); + SymbolicBayesTree::insert(bayesTree, E); + SymbolicBayesTree::insert(bayesTree, S); + SymbolicBayesTree::insert(bayesTree, T); + SymbolicBayesTree::insert(bayesTree, X); + + EXPECT(equalsObj(bayesTree)); + EXPECT(equalsXML(bayesTree)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am index 811ae7458..bd78bae52 100644 --- a/gtsam/linear/Makefile.am +++ b/gtsam/linear/Makefile.am @@ -41,6 +41,11 @@ sources += iterative.cpp SubgraphPreconditioner.cpp SubgraphSolver.cpp headers += IterativeSolver.h IterativeOptimizationParameters.h headers += SubgraphSolver-inl.h +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationLinear +endif + # Timing tests noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike @@ -58,6 +63,11 @@ AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) AM_CXXFLAGS = +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationLinear_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp new file mode 100644 index 000000000..8082d3626 --- /dev/null +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSerializationLinear.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Noise model components +/* ************************************************************************* */ + +/* ************************************************************************* */ +// Export 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::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +/* ************************************************************************* */ +// example noise models +noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); +noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); +noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); +noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); + +/* ************************************************************************* */ +TEST (Serialization, noiseModels) { + // tests using pointers to the derived class + EXPECT( equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT( equalsDereferenced(gaussian3)); + EXPECT(equalsDereferencedXML(gaussian3)); + + EXPECT( equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT( equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); + + EXPECT( equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); +} + +/* ************************************************************************* */ +TEST (Serialization, SharedNoiseModel_noiseModels) { + SharedNoiseModel diag3_sg = diag3; + EXPECT(equalsDereferenced(diag3_sg)); + EXPECT(equalsDereferencedXML(diag3_sg)); + + EXPECT(equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT(equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT(equalsDereferenced(gaussian3)); + EXPECT(equalsDereferencedXML(gaussian3)); + + EXPECT(equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); + + EXPECT(equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); +} + +/* ************************************************************************* */ +TEST (Serialization, SharedDiagonal_noiseModels) { + EXPECT(equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT(equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT(equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); + + EXPECT(equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); +} + +/* ************************************************************************* */ +// Linear components +/* ************************************************************************* */ + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); + +/* ************************************************************************* */ +TEST (Serialization, linear_factors) { + VectorValues values; + values.insert(0, Vector_(1, 1.0)); + values.insert(1, Vector_(2, 2.0,3.0)); + values.insert(2, Vector_(2, 4.0,5.0)); + EXPECT(equalsObj(values)); + EXPECT(equalsXML(values)); + + Index i1 = 4, i2 = 7; + Matrix A1 = eye(3), A2 = -1.0 * eye(3); + Vector b = ones(3); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); + JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); + EXPECT(equalsObj(jacobianfactor)); + EXPECT(equalsXML(jacobianfactor)); + + HessianFactor hessianfactor(jacobianfactor); + EXPECT(equalsObj(hessianfactor)); + EXPECT(equalsXML(hessianfactor)); +} + +/* ************************************************************************* */ +TEST (Serialization, gaussian_conditional) { + Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.); + Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4); + Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34); + Vector d(2); d << 0.2, 0.5; + GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2)); + + EXPECT(equalsObj(cg)); + EXPECT(equalsXML(cg)); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 0512c36e1..15651a22a 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -44,6 +44,11 @@ headers += WhiteNoiseFactor.h # Kalman Filter headers += ExtendedKalmanFilter.h ExtendedKalmanFilter-inl.h +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationNonlinear +endif + #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am @@ -58,6 +63,11 @@ AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) AM_CXXFLAGS = +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationNonlinear_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp new file mode 100644 index 000000000..120a9fa94 --- /dev/null +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSerializationNonlinear.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Export all classes derived from Value +BOOST_CLASS_EXPORT(gtsam::Cal3_S2) +BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) +BOOST_CLASS_EXPORT(gtsam::Point3) +BOOST_CLASS_EXPORT(gtsam::Pose3) +BOOST_CLASS_EXPORT(gtsam::Rot3) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) + +/* ************************************************************************* */ +typedef PinholeCamera PinholeCal3S2; +typedef PinholeCamera PinholeCal3DS2; +typedef PinholeCamera PinholeCal3Bundler; + +/* ************************************************************************* */ +Point3 pt3(1.0, 2.0, 3.0); +Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +Pose3 pose3(rt3, pt3); + +Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); +Cal3Bundler cal3(1.0, 2.0, 3.0); + +TEST (Serialization, TemplatedValues) { + Values values; + values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1)); + values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2)); + values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3)); + values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1)); + EXPECT(equalsObj(values)); + EXPECT(equalsXML(values)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 71eb94d64..cfa37c29d 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -54,6 +54,11 @@ check_PROGRAMS += tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bund headers += StereoFactor.h check_PROGRAMS += tests/testStereoFactor +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationSLAM +endif + #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am @@ -67,6 +72,11 @@ libslam_la_SOURCES = $(sources) AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationSLAM_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/slam/tests/testSerializationSLAM.cpp b/gtsam/slam/tests/testSerializationSLAM.cpp new file mode 100644 index 000000000..c75e0a148 --- /dev/null +++ b/gtsam/slam/tests/testSerializationSLAM.cpp @@ -0,0 +1,209 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSerializationSLAM.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#define GTSAM_MAGIC_KEY + +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); + +/* ************************************************************************* */ +// Export 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::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +/* ************************************************************************* */ +TEST (Serialization, smallExample_linear) { + using namespace example; + + Ordering ordering; ordering += "x1","x2","l1"; + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + EXPECT(equalsObj(ordering)); + EXPECT(equalsXML(ordering)); + + EXPECT(equalsObj(fg)); + EXPECT(equalsXML(fg)); + + GaussianBayesNet cbn = createSmallGaussianBayesNet(); + EXPECT(equalsObj(cbn)); + EXPECT(equalsXML(cbn)); +} + +/* ************************************************************************* */ +TEST (Serialization, gaussianISAM) { + using namespace example; + Ordering ordering; + GaussianFactorGraph smoother; + boost::tie(smoother, ordering) = createSmoother(7); + BayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianISAM isam(bayesTree); + + EXPECT(equalsObj(isam)); + EXPECT(equalsXML(isam)); +} + +/* ************************************************************************* */ +/* Create GUIDs for factors in simulated2D */ +BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" ) +BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" ) +BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement") +BOOST_CLASS_EXPORT(gtsam::Point2) + +/* ************************************************************************* */ +TEST (Serialization, smallExample) { + using namespace example; + Graph nfg = createNonlinearFactorGraph(); + Values c1 = createValues(); + EXPECT(equalsObj(nfg)); + EXPECT(equalsXML(nfg)); + + EXPECT(equalsObj(c1)); + EXPECT(equalsXML(c1)); +} + +/* ************************************************************************* */ +/* Create GUIDs for factors */ +BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior, "gtsam::planarSLAM::Prior"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing, "gtsam::planarSLAM::Bearing"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Range, "gtsam::planarSLAM::Range"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry, "gtsam::planarSLAM::Odometry"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint"); + +BOOST_CLASS_EXPORT(gtsam::Pose2) + +/* ************************************************************************* */ +TEST (Serialization, planar_system) { + using namespace planarSLAM; + planarSLAM::Values values; + values.insert(PointKey(3), Point2(1.0, 2.0)); + values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); + + SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); + SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); + SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); + + Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1); + Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1); + Range range(PoseKey(2), PointKey(9), 7.0, model1); + BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2); + Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3); + Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2)); + + Graph graph; + graph.add(prior); + graph.add(bearing); + graph.add(range); + graph.add(bearingRange); + graph.add(odometry); + graph.add(constraint); + + // text + EXPECT(equalsObj(PoseKey(2))); + EXPECT(equalsObj(PointKey(3))); + EXPECT(equalsObj(values)); + EXPECT(equalsObj(prior)); + EXPECT(equalsObj(bearing)); + EXPECT(equalsObj(bearingRange)); + EXPECT(equalsObj(range)); + EXPECT(equalsObj(odometry)); + EXPECT(equalsObj(constraint)); + EXPECT(equalsObj(graph)); + + // xml + EXPECT(equalsXML(PoseKey(2))); + EXPECT(equalsXML(PointKey(3))); + EXPECT(equalsXML(values)); + EXPECT(equalsXML(prior)); + EXPECT(equalsXML(bearing)); + EXPECT(equalsXML(bearingRange)); + EXPECT(equalsXML(range)); + EXPECT(equalsXML(odometry)); + EXPECT(equalsXML(constraint)); + EXPECT(equalsXML(graph)); +} + +/* ************************************************************************* */ +/* Create GUIDs for factors */ +BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor"); + +BOOST_CLASS_EXPORT(gtsam::Pose3) +BOOST_CLASS_EXPORT(gtsam::Point3) + +Point3 pt3(1.0, 2.0, 3.0); +Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +Pose3 pose3(rt3, pt3); +Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); + +/* ************************************************************************* */ +TEST (Serialization, visual_system) { + using namespace visualSLAM; + Values values; + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); + Pose3 pose1 = pose3, pose2 = pose3.inverse(); + Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); + values.insert(x1, pose1); + values.insert(l1, pt1); + SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); + SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); + SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); + boost::shared_ptr K(new Cal3_S2(cal1)); + + Graph graph; + graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K); + graph.addPointConstraint(1, pt1); + graph.addPointPrior(1, pt2, model3); + graph.addPoseConstraint(1, pose1); + graph.addPosePrior(1, pose2, model6); + + EXPECT(equalsObj(values)); + EXPECT(equalsObj(graph)); + + EXPECT(equalsXML(values)); + EXPECT(equalsXML(graph)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/tests/Makefile.am b/tests/Makefile.am index fb1172222..66044ccfc 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -24,11 +24,6 @@ check_PROGRAMS += testGaussianISAM2 check_PROGRAMS += testExtendedKalmanFilter check_PROGRAMS += testRot3Optimization -## flag disabled to force this test to get updated properly -#if ENABLE_SERIALIZATION -check_PROGRAMS += testSerialization -#endif - # Timing tests noinst_PROGRAMS = timeGaussianFactorGraph timeSequentialOnDataset timeMultifrontalOnDataset @@ -39,11 +34,6 @@ TESTS = $(check_PROGRAMS) AM_LDFLAGS = $(BOOST_LDFLAGS) AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -# link to serialization library for test -#if ENABLE_SERIALIZATION -AM_LDFLAGS += -lboost_serialization -#endif - LDADD = ../gtsam/libgtsam.la ../CppUnitLite/libCppUnitLite.a AM_DEFAULT_SOURCE_EXT = .cpp diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp deleted file mode 100644 index fa9944d1b..000000000 --- a/tests/testSerialization.cpp +++ /dev/null @@ -1,588 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/** - * @brief Unit tests for serialization of library classes - * @author Frank Dellaert - * @author Alex Cunningham - */ - -/* ************************************************************************* */ -// Serialization testing code. -/* ************************************************************************* */ - -#include -#include - -// includes for standard serialization types -#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; - -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 -void roundtrip(const T& input, T& output) { - // Serialize - std::string serialized = serialize(input); - if (verbose) std::cout << serialized << std::endl << std::endl; - - deserialize(serialized, output); -} - -// This version requires equality operator -template -bool equality(const T& input = T()) { - T output; - roundtrip(input,output); - return input==output; -} - -// This version requires equals -template -bool equalsObj(const T& input = T()) { - T output; - roundtrip(input,output); - return input.equals(output); -} - -// De-referenced version for pointers -template -bool equalsDereferenced(const T& input) { - T output; - roundtrip(input,output); - 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) { - // Serialize - std::string serialized = serializeXML(input); - if (verbose) std::cout << serialized << std::endl << std::endl; - - // De-serialize - deserializeXML(serialized, output); -} - -// This version requires equality operator -template -bool equalityXML(const T& input = T()) { - T output; - roundtripXML(input,output); - return input==output; -} - -// This version requires equals -template -bool equalsXML(const T& input = T()) { - T output; - roundtripXML(input,output); - return input.equals(output); -} - -// This version is for pointers -template -bool equalsDereferencedXML(const T& input = T()) { - T output; - roundtripXML(input,output); - return input->equals(*output); -} - -/* ************************************************************************* */ -// Actual Tests -/* ************************************************************************* */ - -// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using namespace std; -using namespace gtsam; - - -/* ************************************************************************* */ -TEST (Serialization, matrix_vector) { - EXPECT(equality(Vector_(4, 1.0, 2.0, 3.0, 4.0))); - EXPECT(equality(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); - - EXPECT(equalityXML(Vector_(4, 1.0, 2.0, 3.0, 4.0))); - EXPECT(equalityXML(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); -} - -/* ************************************************************************* */ -// Export all classes derived from Value -BOOST_CLASS_EXPORT(gtsam::Cal3_S2) -BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo) -BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) -BOOST_CLASS_EXPORT(gtsam::CalibratedCamera) -BOOST_CLASS_EXPORT(gtsam::Point2) -BOOST_CLASS_EXPORT(gtsam::Point3) -BOOST_CLASS_EXPORT(gtsam::Pose2) -BOOST_CLASS_EXPORT(gtsam::Pose3) -BOOST_CLASS_EXPORT(gtsam::Rot2) -BOOST_CLASS_EXPORT(gtsam::Rot3) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::StereoPoint2) - -/* ************************************************************************* */ -Point3 pt3(1.0, 2.0, 3.0); -Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -Pose3 pose3(rt3, pt3); - -Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); -Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); -Cal3Bundler cal3(1.0, 2.0, 3.0); -Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); -Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); -CalibratedCamera cal5(Pose3(rt3, pt3)); - -PinholeCamera cam1(pose3, cal1); -StereoCamera cam2(pose3, cal4ptr); -StereoPoint2 spt(1.0, 2.0, 3.0); - -/* ************************************************************************* */ -TEST (Serialization, text_geometry) { - EXPECT(equalsObj(Point2(1.0, 2.0))); - EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); - EXPECT(equalsObj(Rot2::fromDegrees(30.0))); - - EXPECT(equalsObj(pt3)); - EXPECT(equalsObj(rt3)); - EXPECT(equalsObj(Pose3(rt3, pt3))); - - EXPECT(equalsObj(cal1)); - EXPECT(equalsObj(cal2)); - EXPECT(equalsObj(cal3)); - EXPECT(equalsObj(cal4)); - EXPECT(equalsObj(cal5)); - - EXPECT(equalsObj(cam1)); - EXPECT(equalsObj(cam2)); - EXPECT(equalsObj(spt)); -} - -/* ************************************************************************* */ -TEST (Serialization, xml_geometry) { - EXPECT(equalsXML(Point2(1.0, 2.0))); - EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); - EXPECT(equalsXML(Rot2::fromDegrees(30.0))); - - EXPECT(equalsXML(pt3)); - EXPECT(equalsXML(rt3)); - EXPECT(equalsXML(Pose3(rt3, pt3))); - - EXPECT(equalsXML(cal1)); - EXPECT(equalsXML(cal2)); - EXPECT(equalsXML(cal3)); - EXPECT(equalsXML(cal4)); - EXPECT(equalsXML(cal5)); - - EXPECT(equalsXML(cam1)); - EXPECT(equalsXML(cam2)); - EXPECT(equalsXML(spt)); -} - -/* ************************************************************************* */ -typedef PinholeCamera PinholeCal3S2; -typedef PinholeCamera PinholeCal3DS2; -typedef PinholeCamera PinholeCal3Bundler; - -TEST (Serialization, TemplatedValues) { - Values values; - values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1)); - values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2)); - values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3)); - values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1)); - EXPECT(equalsObj(values)); - EXPECT(equalsXML(values)); -} - -/* ************************************************************************* */ -// Export 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::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); - -/* ************************************************************************* */ -// example noise models -noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); -noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); -noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); -noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); -noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); - -/* ************************************************************************* */ -TEST (Serialization, noiseModels) { - // tests using pointers to the derived class - EXPECT( equalsDereferenced(diag3)); - EXPECT(equalsDereferencedXML(diag3)); - - EXPECT( equalsDereferenced(gaussian3)); - EXPECT(equalsDereferencedXML(gaussian3)); - - EXPECT( equalsDereferenced(iso3)); - EXPECT(equalsDereferencedXML(iso3)); - - EXPECT( equalsDereferenced(constrained3)); - EXPECT(equalsDereferencedXML(constrained3)); - - EXPECT( equalsDereferenced(unit3)); - EXPECT(equalsDereferencedXML(unit3)); -} - -/* ************************************************************************* */ -TEST (Serialization, SharedNoiseModel_noiseModels) { - SharedNoiseModel diag3_sg = diag3; - EXPECT(equalsDereferenced(diag3_sg)); - EXPECT(equalsDereferencedXML(diag3_sg)); - - EXPECT(equalsDereferenced(diag3)); - EXPECT(equalsDereferencedXML(diag3)); - - EXPECT(equalsDereferenced(iso3)); - EXPECT(equalsDereferencedXML(iso3)); - - EXPECT(equalsDereferenced(gaussian3)); - EXPECT(equalsDereferencedXML(gaussian3)); - - EXPECT(equalsDereferenced(unit3)); - EXPECT(equalsDereferencedXML(unit3)); - - EXPECT(equalsDereferenced(constrained3)); - EXPECT(equalsDereferencedXML(constrained3)); -} - -/* ************************************************************************* */ -TEST (Serialization, SharedDiagonal_noiseModels) { - EXPECT(equalsDereferenced(diag3)); - EXPECT(equalsDereferencedXML(diag3)); - - EXPECT(equalsDereferenced(iso3)); - EXPECT(equalsDereferencedXML(iso3)); - - EXPECT(equalsDereferenced(unit3)); - EXPECT(equalsDereferencedXML(unit3)); - - EXPECT(equalsDereferenced(constrained3)); - EXPECT(equalsDereferencedXML(constrained3)); -} - -/* ************************************************************************* */ -// Linear components -/* ************************************************************************* */ - -/* ************************************************************************* */ -TEST (Serialization, linear_factors) { - VectorValues values; - values.insert(0, Vector_(1, 1.0)); - values.insert(1, Vector_(2, 2.0,3.0)); - values.insert(2, Vector_(2, 4.0,5.0)); - EXPECT(equalsObj(values)); - EXPECT(equalsXML(values)); - - Index i1 = 4, i2 = 7; - Matrix A1 = eye(3), A2 = -1.0 * eye(3); - Vector b = ones(3); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); - JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); - EXPECT(equalsObj(jacobianfactor)); - EXPECT(equalsXML(jacobianfactor)); - - HessianFactor hessianfactor(jacobianfactor); - EXPECT(equalsObj(hessianfactor)); - EXPECT(equalsXML(hessianfactor)); -} - -/* ************************************************************************* */ -TEST (Serialization, gaussian_conditional) { - Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.); - Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4); - Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34); - Vector d(2); d << 0.2, 0.5; - GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2)); - - EXPECT(equalsObj(cg)); - EXPECT(equalsXML(cg)); -} - -/* Create GUIDs for factors */ -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -/* ************************************************************************* */ -TEST (Serialization, smallExample_linear) { - using namespace example; - - Ordering ordering; ordering += "x1","x2","l1"; - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - EXPECT(equalsObj(ordering)); - EXPECT(equalsXML(ordering)); - - EXPECT(equalsObj(fg)); - EXPECT(equalsXML(fg)); - - GaussianBayesNet cbn = createSmallGaussianBayesNet(); - EXPECT(equalsObj(cbn)); - EXPECT(equalsXML(cbn)); -} - -/* ************************************************************************* */ -TEST (Serialization, symbolic_graph) { - Ordering o; o += "x1","l1","x2"; - // construct expected symbolic graph - SymbolicFactorGraph sfg; - sfg.push_factor(o["x1"]); - sfg.push_factor(o["x1"],o["x2"]); - sfg.push_factor(o["x1"],o["l1"]); - sfg.push_factor(o["l1"],o["x2"]); - - EXPECT(equalsObj(sfg)); - EXPECT(equalsXML(sfg)); -} - -/* ************************************************************************* */ -TEST (Serialization, symbolic_bn) { - Ordering o; o += "x2","l1","x1"; - - IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"])); - IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"])); - IndexConditional::shared_ptr x1(new IndexConditional(o["x1"])); - - SymbolicBayesNet sbn; - sbn.push_back(x2); - sbn.push_back(l1); - sbn.push_back(x1); - - EXPECT(equalsObj(sbn)); - EXPECT(equalsXML(sbn)); -} - -/* ************************************************************************* */ -TEST (Serialization, symbolic_bayes_tree ) { - typedef BayesTree SymbolicBayesTree; - static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; - IndexConditional::shared_ptr - B(new IndexConditional(_B_)), - L(new IndexConditional(_L_, _B_)), - E(new IndexConditional(_E_, _L_, _B_)), - S(new IndexConditional(_S_, _L_, _B_)), - T(new IndexConditional(_T_, _E_, _L_)), - X(new IndexConditional(_X_, _E_)); - - // Bayes Tree for Asia example - SymbolicBayesTree bayesTree; - SymbolicBayesTree::insert(bayesTree, B); - SymbolicBayesTree::insert(bayesTree, L); - SymbolicBayesTree::insert(bayesTree, E); - SymbolicBayesTree::insert(bayesTree, S); - SymbolicBayesTree::insert(bayesTree, T); - SymbolicBayesTree::insert(bayesTree, X); - - EXPECT(equalsObj(bayesTree)); - EXPECT(equalsXML(bayesTree)); -} - -/* ************************************************************************* */ -TEST (Serialization, gaussianISAM) { - using namespace example; - Ordering ordering; - GaussianFactorGraph smoother; - boost::tie(smoother, ordering) = createSmoother(7); - BayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); - GaussianISAM isam(bayesTree); - - EXPECT(equalsObj(isam)); - EXPECT(equalsXML(isam)); -} - -/* ************************************************************************* */ -/* Create GUIDs for factors in simulated2D */ -BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" ) -BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" ) -BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement") - -/* ************************************************************************* */ -TEST (Serialization, smallExample) { - using namespace example; - Graph nfg = createNonlinearFactorGraph(); - Values c1 = createValues(); - EXPECT(equalsObj(nfg)); - EXPECT(equalsXML(nfg)); - - EXPECT(equalsObj(c1)); - EXPECT(equalsXML(c1)); -} - -/* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior, "gtsam::planarSLAM::Prior"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing, "gtsam::planarSLAM::Bearing"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Range, "gtsam::planarSLAM::Range"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry, "gtsam::planarSLAM::Odometry"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint"); -/* ************************************************************************* */ -TEST (Serialization, planar_system) { - using namespace planarSLAM; - planarSLAM::Values values; - values.insert(PointKey(3), Point2(1.0, 2.0)); - values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); - - SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); - SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); - SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); - - Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1); - Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1); - Range range(PoseKey(2), PointKey(9), 7.0, model1); - BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2); - Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3); - Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2)); - - Graph graph; - graph.add(prior); - graph.add(bearing); - graph.add(range); - graph.add(bearingRange); - graph.add(odometry); - graph.add(constraint); - - // text - EXPECT(equalsObj(PoseKey(2))); - EXPECT(equalsObj(PointKey(3))); - EXPECT(equalsObj(values)); - EXPECT(equalsObj(prior)); - EXPECT(equalsObj(bearing)); - EXPECT(equalsObj(bearingRange)); - EXPECT(equalsObj(range)); - EXPECT(equalsObj(odometry)); - EXPECT(equalsObj(constraint)); - EXPECT(equalsObj(graph)); - - // xml - EXPECT(equalsXML(PoseKey(2))); - EXPECT(equalsXML(PointKey(3))); - EXPECT(equalsXML(values)); - EXPECT(equalsXML(prior)); - EXPECT(equalsXML(bearing)); - EXPECT(equalsXML(bearingRange)); - EXPECT(equalsXML(range)); - EXPECT(equalsXML(odometry)); - EXPECT(equalsXML(constraint)); - EXPECT(equalsXML(graph)); -} - -/* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor"); - -/* ************************************************************************* */ -TEST (Serialization, visual_system) { - using namespace visualSLAM; - Values values; - Symbol x1('x',1), x2('x',2); - Symbol l1('l',1), l2('l',2); - Pose3 pose1 = pose3, pose2 = pose3.inverse(); - Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); - values.insert(x1, pose1); - values.insert(l1, pt1); - SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); - SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); - SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); - boost::shared_ptr K(new Cal3_S2(cal1)); - - Graph graph; - graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K); - graph.addPointConstraint(1, pt1); - graph.addPointPrior(1, pt2, model3); - graph.addPoseConstraint(1, pose1); - graph.addPosePrior(1, pose2, model6); - - EXPECT(equalsObj(values)); - EXPECT(equalsObj(graph)); - - EXPECT(equalsXML(values)); - EXPECT(equalsXML(graph)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */