Split up serialization unit tests into subdirectories to speed up
Richard Roberts
parent
3d40f5e6fc
commit
669c7c8dc8
|
|
@ -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
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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 <sstream>
|
||||
#include <string>
|
||||
|
||||
// includes for standard serialization types
|
||||
#include <boost/serialization/export.hpp>
|
||||
#include <boost/serialization/optional.hpp>
|
||||
#include <boost/serialization/shared_ptr.hpp>
|
||||
#include <boost/serialization/vector.hpp>
|
||||
#include <boost/serialization/map.hpp>
|
||||
#include <boost/serialization/list.hpp>
|
||||
#include <boost/serialization/deque.hpp>
|
||||
#include <boost/serialization/weak_ptr.hpp>
|
||||
|
||||
#include <boost/archive/text_oarchive.hpp>
|
||||
#include <boost/archive/text_iarchive.hpp>
|
||||
#include <boost/archive/xml_iarchive.hpp>
|
||||
#include <boost/archive/xml_oarchive.hpp>
|
||||
|
||||
// whether to print the serialized text to stdout
|
||||
const bool verbose = false;
|
||||
|
||||
namespace gtsam { namespace serializationTestHelpers {
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Serialization testing code.
|
||||
/* ************************************************************************* */
|
||||
|
||||
template<class T>
|
||||
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<class T>
|
||||
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<class T>
|
||||
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<class T>
|
||||
bool equality(const T& input = T()) {
|
||||
T output;
|
||||
roundtrip<T>(input,output);
|
||||
return input==output;
|
||||
}
|
||||
|
||||
// This version requires equals
|
||||
template<class T>
|
||||
bool equalsObj(const T& input = T()) {
|
||||
T output;
|
||||
roundtrip<T>(input,output);
|
||||
return input.equals(output);
|
||||
}
|
||||
|
||||
// De-referenced version for pointers
|
||||
template<class T>
|
||||
bool equalsDereferenced(const T& input) {
|
||||
T output;
|
||||
roundtrip<T>(input,output);
|
||||
return input->equals(*output);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class T>
|
||||
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<class T>
|
||||
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<class T>
|
||||
void roundtripXML(const T& input, T& output) {
|
||||
// Serialize
|
||||
std::string serialized = serializeXML<T>(input);
|
||||
if (verbose) std::cout << serialized << std::endl << std::endl;
|
||||
|
||||
// De-serialize
|
||||
deserializeXML(serialized, output);
|
||||
}
|
||||
|
||||
// This version requires equality operator
|
||||
template<class T>
|
||||
bool equalityXML(const T& input = T()) {
|
||||
T output;
|
||||
roundtripXML<T>(input,output);
|
||||
return input==output;
|
||||
}
|
||||
|
||||
// This version requires equals
|
||||
template<class T>
|
||||
bool equalsXML(const T& input = T()) {
|
||||
T output;
|
||||
roundtripXML<T>(input,output);
|
||||
return input.equals(output);
|
||||
}
|
||||
|
||||
// This version is for pointers
|
||||
template<class T>
|
||||
bool equalsDereferencedXML(const T& input = T()) {
|
||||
T output;
|
||||
roundtripXML<T>(input,output);
|
||||
return input->equals(*output);
|
||||
}
|
||||
|
||||
} }
|
||||
|
|
@ -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 <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace gtsam::serializationTestHelpers;
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, matrix_vector) {
|
||||
EXPECT(equality<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
|
||||
EXPECT(equality<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
|
||||
|
||||
EXPECT(equalityXML<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
|
||||
EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Cal3_S2Stereo.h>
|
||||
#include <gtsam/geometry/CalibratedCamera.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/StereoCamera.h>
|
||||
#include <gtsam/geometry/StereoPoint2.h>
|
||||
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
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<Cal3_S2>)
|
||||
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
|
||||
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
|
||||
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<Cal3_S2> cam1(pose3, cal1);
|
||||
StereoCamera cam2(pose3, cal4ptr);
|
||||
StereoPoint2 spt(1.0, 2.0, 3.0);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, text_geometry) {
|
||||
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||
|
||||
EXPECT(equalsObj(pt3));
|
||||
EXPECT(equalsObj<gtsam::Rot3>(rt3));
|
||||
EXPECT(equalsObj<gtsam::Pose3>(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<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||
|
||||
EXPECT(equalsXML<gtsam::Point3>(pt3));
|
||||
EXPECT(equalsXML<gtsam::Rot3>(rt3));
|
||||
EXPECT(equalsXML<gtsam::Pose3>(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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/inference/IndexConditional.h>
|
||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
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<IndexConditional> 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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam/linear/GaussianISAM.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/linear/SharedNoiseModel.h>
|
||||
#include <gtsam/linear/SharedDiagonal.h>
|
||||
#include <gtsam/linear/SharedGaussian.h>
|
||||
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
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<noiseModel::Diagonal::shared_ptr>(diag3));
|
||||
EXPECT(equalsDereferencedXML<noiseModel::Diagonal::shared_ptr>(diag3));
|
||||
|
||||
EXPECT( equalsDereferenced<noiseModel::Gaussian::shared_ptr>(gaussian3));
|
||||
EXPECT(equalsDereferencedXML<noiseModel::Gaussian::shared_ptr>(gaussian3));
|
||||
|
||||
EXPECT( equalsDereferenced<noiseModel::Isotropic::shared_ptr>(iso3));
|
||||
EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
|
||||
|
||||
EXPECT( equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
|
||||
EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
|
||||
|
||||
EXPECT( equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
|
||||
EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, SharedNoiseModel_noiseModels) {
|
||||
SharedNoiseModel diag3_sg = diag3;
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3_sg));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3_sg));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(iso3));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(iso3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(gaussian3));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(gaussian3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(unit3));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(unit3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, SharedDiagonal_noiseModels) {
|
||||
EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
|
||||
EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedDiagonal>(iso3));
|
||||
EXPECT(equalsDereferencedXML<SharedDiagonal>(iso3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedDiagonal>(unit3));
|
||||
EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
|
||||
EXPECT(equalsDereferencedXML<SharedDiagonal>(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<VectorValues>(values));
|
||||
EXPECT(equalsXML<VectorValues>(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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
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<Cal3_S2>)
|
||||
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
|
||||
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
|
||||
|
||||
/* ************************************************************************* */
|
||||
typedef PinholeCamera<Cal3_S2> PinholeCal3S2;
|
||||
typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
|
||||
typedef PinholeCamera<Cal3Bundler> 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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/linear/GaussianISAM.h>
|
||||
#include <gtsam/linear/GaussianMultifrontalSolver.h>
|
||||
#include <gtsam/slam/smallExample.h>
|
||||
#include <gtsam/slam/planarSLAM.h>
|
||||
#include <gtsam/slam/visualSLAM.h>
|
||||
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
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<GaussianConditional> 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<Symbol>(PoseKey(2)));
|
||||
EXPECT(equalsObj<Symbol>(PointKey(3)));
|
||||
EXPECT(equalsObj<planarSLAM::Values>(values));
|
||||
EXPECT(equalsObj<Prior>(prior));
|
||||
EXPECT(equalsObj<Bearing>(bearing));
|
||||
EXPECT(equalsObj<BearingRange>(bearingRange));
|
||||
EXPECT(equalsObj<Range>(range));
|
||||
EXPECT(equalsObj<Odometry>(odometry));
|
||||
EXPECT(equalsObj<Constraint>(constraint));
|
||||
EXPECT(equalsObj<Graph>(graph));
|
||||
|
||||
// xml
|
||||
EXPECT(equalsXML<Symbol>(PoseKey(2)));
|
||||
EXPECT(equalsXML<Symbol>(PointKey(3)));
|
||||
EXPECT(equalsXML<planarSLAM::Values>(values));
|
||||
EXPECT(equalsXML<Prior>(prior));
|
||||
EXPECT(equalsXML<Bearing>(bearing));
|
||||
EXPECT(equalsXML<BearingRange>(bearingRange));
|
||||
EXPECT(equalsXML<Range>(range));
|
||||
EXPECT(equalsXML<Odometry>(odometry));
|
||||
EXPECT(equalsXML<Constraint>(constraint));
|
||||
EXPECT(equalsXML<Graph>(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<Cal3_S2> 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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <sstream>
|
||||
#include <string>
|
||||
|
||||
// includes for standard serialization types
|
||||
#include <boost/serialization/export.hpp>
|
||||
#include <boost/serialization/optional.hpp>
|
||||
#include <boost/serialization/shared_ptr.hpp>
|
||||
#include <boost/serialization/vector.hpp>
|
||||
#include <boost/serialization/map.hpp>
|
||||
#include <boost/serialization/list.hpp>
|
||||
#include <boost/serialization/deque.hpp>
|
||||
#include <boost/serialization/weak_ptr.hpp>
|
||||
|
||||
#include <boost/archive/text_oarchive.hpp>
|
||||
#include <boost/archive/text_iarchive.hpp>
|
||||
#include <boost/archive/xml_iarchive.hpp>
|
||||
#include <boost/archive/xml_oarchive.hpp>
|
||||
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
// whether to print the serialized text to stdout
|
||||
const bool verbose = false;
|
||||
|
||||
template<class T>
|
||||
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<class T>
|
||||
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<class T>
|
||||
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<class T>
|
||||
bool equality(const T& input = T()) {
|
||||
T output;
|
||||
roundtrip<T>(input,output);
|
||||
return input==output;
|
||||
}
|
||||
|
||||
// This version requires equals
|
||||
template<class T>
|
||||
bool equalsObj(const T& input = T()) {
|
||||
T output;
|
||||
roundtrip<T>(input,output);
|
||||
return input.equals(output);
|
||||
}
|
||||
|
||||
// De-referenced version for pointers
|
||||
template<class T>
|
||||
bool equalsDereferenced(const T& input) {
|
||||
T output;
|
||||
roundtrip<T>(input,output);
|
||||
return input->equals(*output);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class T>
|
||||
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<class T>
|
||||
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<class T>
|
||||
void roundtripXML(const T& input, T& output) {
|
||||
// Serialize
|
||||
std::string serialized = serializeXML<T>(input);
|
||||
if (verbose) std::cout << serialized << std::endl << std::endl;
|
||||
|
||||
// De-serialize
|
||||
deserializeXML(serialized, output);
|
||||
}
|
||||
|
||||
// This version requires equality operator
|
||||
template<class T>
|
||||
bool equalityXML(const T& input = T()) {
|
||||
T output;
|
||||
roundtripXML<T>(input,output);
|
||||
return input==output;
|
||||
}
|
||||
|
||||
// This version requires equals
|
||||
template<class T>
|
||||
bool equalsXML(const T& input = T()) {
|
||||
T output;
|
||||
roundtripXML<T>(input,output);
|
||||
return input.equals(output);
|
||||
}
|
||||
|
||||
// This version is for pointers
|
||||
template<class T>
|
||||
bool equalsDereferencedXML(const T& input = T()) {
|
||||
T output;
|
||||
roundtripXML<T>(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 <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam/linear/GaussianISAM.h>
|
||||
#include <gtsam/slam/smallExample.h>
|
||||
#include <gtsam/slam/planarSLAM.h>
|
||||
#include <gtsam/slam/pose3SLAM.h>
|
||||
#include <gtsam/slam/visualSLAM.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, matrix_vector) {
|
||||
EXPECT(equality<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
|
||||
EXPECT(equality<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
|
||||
|
||||
EXPECT(equalityXML<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
|
||||
EXPECT(equalityXML<Matrix>(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<Cal3_S2>)
|
||||
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
|
||||
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
|
||||
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<Cal3_S2> cam1(pose3, cal1);
|
||||
StereoCamera cam2(pose3, cal4ptr);
|
||||
StereoPoint2 spt(1.0, 2.0, 3.0);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, text_geometry) {
|
||||
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||
|
||||
EXPECT(equalsObj(pt3));
|
||||
EXPECT(equalsObj<gtsam::Rot3>(rt3));
|
||||
EXPECT(equalsObj<gtsam::Pose3>(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<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||
|
||||
EXPECT(equalsXML<gtsam::Point3>(pt3));
|
||||
EXPECT(equalsXML<gtsam::Rot3>(rt3));
|
||||
EXPECT(equalsXML<gtsam::Pose3>(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<Cal3_S2> PinholeCal3S2;
|
||||
typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
|
||||
typedef PinholeCamera<Cal3Bundler> 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<noiseModel::Diagonal::shared_ptr>(diag3));
|
||||
EXPECT(equalsDereferencedXML<noiseModel::Diagonal::shared_ptr>(diag3));
|
||||
|
||||
EXPECT( equalsDereferenced<noiseModel::Gaussian::shared_ptr>(gaussian3));
|
||||
EXPECT(equalsDereferencedXML<noiseModel::Gaussian::shared_ptr>(gaussian3));
|
||||
|
||||
EXPECT( equalsDereferenced<noiseModel::Isotropic::shared_ptr>(iso3));
|
||||
EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
|
||||
|
||||
EXPECT( equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
|
||||
EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
|
||||
|
||||
EXPECT( equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
|
||||
EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, SharedNoiseModel_noiseModels) {
|
||||
SharedNoiseModel diag3_sg = diag3;
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3_sg));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3_sg));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(iso3));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(iso3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(gaussian3));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(gaussian3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(unit3));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(unit3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
|
||||
EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, SharedDiagonal_noiseModels) {
|
||||
EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
|
||||
EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedDiagonal>(iso3));
|
||||
EXPECT(equalsDereferencedXML<SharedDiagonal>(iso3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedDiagonal>(unit3));
|
||||
EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
|
||||
|
||||
EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
|
||||
EXPECT(equalsDereferencedXML<SharedDiagonal>(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<VectorValues>(values));
|
||||
EXPECT(equalsXML<VectorValues>(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<IndexConditional> 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<GaussianConditional> 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<Symbol>(PoseKey(2)));
|
||||
EXPECT(equalsObj<Symbol>(PointKey(3)));
|
||||
EXPECT(equalsObj<planarSLAM::Values>(values));
|
||||
EXPECT(equalsObj<Prior>(prior));
|
||||
EXPECT(equalsObj<Bearing>(bearing));
|
||||
EXPECT(equalsObj<BearingRange>(bearingRange));
|
||||
EXPECT(equalsObj<Range>(range));
|
||||
EXPECT(equalsObj<Odometry>(odometry));
|
||||
EXPECT(equalsObj<Constraint>(constraint));
|
||||
EXPECT(equalsObj<Graph>(graph));
|
||||
|
||||
// xml
|
||||
EXPECT(equalsXML<Symbol>(PoseKey(2)));
|
||||
EXPECT(equalsXML<Symbol>(PointKey(3)));
|
||||
EXPECT(equalsXML<planarSLAM::Values>(values));
|
||||
EXPECT(equalsXML<Prior>(prior));
|
||||
EXPECT(equalsXML<Bearing>(bearing));
|
||||
EXPECT(equalsXML<BearingRange>(bearingRange));
|
||||
EXPECT(equalsXML<Range>(range));
|
||||
EXPECT(equalsXML<Odometry>(odometry));
|
||||
EXPECT(equalsXML<Constraint>(constraint));
|
||||
EXPECT(equalsXML<Graph>(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<Cal3_S2> 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); }
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue