Main MRPT website > C++ reference for MRPT 1.5.8
ScalarFactorGraph_unittest.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 
12 #include <gtest/gtest.h>
13 
14 using namespace mrpt;
15 using namespace mrpt::graphs;
16 using namespace std;
17 
18 #if EIGEN_VERSION_AT_LEAST(3,1,0) // Requires Eigen>=3.1
19 
20 struct MySimpleUnaryEdge : public ScalarFactorGraph::UnaryFactorVirtualBase
21 {
22  MySimpleUnaryEdge(vector<double> &parent, size_t nodeid, double observation, double information) :
23  m_parent(parent),
24  m_observation(observation),
25  m_information(information)
26  {
27  this->node_id = nodeid;
28  }
29 
30  double evaluateResidual() const MRPT_OVERRIDE
31  {
32  return m_parent[node_id] - m_observation;
33  }
34  double getInformation() const MRPT_OVERRIDE
35  {
36  return m_information;
37  }
38  void evalJacobian(double &dr_dx) const MRPT_OVERRIDE
39  {
40  dr_dx = 1.0;
41  }
42 
43 protected:
44  vector<double> &m_parent;
45  double m_observation, m_information;
46 };
47 
48 struct MySimpleBinaryEdge : public ScalarFactorGraph::BinaryFactorVirtualBase
49 {
50  MySimpleBinaryEdge(vector<double> &parent, size_t nodeid_i, size_t nodeid_j, double information) :
51  m_parent(parent),
52  m_information(information)
53  {
54  this->node_id_i = nodeid_i;
55  this->node_id_j = nodeid_j;
56  }
57 
58  double evaluateResidual() const MRPT_OVERRIDE
59  {
60  return m_parent[node_id_i] - m_parent[node_id_j];
61  }
62  double getInformation() const MRPT_OVERRIDE
63  {
64  return m_information;
65  }
66  void evalJacobian(double &dr_dx_i, double &dr_dx_j ) const MRPT_OVERRIDE
67  {
68  dr_dx_i = +1.0;
69  dr_dx_j = -1.0;
70  }
71 
72 protected:
73  vector<double> &m_parent;
74  double m_information;
75 };
76 
77 TEST(ScalarFactorGraph, MiniMRF_UnaryEdges)
78 {
79  const size_t N = 4;
80  vector<double> my_map(N, .0);
81 
82  ScalarFactorGraph gmrf;
83  gmrf.enableProfiler(false);
84 
85  gmrf.initialize(N);
86 
87  std::deque<MySimpleUnaryEdge> edges1;
88 
89  edges1.push_back(MySimpleUnaryEdge(my_map, 0, 1.0, 4.0));
90  edges1.push_back(MySimpleUnaryEdge(my_map, 1, 5.0, 4.0));
91  edges1.push_back(MySimpleUnaryEdge(my_map, 2, 3.0, 4.0));
92  edges1.push_back(MySimpleUnaryEdge(my_map, 3, 2.0, 16.0));
93 
94  for (const auto &e:edges1)
95  gmrf.addConstraint(e);
96 
97  // Test 1:
98  // --------------
99  {
100  my_map.assign(N, .0);
101 
102  Eigen::VectorXd x_incr, x_var;
103  gmrf.updateEstimation(x_incr, &x_var);
104 
105  EXPECT_NEAR(x_incr[0], 1.0, 1e-9);
106  EXPECT_NEAR(x_incr[1], 5.0, 1e-9);
107  EXPECT_NEAR(x_incr[2], 3.0, 1e-9);
108  EXPECT_NEAR(x_incr[3], 2.0, 1e-9);
109 
110  EXPECT_NEAR(x_var[0], 1.0 / 4.0, 1e-9);
111  EXPECT_NEAR(x_var[1], 1.0 / 4.0, 1e-9);
112  EXPECT_NEAR(x_var[2], 1.0 / 4.0, 1e-9);
113  EXPECT_NEAR(x_var[3], 1.0 / 16.0, 1e-9);
114  }
115 
116  // Test 2:
117  // --------------
118  {
119  my_map.assign(N, .0);
120 
121  // Add new edge:
122  edges1.push_back(MySimpleUnaryEdge(my_map, 0, 4.0, 2.0));
123  gmrf.addConstraint(*edges1.rbegin());
124 
125  Eigen::VectorXd x_incr, x_var;
126  gmrf.updateEstimation(x_incr, &x_var);
127 
128  EXPECT_NEAR(x_incr[0], 2.0, 1e-9);
129  EXPECT_NEAR(x_incr[1], 5.0, 1e-9);
130  EXPECT_NEAR(x_incr[2], 3.0, 1e-9);
131  EXPECT_NEAR(x_incr[3], 2.0, 1e-9);
132 
133  EXPECT_NEAR(x_var[0], 1.0/(4.0 + 2.0), 1e-9);
134  EXPECT_NEAR(x_var[1], 1.0 / 4.0, 1e-9);
135  EXPECT_NEAR(x_var[2], 1.0 / 4.0, 1e-9);
136  EXPECT_NEAR(x_var[3], 1.0 / 16.0, 1e-9);
137  }
138 }
139 
140 TEST(ScalarFactorGraph, MiniMRF_BinaryEdges)
141 {
142  const size_t N = 4;
143  vector<double> my_map(N, .0);
144 
145  ScalarFactorGraph gmrf;
146  gmrf.enableProfiler(false);
147 
148  gmrf.initialize(N);
149 
150  // Edge to assign a value to node 0:
151  MySimpleUnaryEdge edge_val0(my_map, 0, 1.0 /*value*/, 1.0 /*information*/);
152  gmrf.addConstraint(edge_val0);
153 
154  MySimpleBinaryEdge edge_01(my_map, 0, 1, .1);
155  gmrf.addConstraint(edge_01);
156  MySimpleBinaryEdge edge_12(my_map, 1, 2, .1);
157  gmrf.addConstraint(edge_12);
158  MySimpleBinaryEdge edge_23(my_map, 2, 3, .1);
159  gmrf.addConstraint(edge_23);
160  MySimpleBinaryEdge edge_30(my_map, 3, 0, .1);
161  gmrf.addConstraint(edge_30);
162 
163  // Test 1:
164  // --------------
165  {
166  my_map.assign(N, .0);
167 
168  Eigen::VectorXd x_incr, x_var;
169  gmrf.updateEstimation(x_incr, &x_var);
170 
171  EXPECT_NEAR(x_incr[0], 1.0, 1e-6);
172  EXPECT_NEAR(x_incr[1], 1.0, 1e-6);
173  EXPECT_NEAR(x_incr[2], 1.0, 1e-6);
174  EXPECT_NEAR(x_incr[3], 1.0, 1e-6);
175 
176  EXPECT_NEAR(x_var[0], 1.0 , 1e-6);
177 
178  EXPECT_GT(x_var[1], x_var[0]);
179  EXPECT_GT(x_var[3], x_var[0]);
180 
181  EXPECT_GT(x_var[2], x_var[1]);
182  EXPECT_GT(x_var[2], x_var[3]);
183  }
184 }
185 
186 #endif // Eigen>=3.1
void initialize(const size_t nodeCount)
Initialize the GMRF internal state and copy the prior factors.
Simple, scalar (1-dim) constraint (edge) for a GMRF.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Abstract graph and tree data structures, plus generic graph algorithms.
STL namespace.
void enableProfiler(bool enable=true)
TEST(Compress, DataBlockGZ)
void addConstraint(const UnaryFactorVirtualBase &listOfConstraints)
Insert constraints into the GMRF problem.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void updateEstimation(Eigen::VectorXd &solved_x_inc, Eigen::VectorXd *solved_variances=NULL)
Simple, scalar (1-dim) constraint (edge) for a GMRF.
Sparse solver for GMRF (Gaussian Markov Random Fields) graphical models.



Page generated by Doxygen 1.8.14 for MRPT 1.5.8 Git: f67d0f871 Wed Sep 25 18:32:17 2019 +0200 at lun oct 28 01:58:29 CET 2019