10 #include <gtest/gtest.h> 12 #include <Eigen/Dense> 18 #if EIGEN_VERSION_AT_LEAST(3, 1, 0) // Requires Eigen>=3.1 23 vector<double>& parent,
size_t nodeid,
double observation,
26 m_observation(observation),
27 m_information(information)
29 this->node_id = nodeid;
32 double evaluateResidual()
const override 34 return m_parent[node_id] - m_observation;
36 double getInformation()
const override {
return m_information; }
37 void evalJacobian(
double& dr_dx)
const override { dr_dx = 1.0; }
40 vector<double>& m_parent;
41 double m_observation, m_information;
47 vector<double>& parent,
size_t nodeid_i,
size_t nodeid_j,
49 : m_parent(parent), m_information(information)
51 this->node_id_i = nodeid_i;
52 this->node_id_j = nodeid_j;
55 double evaluateResidual()
const override 57 return m_parent[node_id_i] - m_parent[node_id_j];
59 double getInformation()
const override {
return m_information; }
60 void evalJacobian(
double& dr_dx_i,
double& dr_dx_j)
const override 67 vector<double>& m_parent;
74 vector<double> my_map(N, .0);
81 std::deque<MySimpleUnaryEdge> edges1;
83 edges1.emplace_back(my_map, 0, 1.0, 4.0);
84 edges1.emplace_back(my_map, 1, 5.0, 4.0);
85 edges1.emplace_back(my_map, 2, 3.0, 4.0);
86 edges1.emplace_back(my_map, 3, 2.0, 16.0);
112 my_map.assign(N, .0);
115 edges1.emplace_back(my_map, 0, 4.0, 2.0);
136 vector<double> my_map(N, .0);
144 MySimpleUnaryEdge edge_val0(my_map, 0, 1.0 , 1.0 );
147 MySimpleBinaryEdge edge_01(my_map, 0, 1, .1);
149 MySimpleBinaryEdge edge_12(my_map, 1, 2, .1);
151 MySimpleBinaryEdge edge_23(my_map, 2, 3, .1);
153 MySimpleBinaryEdge edge_30(my_map, 3, 0, .1);
159 my_map.assign(N, .0);
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.
EXPECT_GT(out.final_iters, 10UL)
void updateEstimation(mrpt::math::CVectorDouble &solved_x_inc, mrpt::math::CVectorDouble *solved_variances=nullptr)
Abstract graph and tree data structures, plus generic graph algorithms.
void enableProfiler(bool enable=true)
TEST(ICP_SLAM_App, MapFromRawlog_PointMap)
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.
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
Sparse solver for GMRF (Gaussian Markov Random Fields) graphical models.