11 #include <gtest/gtest.h> 17 #if EIGEN_VERSION_AT_LEAST(3, 1, 0) // Requires Eigen>=3.1 22 vector<double>& parent,
size_t nodeid,
double observation,
25 m_observation(observation),
26 m_information(information)
28 this->node_id = nodeid;
31 double evaluateResidual()
const override 33 return m_parent[node_id] - m_observation;
35 double getInformation()
const override {
return m_information; }
36 void evalJacobian(
double& dr_dx)
const override { dr_dx = 1.0; }
38 vector<double>& m_parent;
39 double m_observation, m_information;
45 vector<double>& parent,
size_t nodeid_i,
size_t nodeid_j,
47 : m_parent(parent), m_information(information)
49 this->node_id_i = nodeid_i;
50 this->node_id_j = nodeid_j;
53 double evaluateResidual()
const override 55 return m_parent[node_id_i] - m_parent[node_id_j];
57 double getInformation()
const override {
return m_information; }
58 void evalJacobian(
double& dr_dx_i,
double& dr_dx_j)
const override 65 vector<double>& m_parent;
72 vector<double> my_map(N, .0);
79 std::deque<MySimpleUnaryEdge> edges1;
81 edges1.push_back(MySimpleUnaryEdge(my_map, 0, 1.0, 4.0));
82 edges1.push_back(MySimpleUnaryEdge(my_map, 1, 5.0, 4.0));
83 edges1.push_back(MySimpleUnaryEdge(my_map, 2, 3.0, 4.0));
84 edges1.push_back(MySimpleUnaryEdge(my_map, 3, 2.0, 16.0));
93 Eigen::VectorXd x_incr, x_var;
96 EXPECT_NEAR(x_incr[0], 1.0, 1e-9);
97 EXPECT_NEAR(x_incr[1], 5.0, 1e-9);
98 EXPECT_NEAR(x_incr[2], 3.0, 1e-9);
99 EXPECT_NEAR(x_incr[3], 2.0, 1e-9);
101 EXPECT_NEAR(x_var[0], 1.0 / 4.0, 1e-9);
102 EXPECT_NEAR(x_var[1], 1.0 / 4.0, 1e-9);
103 EXPECT_NEAR(x_var[2], 1.0 / 4.0, 1e-9);
104 EXPECT_NEAR(x_var[3], 1.0 / 16.0, 1e-9);
110 my_map.assign(N, .0);
113 edges1.push_back(MySimpleUnaryEdge(my_map, 0, 4.0, 2.0));
116 Eigen::VectorXd x_incr, x_var;
119 EXPECT_NEAR(x_incr[0], 2.0, 1e-9);
120 EXPECT_NEAR(x_incr[1], 5.0, 1e-9);
121 EXPECT_NEAR(x_incr[2], 3.0, 1e-9);
122 EXPECT_NEAR(x_incr[3], 2.0, 1e-9);
124 EXPECT_NEAR(x_var[0], 1.0 / (4.0 + 2.0), 1e-9);
125 EXPECT_NEAR(x_var[1], 1.0 / 4.0, 1e-9);
126 EXPECT_NEAR(x_var[2], 1.0 / 4.0, 1e-9);
127 EXPECT_NEAR(x_var[3], 1.0 / 16.0, 1e-9);
134 vector<double> my_map(N, .0);
142 MySimpleUnaryEdge edge_val0(my_map, 0, 1.0 , 1.0 );
145 MySimpleBinaryEdge edge_01(my_map, 0, 1, .1);
147 MySimpleBinaryEdge edge_12(my_map, 1, 2, .1);
149 MySimpleBinaryEdge edge_23(my_map, 2, 3, .1);
151 MySimpleBinaryEdge edge_30(my_map, 3, 0, .1);
157 my_map.assign(N, .0);
159 Eigen::VectorXd x_incr, x_var;
162 EXPECT_NEAR(x_incr[0], 1.0, 1e-6);
163 EXPECT_NEAR(x_incr[1], 1.0, 1e-6);
164 EXPECT_NEAR(x_incr[2], 1.0, 1e-6);
165 EXPECT_NEAR(x_incr[3], 1.0, 1e-6);
167 EXPECT_NEAR(x_var[0], 1.0, 1e-6);
169 EXPECT_GT(x_var[1], x_var[0]);
170 EXPECT_GT(x_var[3], x_var[0]);
172 EXPECT_GT(x_var[2], x_var[1]);
173 EXPECT_GT(x_var[2], x_var[3]);
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.
Abstract graph and tree data structures, plus generic graph algorithms.
TEST(NodeletsTests, pub_sub_multithread_test)
void enableProfiler(bool enable=true)
void addConstraint(const UnaryFactorVirtualBase &listOfConstraints)
Insert constraints into the GMRF problem.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Simple, scalar (1-dim) constraint (edge) for a GMRF.
void updateEstimation(Eigen::VectorXd &solved_x_inc, Eigen::VectorXd *solved_variances=nullptr)
Sparse solver for GMRF (Gaussian Markov Random Fields) graphical models.