testSerialization.cpp
Go to the documentation of this file.
1 
11 #include <iostream>
12 
13 #include <gtsam/slam/serialization.h>
14 
15 #include <gtsam/geometry/Pose2.h>
16 #include <gtsam/geometry/Pose3.h>
17 
20 
21 #include <stdlib.h>
22 #include <fstream>
23 #include <sstream>
24 #include <boost/assign/std/vector.hpp>
25 #include <boost/filesystem.hpp>
26 
27 using namespace std;
28 using namespace gtsam;
29 using namespace boost::assign;
30 namespace fs = boost::filesystem;
31 #ifdef TOPSRCDIR
32 static string topdir = TOPSRCDIR;
33 #else
34 static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not defined, we error
35 #endif
36 
38  Values result;
39  result.insert(234, gtsam::Rot2::fromAngle(0.1));
40  result.insert(123, gtsam::Point2(1.0, 2.0));
41  result.insert(254, gtsam::Pose2(1.0, 2.0, 0.3));
42  result.insert(678, gtsam::Rot3::Rx(0.1));
43  result.insert(498, gtsam::Point3(1.0, 2.0, 3.0));
44  result.insert(345, gtsam::Pose3(Rot3::Rx(0.1), Point3(1.0, 2.0, 3.0)));
45  return result;
46 }
47 
50  graph.addPrior(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)));
51  graph.add(BetweenFactor<Pose2>(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))));
52  graph.add(BearingRangeFactor<Pose2,Point2>(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2))));
53  return graph;
54 }
55 
56 /* ************************************************************************* */
57 TEST( testSerialization, text_graph_serialization ) {
59  string serialized = serializeGraph(graph);
60  NonlinearFactorGraph actGraph = *deserializeGraph(serialized);
61  EXPECT(assert_equal(graph, actGraph));
62 }
63 
64 /* ************************************************************************* */
65 TEST( testSerialization, xml_graph_serialization ) {
67  string serialized = serializeGraphXML(graph, "graph1");
68  NonlinearFactorGraph actGraph = *deserializeGraphXML(serialized, "graph1");
69  EXPECT(assert_equal(graph, actGraph));
70 }
71 
72 /* ************************************************************************* */
73 TEST( testSerialization, text_values_serialization ) {
75  string serialized = serializeValues(values);
76  Values actValues = *deserializeValues(serialized);
77  EXPECT(assert_equal(values, actValues));
78 }
79 
80 /* ************************************************************************* */
81 TEST( testSerialization, xml_values_serialization ) {
83  string serialized = serializeValuesXML(values, "values1");
84  Values actValues = *deserializeValuesXML(serialized, "values1");
85  EXPECT(assert_equal(values, actValues, 1e-5));
86 }
87 
88 /* ************************************************************************* */
89 TEST( testSerialization, serialization_file ) {
90  // Create files in folder in build folder
91  fs::remove_all("actual");
92  fs::create_directory("actual");
93  string path = "actual/";
94 
97 
98  // Serialize objects using each configuration
99  EXPECT(serializeGraphToFile(graph, path + "graph.dat"));
100  EXPECT(serializeGraphToXMLFile(graph, path + "graph.xml", "graph1"));
101 
102  EXPECT(serializeValuesToFile(values, path + "values.dat"));
103  EXPECT(serializeValuesToXMLFile(values, path + "values.xml", "values1"));
104 
105  // Deserialize
106  NonlinearFactorGraph actGraph = *deserializeGraphFromFile(path + "graph.dat");
107  NonlinearFactorGraph actGraphXML = *deserializeGraphFromXMLFile(path + "graph.xml", "graph1");
108 
109  Values actValues = *deserializeValuesFromFile(path + "values.dat");
110  Values actValuesXML = *deserializeValuesFromXMLFile(path + "values.xml", "values1");
111 
112  // Verify
113  EXPECT(assert_equal(graph, actGraph));
114  EXPECT(assert_equal(graph, actGraphXML));
115 
116  EXPECT(assert_equal(values, actValues));
117  EXPECT(assert_equal(values, actValuesXML));
118 }
119 
120 /* ************************************************************************* */
121 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
122 /* ************************************************************************* */
NonlinearFactorGraph::shared_ptr deserializeGraphFromXMLFile(const std::string &fname, const std::string &name="graph")
Values::shared_ptr deserializeValuesFromXMLFile(const std::string &fname, const std::string &name="values")
static int runAllTests(TestResult &result)
void insert(Key j, const Value &val)
Definition: Values.cpp:140
NonlinearFactorGraph::shared_ptr deserializeGraph(const std::string &serialized_graph)
Vector2 Point2
Definition: Point2.h:27
leaf::MyValues values
int main()
Definition: Half.h:150
Values::shared_ptr deserializeValues(const std::string &serialized_values)
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:190
NonlinearFactorGraph graph
bool serializeGraphToFile(const NonlinearFactorGraph &graph, const std::string &fname)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
bool serializeGraphToXMLFile(const NonlinearFactorGraph &graph, const std::string &fname, const std::string &name="graph")
static Rot3 Rx(double t)
Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when look...
Definition: Rot3M.cpp:57
Values::shared_ptr deserializeValuesXML(const std::string &serialized_values, const std::string &name="values")
Values::shared_ptr deserializeValuesFromFile(const std::string &fname)
static string topdir
Values result
Values exampleValues()
#define EXPECT(condition)
Definition: Test.h:151
Array< double, 1, 3 > e(1./3., 0.5, 2.)
std::string serializeGraph(const NonlinearFactorGraph &graph)
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
std::string serializeGraphXML(const NonlinearFactorGraph &graph, const std::string &name="graph")
NonlinearFactorGraph::shared_ptr deserializeGraphXML(const std::string &serialized_graph, const std::string &name="graph")
NonlinearFactorGraph::shared_ptr deserializeGraphFromFile(const std::string &fname)
TEST(testSerialization, text_graph_serialization)
std::string serializeValues(const Values &values)
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:58
std::string serializeValuesXML(const Values &values, const std::string &name="values")
Vector3 Point3
Definition: Point3.h:35
2D Pose
3D Pose
NonlinearFactorGraph exampleGraph()
bool serializeValuesToXMLFile(const Values &values, const std::string &fname, const std::string &name="values")
bool serializeValuesToFile(const Values &values, const std::string &fname)


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:23