testBetweenFactorEM.cpp
Go to the documentation of this file.
1 
7 #include <CppUnitLite/TestHarness.h>
8 
9 
10 #include <gtsam_unstable/slam/BetweenFactorEM.h>
11 #include <gtsam/geometry/Pose2.h>
12 #include <gtsam/nonlinear/Values.h>
13 #include <gtsam/base/numericalDerivative.h>
14 #include <gtsam/base/Vector.h>
15 
16 #include <gtsam/slam/BetweenFactor.h>
17 
18 
19 using namespace std;
20 using namespace gtsam;
21 
22 
23 // Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below
24 // to reenable the test.
25 // #if 0
26 
27 /* ************************************************************************* */
28 Vector predictionError(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactorEM<gtsam::Pose2>& factor){
29  gtsam::Values values;
30  values.insert(key1, p1);
31  values.insert(key2, p2);
32  return factor.whitenedError(values);
33 }
34 
35 /* ************************************************************************* */
36 Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactor<gtsam::Pose2>& factor){
37  gtsam::Values values;
38  values.insert(key1, p1);
39  values.insert(key2, p2);
40  // Vector err = factor.whitenedError(values);
41  // return err;
42  return factor.whitenedError(values);
43 }
44 
45 /* ************************************************************************* */
46 TEST( BetweenFactorEM, ConstructorAndEquals)
47 {
48  gtsam::Key key1(1);
49  gtsam::Key key2(2);
50 
51  gtsam::Pose2 p1(10.0, 15.0, 0.1);
52  gtsam::Pose2 p2(15.0, 15.0, 0.3);
53  gtsam::Pose2 noise(0.5, 0.4, 0.01);
54  gtsam::Pose2 rel_pose_ideal = p1.between(p2);
55  gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
56 
57  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05)));
58  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(5, 5, 1.0)));
59 
60  double prior_outlier = 0.5;
61  double prior_inlier = 0.5;
62 
63  // Constructor
64  BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
65  prior_inlier, prior_outlier);
66  BetweenFactorEM<gtsam::Pose2> g(key1, key2, rel_pose_msr, model_inlier, model_outlier,
67  prior_inlier, prior_outlier);
68 
69  // Equals
70  CHECK(assert_equal(f, g, 1e-5));
71 }
72 
73 /* ************************************************************************* */
74 TEST( BetweenFactorEM, EvaluateError)
75 {
76  gtsam::Key key1(1);
77  gtsam::Key key2(2);
78 
79  // Inlier test
80  gtsam::Pose2 p1(10.0, 15.0, 0.1);
81  gtsam::Pose2 p2(15.0, 15.0, 0.3);
82  gtsam::Pose2 noise(0.5, 0.4, 0.01);
83  gtsam::Pose2 rel_pose_ideal = p1.between(p2);
84  gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
85 
86  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05)));
87  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0)));
88 
89  gtsam::Values values;
90  values.insert(key1, p1);
91  values.insert(key2, p2);
92 
93  double prior_outlier = 0.5;
94  double prior_inlier = 0.5;
95 
96  BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
97  prior_inlier, prior_outlier);
98 
99  Vector actual_err_wh = f.whitenedError(values);
100 
101  Vector3 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
102  Vector3 actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
103 
104  // cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "<<actual_err_wh_inlier.norm()<<","<<actual_err_wh_outlier.norm()<<endl;
105  // cout<<actual_err_wh[0]<<" "<<actual_err_wh[1]<<" "<<actual_err_wh[2]<<actual_err_wh[3]<<" "<<actual_err_wh[4]<<" "<<actual_err_wh[5]<<endl;
106 
107  // in case of inlier, inlier-mode whitented error should be dominant
108  // CHECK(actual_err_wh_inlier.norm() > 1000.0*actual_err_wh_outlier.norm());
109 
110  // Outlier test
111  noise = gtsam::Pose2(10.5, 20.4, 2.01);
112  gtsam::Pose2 rel_pose_msr_test2 = rel_pose_ideal.compose(noise);
113 
114  BetweenFactorEM<gtsam::Pose2> g(key1, key2, rel_pose_msr_test2, model_inlier, model_outlier,
115  prior_inlier, prior_outlier);
116 
117  actual_err_wh = g.whitenedError(values);
118 
119  actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
120  actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
121 
122  // in case of outlier, outlier-mode whitented error should be dominant
123  // CHECK(actual_err_wh_inlier.norm() < 1000.0*actual_err_wh_outlier.norm());
124  //
125  // cout << "Outlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "<<actual_err_wh_inlier.norm()<<","<<actual_err_wh_outlier<<endl;
126  // cout<<actual_err_wh[0]<<" "<<actual_err_wh[1]<<" "<<actual_err_wh[2]<<actual_err_wh[3]<<" "<<actual_err_wh[4]<<" "<<actual_err_wh[5]<<endl;
127 
128  // Compare with standard between factor for the inlier case
129  prior_outlier = 0.0;
130  prior_inlier = 1.0;
131  BetweenFactorEM<gtsam::Pose2> h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier,
132  prior_inlier, prior_outlier);
133  actual_err_wh = h_EM.whitenedError(values);
134  actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
135 
136  BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier );
137  Vector actual_err_wh_stnd = h.whitenedError(values);
138 
139  // cout<<"actual_err_wh: "<<actual_err_wh_inlier[0]<<", "<<actual_err_wh_inlier[1]<<", "<<actual_err_wh_inlier[2]<<endl;
140  // cout<<"actual_err_wh_stnd: "<<actual_err_wh_stnd[0]<<", "<<actual_err_wh_stnd[1]<<", "<<actual_err_wh_stnd[2]<<endl;
141  //
142  // CHECK( assert_equal(actual_err_wh_inlier, actual_err_wh_stnd, 1e-8));
143 }
144 
146 TEST (BetweenFactorEM, jacobian ) {
147 
148  gtsam::Key key1(1);
149  gtsam::Key key2(2);
150 
151  // Inlier test
152  gtsam::Pose2 p1(10.0, 15.0, 0.1);
153  gtsam::Pose2 p2(15.0, 15.0, 0.3);
154  gtsam::Pose2 noise(0.5, 0.4, 0.01);
155  gtsam::Pose2 rel_pose_ideal = p1.between(p2);
156  gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
157 
158  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05)));
159  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0)));
160 
161  gtsam::Values values;
162  values.insert(key1, p1);
163  values.insert(key2, p2);
164 
165  double prior_outlier = 0.0;
166  double prior_inlier = 1.0;
167 
168  BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
169  prior_inlier, prior_outlier);
170 
171  std::vector<gtsam::Matrix> H_actual(2);
172  Vector actual_err_wh = f.whitenedError(values, H_actual);
173 
174  Matrix H1_actual = H_actual[0];
175  Matrix H2_actual = H_actual[1];
176 
177  // compare to standard between factor
178  BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier );
179  Vector actual_err_wh_stnd = h.whitenedError(values);
180  Vector actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
181 // CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
182  std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
183  (void)h.unwhitenedError(values, H_actual_stnd_unwh);
184  Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0];
185  Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1];
186  Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh);
187  Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh);
188 // CHECK( assert_equal(H1_actual_stnd, H1_actual, 1e-8));
189 // CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
190 
191  double stepsize = 1.0e-9;
192  using std::placeholders::_1;
193  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize);
194  Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize);
195 
196 
197  // try to check numerical derivatives of a standard between factor
198  Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize);
199 // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
200 //
201 //
202 // CHECK( assert_equal(H1_expected, H1_actual, 1e-8));
203 // CHECK( assert_equal(H2_expected, H2_actual, 1e-8));
204 
205 }
206 
207 
208 /* ************************************************************************* */
209 TEST( BetweenFactorEM, CaseStudy)
210 {
211 
212  bool debug = false;
213 
214  gtsam::Key key1(1);
215  gtsam::Key key2(2);
216 
217  // Inlier test
218  gtsam::Pose2 p1;
219  gtsam::Pose2 p2(-0.0491752554, -0.289649075, -0.328993962);
220  gtsam::Pose2 rel_pose_msr(0.0316191379, 0.0247539161, 0.004102182);
221 
222  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.4021, 0.286, 0.428)));
223  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(4.9821, 4.614, 1.8387)));
224 
225  gtsam::Values values;
226  values.insert(key1, p1);
227  values.insert(key2, p2);
228 
229  double prior_outlier = 0.5;
230  double prior_inlier = 0.5;
231 
232  BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
233  prior_inlier, prior_outlier);
234 
235  if (debug)
236  cout << "==== inside CaseStudy ===="<<endl;
237 
238  gtsam::Vector p_inlier_outler = f.calcIndicatorProb(values);
239 
240  Vector actual_err_unw = f.unwhitenedError(values);
241  Vector actual_err_wh = f.whitenedError(values);
242 
243  Vector3 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
244  Vector3 actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
245 
246  if (debug){
247  cout << "p_inlier_outler: "<<p_inlier_outler[0]<<", "<<p_inlier_outler[1]<<endl;
248  cout<<"actual_err_unw: "<<actual_err_unw[0]<<", "<<actual_err_unw[1]<<", "<<actual_err_unw[2]<<endl;
249  cout<<"actual_err_wh_inlier: "<<actual_err_wh_inlier[0]<<", "<<actual_err_wh_inlier[1]<<", "<<actual_err_wh_inlier[2]<<endl;
250  cout<<"actual_err_wh_outlier: "<<actual_err_wh_outlier[0]<<", "<<actual_err_wh_outlier[1]<<", "<<actual_err_wh_outlier[2]<<endl;
251  }
252 }
253 
254 
256 TEST (BetweenFactorEM, updateNoiseModel ) {
257  gtsam::Key key1(1);
258  gtsam::Key key2(2);
259 
260  gtsam::Pose2 p1(10.0, 15.0, 0.1);
261  gtsam::Pose2 p2(15.0, 15.0, 0.3);
262  gtsam::Pose2 noise(0.5, 0.4, 0.01);
263  gtsam::Pose2 rel_pose_ideal = p1.between(p2);
264  gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
265 
266  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(1.5, 2.5, 4.05)));
267  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(50.0, 50.0, 10.0)));
268 
269  gtsam::Values values;
270  values.insert(key1, p1);
271  values.insert(key2, p2);
272 
273  double prior_outlier = 0.0;
274  double prior_inlier = 1.0;
275 
276  BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
277  prior_inlier, prior_outlier);
278 
279  SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2));
280 
281  NonlinearFactorGraph graph;
282  graph.addPrior(key1, p1, model);
283  graph.addPrior(key2, p2, model);
284 
285  f.updateNoiseModels(values, graph);
286 
287  SharedGaussian model_inlier_new = f.get_model_inlier();
288  SharedGaussian model_outlier_new = f.get_model_outlier();
289 
290  // model_inlier->print("model_inlier:");
291  // model_outlier->print("model_outlier:");
292  // model_inlier_new->print("model_inlier_new:");
293  // model_outlier_new->print("model_outlier_new:");
294 }
295 
296 
297 // #endif
298 
299 /* ************************************************************************* */
300  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
301 /* ************************************************************************* */
key1
const Symbol key1('v', 1)
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Pose2.h
2D Pose
Vector.h
typedef and functions to augment Eigen's VectorXd
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
TestHarness.h
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
h
const double h
Definition: testSimpleHelicopter.cpp:19
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
BetweenFactorEM.h
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::NoiseModelFactor::whitenedError
Vector whitenedError(const Values &c) const
Definition: NonlinearFactor.cpp:107
key2
const Symbol key2('v', 2)
BetweenFactor.h
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
debug
static constexpr bool debug
Definition: testDiscreteBayesTree.cpp:31
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::SharedGaussian
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:763
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
TestResult
Definition: TestResult.h:26
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam
traits
Definition: SFMdata.h:40
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
gtsam::BetweenFactorEM
Definition: BetweenFactorEM.h:34
gtsam::BetweenFactorEM::whitenedError
Vector whitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const
Definition: BetweenFactorEM.h:147
main
int main()
Definition: testBetweenFactorEM.cpp:300
predictionError_standard
Vector predictionError_standard(const Pose2 &p1, const Pose2 &p2, const gtsam::Key &key1, const gtsam::Key &key2, const BetweenFactor< gtsam::Pose2 > &factor)
Definition: testBetweenFactorEM.cpp:36
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
TEST
TEST(BetweenFactorEM, ConstructorAndEquals)
Definition: testBetweenFactorEM.cpp:46
predictionError
Vector predictionError(const Pose2 &p1, const Pose2 &p2, const gtsam::Key &key1, const gtsam::Key &key2, const BetweenFactorEM< gtsam::Pose2 > &factor)
Definition: testBetweenFactorEM.cpp:28
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::Pose2
Definition: Pose2.h:39
gtsam::BetweenFactor
Definition: BetweenFactor.h:40

gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:17