122 lines
4.3 KiB
C++
122 lines
4.3 KiB
C++
/**
|
|
* @file testSerialization.cpp
|
|
*
|
|
* @brief Tests for serialization global functions using boost.serialization
|
|
*
|
|
* @date Jun 12, 2013
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <gtsam_unstable/slam/serialization.h>
|
|
|
|
#include <gtsam/geometry/Pose2.h>
|
|
#include <gtsam/geometry/Pose3.h>
|
|
|
|
#include <gtsam/slam/BetweenFactor.h>
|
|
#include <gtsam/sam/BearingRangeFactor.h>
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include <boost/filesystem.hpp>
|
|
|
|
#include <iostream>
|
|
#include <cstdlib>
|
|
#include <fstream>
|
|
#include <sstream>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
namespace fs = boost::filesystem;
|
|
#ifdef TOPSRCDIR
|
|
static string topdir = TOPSRCDIR;
|
|
#else
|
|
static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not defined, we error
|
|
#endif
|
|
|
|
Values exampleValues() {
|
|
Values result;
|
|
result.insert(234, gtsam::Rot2::fromAngle(0.1));
|
|
result.insert(123, gtsam::Point2(1.0, 2.0));
|
|
result.insert(254, gtsam::Pose2(1.0, 2.0, 0.3));
|
|
result.insert(678, gtsam::Rot3::Rx(0.1));
|
|
result.insert(498, gtsam::Point3(1.0, 2.0, 3.0));
|
|
result.insert(345, gtsam::Pose3(Rot3::Rx(0.1), Point3(1.0, 2.0, 3.0)));
|
|
return result;
|
|
}
|
|
|
|
NonlinearFactorGraph exampleGraph() {
|
|
NonlinearFactorGraph graph;
|
|
graph.addPrior(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)));
|
|
graph.add(BetweenFactor<Pose2>(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))));
|
|
graph.add(BearingRangeFactor<Pose2,Point2>(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2))));
|
|
return graph;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testSerialization, text_graph_serialization ) {
|
|
NonlinearFactorGraph graph = exampleGraph();
|
|
string serialized = serializeGraph(graph);
|
|
NonlinearFactorGraph actGraph = *deserializeGraph(serialized);
|
|
EXPECT(assert_equal(graph, actGraph));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testSerialization, xml_graph_serialization ) {
|
|
NonlinearFactorGraph graph = exampleGraph();
|
|
string serialized = serializeGraphXML(graph, "graph1");
|
|
NonlinearFactorGraph actGraph = *deserializeGraphXML(serialized, "graph1");
|
|
EXPECT(assert_equal(graph, actGraph));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testSerialization, text_values_serialization ) {
|
|
Values values = exampleValues();
|
|
string serialized = serializeValues(values);
|
|
Values actValues = *deserializeValues(serialized);
|
|
EXPECT(assert_equal(values, actValues));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testSerialization, xml_values_serialization ) {
|
|
Values values = exampleValues();
|
|
string serialized = serializeValuesXML(values, "values1");
|
|
Values actValues = *deserializeValuesXML(serialized, "values1");
|
|
EXPECT(assert_equal(values, actValues, 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testSerialization, serialization_file ) {
|
|
// Create files in folder in build folder
|
|
fs::remove_all("actual");
|
|
fs::create_directory("actual");
|
|
string path = "actual/";
|
|
|
|
NonlinearFactorGraph graph = exampleGraph();
|
|
Values values = exampleValues();
|
|
|
|
// Serialize objects using each configuration
|
|
EXPECT(serializeGraphToFile(graph, path + "graph.dat"));
|
|
EXPECT(serializeGraphToXMLFile(graph, path + "graph.xml", "graph1"));
|
|
|
|
EXPECT(serializeValuesToFile(values, path + "values.dat"));
|
|
EXPECT(serializeValuesToXMLFile(values, path + "values.xml", "values1"));
|
|
|
|
// Deserialize
|
|
NonlinearFactorGraph actGraph = *deserializeGraphFromFile(path + "graph.dat");
|
|
NonlinearFactorGraph actGraphXML = *deserializeGraphFromXMLFile(path + "graph.xml", "graph1");
|
|
|
|
Values actValues = *deserializeValuesFromFile(path + "values.dat");
|
|
Values actValuesXML = *deserializeValuesFromXMLFile(path + "values.xml", "values1");
|
|
|
|
// Verify
|
|
EXPECT(assert_equal(graph, actGraph));
|
|
EXPECT(assert_equal(graph, actGraphXML));
|
|
|
|
EXPECT(assert_equal(values, actValues));
|
|
EXPECT(assert_equal(values, actValuesXML));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|