Reworked testSerialization so that it works for geometry and simple planar objects. Currently only tested in boost 1.40, probably doesn't work under boost 1.44
parent
a461680a2b
commit
f594ebf562
|
@ -13,9 +13,9 @@
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
// includes for standard serialization types
|
// includes for standard serialization types
|
||||||
//#include <boost/serialization/vector.hpp>
|
#include <boost/serialization/vector.hpp>
|
||||||
//#include <boost/serialization/map.hpp>
|
#include <boost/serialization/map.hpp>
|
||||||
//#include <boost/serialization/list.hpp>
|
#include <boost/serialization/list.hpp>
|
||||||
|
|
||||||
#include <boost/archive/text_oarchive.hpp>
|
#include <boost/archive/text_oarchive.hpp>
|
||||||
#include <boost/archive/text_iarchive.hpp>
|
#include <boost/archive/text_iarchive.hpp>
|
||||||
|
@ -28,33 +28,28 @@ const bool verbose = false;
|
||||||
// Templated round-trip serialization
|
// Templated round-trip serialization
|
||||||
template<class T>
|
template<class T>
|
||||||
void roundtrip(const T& input, T& output) {
|
void roundtrip(const T& input, T& output) {
|
||||||
|
|
||||||
// Serialize
|
// Serialize
|
||||||
std::ostringstream out_archive_stream;
|
std::ostringstream out_archive_stream;
|
||||||
|
{
|
||||||
boost::archive::text_oarchive out_archive(out_archive_stream);
|
boost::archive::text_oarchive out_archive(out_archive_stream);
|
||||||
out_archive << input;
|
out_archive << input;
|
||||||
|
}
|
||||||
|
|
||||||
// Convert to string
|
// Convert to string
|
||||||
std::string serialized = out_archive_stream.str();
|
std::string serialized = out_archive_stream.str();
|
||||||
if (verbose) std::cout << serialized << std::endl << std::endl;
|
if (verbose) std::cout << serialized << std::endl << std::endl;
|
||||||
|
|
||||||
// De-serialize
|
// De-serialize
|
||||||
|
{
|
||||||
std::istringstream in_archive_stream(serialized);
|
std::istringstream in_archive_stream(serialized);
|
||||||
boost::archive::text_iarchive in_archive(in_archive_stream);
|
boost::archive::text_iarchive in_archive(in_archive_stream);
|
||||||
in_archive >> output;
|
in_archive >> output;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// This version requires equality operator
|
// This version requires equality operator
|
||||||
template<class T>
|
template<class T>
|
||||||
bool equality() {
|
bool equality(const T& input = T()) {
|
||||||
T input;
|
|
||||||
T output;
|
|
||||||
roundtrip<T>(input,output);
|
|
||||||
return input==output;
|
|
||||||
}
|
|
||||||
|
|
||||||
template<class T>
|
|
||||||
bool equality(const T& input) {
|
|
||||||
T output;
|
T output;
|
||||||
roundtrip<T>(input,output);
|
roundtrip<T>(input,output);
|
||||||
return input==output;
|
return input==output;
|
||||||
|
@ -62,15 +57,7 @@ bool equality(const T& input) {
|
||||||
|
|
||||||
// This version requires equals
|
// This version requires equals
|
||||||
template<class T>
|
template<class T>
|
||||||
bool equalsEmpty() {
|
bool equalsObj(const T& input = T()) {
|
||||||
T input;
|
|
||||||
T output;
|
|
||||||
roundtrip<T>(input,output);
|
|
||||||
return input.equals(output);
|
|
||||||
}
|
|
||||||
|
|
||||||
template<class T>
|
|
||||||
bool equalsObj(const T& input) {
|
|
||||||
T output;
|
T output;
|
||||||
roundtrip<T>(input,output);
|
roundtrip<T>(input,output);
|
||||||
return input.equals(output);
|
return input.equals(output);
|
||||||
|
@ -88,33 +75,27 @@ bool equalsDereferenced(const T& input) {
|
||||||
// Templated round-trip serialization using XML
|
// Templated round-trip serialization using XML
|
||||||
template<class T>
|
template<class T>
|
||||||
void roundtripXML(const T& input, T& output) {
|
void roundtripXML(const T& input, T& output) {
|
||||||
|
|
||||||
// Serialize
|
// Serialize
|
||||||
|
std::string serialized;
|
||||||
|
{
|
||||||
std::ostringstream out_archive_stream;
|
std::ostringstream out_archive_stream;
|
||||||
boost::archive::xml_oarchive out_archive(out_archive_stream);
|
boost::archive::xml_oarchive out_archive(out_archive_stream);
|
||||||
out_archive << BOOST_SERIALIZATION_NVP(input);
|
out_archive << boost::serialization::make_nvp("data", input);
|
||||||
|
|
||||||
// Convert to string
|
// Convert to string
|
||||||
std::string serialized = out_archive_stream.str();
|
serialized = out_archive_stream.str();
|
||||||
|
}
|
||||||
if (verbose) std::cout << serialized << std::endl << std::endl;
|
if (verbose) std::cout << serialized << std::endl << std::endl;
|
||||||
|
|
||||||
// De-serialize
|
// De-serialize
|
||||||
std::istringstream in_archive_stream(serialized);
|
std::istringstream in_archive_stream(serialized);
|
||||||
boost::archive::xml_iarchive in_archive(in_archive_stream);
|
boost::archive::xml_iarchive in_archive(in_archive_stream);
|
||||||
in_archive >> BOOST_SERIALIZATION_NVP(output);
|
in_archive >> boost::serialization::make_nvp("data", output);
|
||||||
}
|
}
|
||||||
|
|
||||||
// This version requires equality operator
|
// This version requires equality operator
|
||||||
template<class T>
|
template<class T>
|
||||||
bool equalityXML() {
|
bool equalityXML(const T& input = T()) {
|
||||||
T input;
|
|
||||||
T output;
|
|
||||||
roundtripXML<T>(input,output);
|
|
||||||
return input==output;
|
|
||||||
}
|
|
||||||
|
|
||||||
template<class T>
|
|
||||||
bool equalityXML(const T& input) {
|
|
||||||
T output;
|
T output;
|
||||||
roundtripXML<T>(input,output);
|
roundtripXML<T>(input,output);
|
||||||
return input==output;
|
return input==output;
|
||||||
|
@ -122,15 +103,7 @@ bool equalityXML(const T& input) {
|
||||||
|
|
||||||
// This version requires equals
|
// This version requires equals
|
||||||
template<class T>
|
template<class T>
|
||||||
bool equalsXML() {
|
bool equalsXML(const T& input = T()) {
|
||||||
T input;
|
|
||||||
T output;
|
|
||||||
roundtripXML<T>(input,output);
|
|
||||||
return input.equals(output);
|
|
||||||
}
|
|
||||||
|
|
||||||
template<class T>
|
|
||||||
bool equalsXML(const T& input) {
|
|
||||||
T output;
|
T output;
|
||||||
roundtripXML<T>(input,output);
|
roundtripXML<T>(input,output);
|
||||||
return input.equals(output);
|
return input.equals(output);
|
||||||
|
@ -141,79 +114,71 @@ bool equalsXML(const T& input) {
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
||||||
#include <gtsam/geometry/Point2.h>
|
#include <gtsam/geometry/Point2.h>
|
||||||
//#include <gtsam/geometry/Pose2.h>
|
#include <gtsam/geometry/Pose2.h>
|
||||||
//#include <gtsam/geometry/Rot2.h>
|
#include <gtsam/geometry/Rot2.h>
|
||||||
//#include <gtsam/geometry/Point3.h>
|
#include <gtsam/geometry/Point3.h>
|
||||||
//#include <gtsam/geometry/Pose3.h>
|
#include <gtsam/geometry/Pose3.h>
|
||||||
//#include <gtsam/geometry/Rot3.h>
|
#include <gtsam/geometry/Rot3.h>
|
||||||
//#include <gtsam/geometry/Cal3_S2.h>
|
#include <gtsam/geometry/Cal3_S2.h>
|
||||||
|
|
||||||
//#include <gtsam/linear/VectorConfig.h>
|
//#include <gtsam/linear/VectorConfig.h>
|
||||||
//#include <gtsam/inference/FactorGraph-inl.h>
|
|
||||||
//#include <gtsam/linear/GaussianConditional.h>
|
//#include <gtsam/linear/GaussianConditional.h>
|
||||||
//#include <gtsam/inference/SymbolicConditional.h>
|
//#include <gtsam/inference/SymbolicConditional.h>
|
||||||
|
|
||||||
|
#include <gtsam/slam/planarSLAM.h>
|
||||||
|
|
||||||
#include <gtsam/CppUnitLite/TestHarness.h>
|
#include <gtsam/CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
//TEST( Point2, equalsEmpty) { CHECK(equalsEmpty<gtsam::Point2>());}
|
TEST (Serialization, text_geometry) {
|
||||||
//TEST( VectorConfig, equalsEmpty) { CHECK(equalsEmpty<VectorConfig>());}
|
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||||
//TEST( GaussianConditional, equalsEmpty) { CHECK(equalsEmpty<GaussianConditional>());}
|
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||||
|
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||||
|
EXPECT(equalsObj<gtsam::Point3>(Point3(1.0, 2.0, 3.0)));
|
||||||
|
EXPECT(equalsObj<gtsam::Pose3>());
|
||||||
|
EXPECT(equalsObj<gtsam::Rot3>(Rot3::RzRyRx(1.0, 3.0, 2.0)));
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// Testing XML
|
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>(Point3(1.0, 2.0, 3.0)));
|
||||||
|
EXPECT(equalsXML<gtsam::Pose3>());
|
||||||
|
EXPECT(equalsXML<gtsam::Rot3>(Rot3::RzRyRx(1.0, 3.0, 2.0)));
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
//TEST( Point2, equalsXML) { CHECK(equalsXML<gtsam::Point2>());}
|
TEST (Serialization, text_linear) {
|
||||||
|
// NOT WORKING:
|
||||||
|
// EXPECT(equalsObj<VectorConfig>());
|
||||||
|
// EXPECT(equalsObj<GaussianConditional>());
|
||||||
|
}
|
||||||
|
|
||||||
//TEST( VectorConfig, equalsXML) { CHECK(equalsXML<VectorConfig>());}
|
/* ************************************************************************* */
|
||||||
//TEST( Cal3_S2, equalsXML) { CHECK(equalsXML<Cal3_S2>());}
|
TEST (Serialization, xml_linear) {
|
||||||
//TEST( GaussianConditional, equalsXML) { CHECK(equalsXML<GaussianConditional>());}
|
// NOT WORKING:
|
||||||
//TEST( SymbolicConditional, equalsXML) { CHECK(equalsXML<SymbolicConditional>());}
|
// EXPECT(equalsXML<VectorConfig>());
|
||||||
|
// EXPECT(equalsXML<GaussianConditional>());
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST (Serialization, text_planar) {
|
||||||
|
EXPECT(equalsObj<gtsam::planarSLAM::PoseKey>(gtsam::planarSLAM::PoseKey(2)));
|
||||||
|
EXPECT(equalsObj<gtsam::planarSLAM::PointKey>(gtsam::planarSLAM::PointKey(2)));
|
||||||
|
EXPECT(equalsObj<gtsam::planarSLAM::Config>());
|
||||||
|
}
|
||||||
|
|
||||||
// The two tests below will not run, however, as CameraMarkerFactor
|
/* ************************************************************************* */
|
||||||
// is not registered by the serialization code in FactorGraph
|
TEST (Serialization, xml_planar) {
|
||||||
//TEST( FactorGraph, equals1)
|
EXPECT(equalsXML<gtsam::planarSLAM::PoseKey>(gtsam::planarSLAM::PoseKey(2)));
|
||||||
//{
|
EXPECT(equalsXML<gtsam::planarSLAM::PointKey>(gtsam::planarSLAM::PointKey(2)));
|
||||||
// FactorGraph<NonlinearFactor> graph;
|
EXPECT(equalsXML<gtsam::planarSLAM::Config>());
|
||||||
// graph.push_back(f);
|
}
|
||||||
// CHECK(equalsObj(graph));
|
|
||||||
//}
|
|
||||||
//TEST( NonlinearFactorGraph, equals)
|
|
||||||
//{
|
|
||||||
// NonlinearFactorGraph graph;
|
|
||||||
// graph.push_back(f);
|
|
||||||
// CHECK(equalsObj(graph));
|
|
||||||
//}
|
|
||||||
|
|
||||||
// It *does* work if we explicitly instantiate with the factor type
|
|
||||||
//TEST( FactorGraph, equals2)
|
|
||||||
//{
|
|
||||||
// FactorGraph<CameraMarkerFactor> graph;
|
|
||||||
// graph.push_back(f);
|
|
||||||
// CHECK(equalsObj(graph));
|
|
||||||
//}
|
|
||||||
|
|
||||||
// And, as we explicitly registered the three CameraMarkerFactor types in
|
|
||||||
// the EasySLAMGraph serialize, this will also work :-))
|
|
||||||
// see http://www.boost.org/doc/libs/1_35_0/libs/serialization/doc/serialization.html#derivedpointers
|
|
||||||
//TEST( EasySLAMGraph, equals2)
|
|
||||||
//{
|
|
||||||
// EasySLAMGraph graph = createExampleGraph();
|
|
||||||
// CHECK(equalsObj(graph));
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//// EasySLAMConfig is no problem either:
|
|
||||||
//TEST( EasySLAMConfig, equals2)
|
|
||||||
//{
|
|
||||||
// EasySLAMConfig c = createExampleConfig();
|
|
||||||
// CHECK(equalsObj(c));
|
|
||||||
//}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() {
|
int main() {
|
||||||
|
|
Loading…
Reference in New Issue