12 #include <gtest/gtest.h>
15 #include <test_mrpt_common.h>
28 {{
"graphslam_SE2_in.graph",
"graphslam_SE2_out_good.graph"},
29 {
"graphslam_SE2_in2.graph",
"graphslam_SE2_out_good2.graph"},
30 {
"graphslam_SE2_in3.graph",
"graphslam_SE2_out_good3.graph"}}},
32 {{
"graphslam_SE2_in.graph",
"graphslam_SE2_out_good.graph"},
33 {
"graphslam_SE2pdf_in.graph",
"graphslam_SE2pdf_out_good.graph"}}},
35 {{
"graphslam_SE3_in_torus3D-first100.graph",
36 "graphslam_SE3_out_good_torus3D-first100.graph"},
37 {
"graphslam_SE2_in2.graph",
"graphslam_SE2_out_good2.graph"}}}};
39 template <
class my_graph_t>
41 public ::testing::Test
52 const my_graph_t graph_initial = graph;
59 params[
"max_iterations"] = 100;
64 graph, levmarq_info,
nullptr,
params);
66 const double err_init = graph_initial.chi2();
67 const double err_end = graph.chi2();
68 std::cout <<
"err_init: " << err_init << std::endl;
69 std::cout <<
"err_end: " << err_end << std::endl;
81 const my_graph_t& g1,
const my_graph_t& g2,
82 const double eps_node_pos = 1e-3,
const double eps_edges = 1e-3)
84 EXPECT_EQ(g1.edges.size(), g2.edges.size());
85 EXPECT_EQ(g1.nodes.size(), g2.nodes.size());
88 if (g1.edges.size() != g2.edges.size() ||
89 g1.nodes.size() != g2.nodes.size())
94 typename my_graph_t::const_iterator it1, it2;
95 for (it1 = g1.edges.begin(), it2 = g2.edges.begin();
96 it1 != g1.edges.end(); ++it1, ++it2)
101 (it1->second.getPoseMean().asVectorVal() -
102 it2->second.getPoseMean().asVectorVal())
110 auto itn1 = g1.nodes.cbegin(), itn2 = g2.nodes.cbegin();
111 for (; itn1 != g1.nodes.cend(); ++itn1, ++itn2)
116 (itn1->second.asVectorVal() - itn2->second.asVectorVal())
119 <<
"Poses of keyframe #" << itn1->first
120 <<
" do not match:" << std::endl
121 <<
"- Expected: " << itn2->second << std::endl
122 <<
"- Got : " << itn1->second << std::endl;
132 std::stringstream ss;
133 graph.writeAsText(ss);
135 my_graph_t read_graph;
137 read_graph.readAsText(ss);
139 compare_two_graphs(graph, read_graph);
151 my_graph_t read_graph;
155 compare_two_graphs(graph, read_graph);
164 const string prefix = UNITTEST_BASEDIR + string(
"/tests/");
165 for (
const auto& tst : files_it->second)
167 std::cout <<
"Testing graph type `" << type <<
"`, in_file=`"
168 << std::get<0>(tst) <<
"`" << std::endl;
170 const string in_f = prefix + std::get<0>(tst);
172 const string good_f = prefix + std::get<1>(tst);
175 my_graph_t graph, graph_good;
176 graph.loadFromTextFile(in_f);
177 graph_good.loadFromTextFile(good_f);
178 ASSERT_(graph.nodeCount() > 1);
179 ASSERT_EQ(graph.nodeCount(), graph_good.nodeCount());
180 ASSERT_EQ(graph.edgeCount(), graph_good.edgeCount());
183 const my_graph_t graph_initial = graph;
185 params[
"max_iterations"] = 100;
190 graph, levmarq_info,
nullptr,
params);
193 const double err_init = graph_initial.chi2();
194 const double err_end = graph.chi2();
195 const double err_good = graph_good.chi2();
196 std::cout <<
"err_init: " << err_init << std::endl;
197 std::cout <<
"err_end: " << err_end << std::endl;
198 std::cout <<
"err_good: " << err_good << std::endl;
206 compare_two_graphs(graph, graph_good);
216 #define GRAPHS_TESTS(_TYPE) \
217 TEST_F(_TYPE, OptimizeSampleRingPath) \
219 for (int seed = 1; seed <= 3; seed++) \
221 getRandomGenerator().randomize(seed); \
222 test_ring_path(#_TYPE); \
225 TEST_F(_TYPE, BinarySerialization) \
227 getRandomGenerator().randomize(123); \
228 test_graph_bin_serialization(); \
230 TEST_F(_TYPE, WriteReadTextFile) \
232 getRandomGenerator().randomize(123); \
233 test_graph_text_serialization(); \
235 TEST_F(_TYPE, OptimizeCompareKnownSolution) \
237 test_optimize_compare_known_solution(#_TYPE); \