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 
15 #include <gtsam/slam/BetweenFactor.h>
16 
17 
18 using namespace std;
19 using namespace gtsam;
20 
21 
22 // Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below
23 // to reenable the test.
24 #if 0
25 
26 /* ************************************************************************* */
27 LieVector predictionError(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactorEM<gtsam::Pose2>& factor){
28  gtsam::Values values;
29  values.insert(key1, p1);
30  values.insert(key2, p2);
31  // LieVector err = factor.whitenedError(values);
32  // return err;
33  return LieVector::Expmap(factor.whitenedError(values));
34 }
35 
36 /* ************************************************************************* */
37 LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactor<gtsam::Pose2>& factor){
38  gtsam::Values values;
39  values.insert(key1, p1);
40  values.insert(key2, p2);
41  // LieVector err = factor.whitenedError(values);
42  // return err;
43  return LieVector::Expmap(factor.whitenedError(values));
44 }
45 
46 /* ************************************************************************* */
47 TEST( BetweenFactorEM, ConstructorAndEquals)
48 {
49  gtsam::Key key1(1);
50  gtsam::Key key2(2);
51 
52  gtsam::Pose2 p1(10.0, 15.0, 0.1);
53  gtsam::Pose2 p2(15.0, 15.0, 0.3);
54  gtsam::Pose2 noise(0.5, 0.4, 0.01);
55  gtsam::Pose2 rel_pose_ideal = p1.between(p2);
56  gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
57 
58  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05)));
59  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(5, 5, 1.0)));
60 
61  double prior_outlier = 0.5;
62  double prior_inlier = 0.5;
63 
64  // Constructor
65  BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
66  prior_inlier, prior_outlier);
67  BetweenFactorEM<gtsam::Pose2> g(key1, key2, rel_pose_msr, model_inlier, model_outlier,
68  prior_inlier, prior_outlier);
69 
70  // Equals
71  CHECK(assert_equal(f, g, 1e-5));
72 }
73 
74 /* ************************************************************************* */
75 TEST( BetweenFactorEM, EvaluateError)
76 {
77  gtsam::Key key1(1);
78  gtsam::Key key2(2);
79 
80  // Inlier test
81  gtsam::Pose2 p1(10.0, 15.0, 0.1);
82  gtsam::Pose2 p2(15.0, 15.0, 0.3);
83  gtsam::Pose2 noise(0.5, 0.4, 0.01);
84  gtsam::Pose2 rel_pose_ideal = p1.between(p2);
85  gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
86 
87  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05)));
88  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0)));
89 
90  gtsam::Values values;
91  values.insert(key1, p1);
92  values.insert(key2, p2);
93 
94  double prior_outlier = 0.5;
95  double prior_inlier = 0.5;
96 
97  BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
98  prior_inlier, prior_outlier);
99 
100  Vector actual_err_wh = f.whitenedError(values);
101 
102  Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
103  Vector actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
104 
105  // cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "<<actual_err_wh_inlier.norm()<<","<<actual_err_wh_outlier.norm()<<endl;
106  // 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;
107 
108  // in case of inlier, inlier-mode whitented error should be dominant
109  // CHECK(actual_err_wh_inlier.norm() > 1000.0*actual_err_wh_outlier.norm());
110 
111  // Outlier test
112  noise = gtsam::Pose2(10.5, 20.4, 2.01);
113  gtsam::Pose2 rel_pose_msr_test2 = rel_pose_ideal.compose(noise);
114 
115  BetweenFactorEM<gtsam::Pose2> g(key1, key2, rel_pose_msr_test2, model_inlier, model_outlier,
116  prior_inlier, prior_outlier);
117 
118  actual_err_wh = g.whitenedError(values);
119 
120  actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
121  actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
122 
123  // in case of outlier, outlier-mode whitented error should be dominant
124  // CHECK(actual_err_wh_inlier.norm() < 1000.0*actual_err_wh_outlier.norm());
125  //
126  // cout << "Outlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "<<actual_err_wh_inlier.norm()<<","<<actual_err_wh_outlier<<endl;
127  // 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;
128 
129  // Compare with standard between factor for the inlier case
130  prior_outlier = 0.0;
131  prior_inlier = 1.0;
132  BetweenFactorEM<gtsam::Pose2> h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier,
133  prior_inlier, prior_outlier);
134  actual_err_wh = h_EM.whitenedError(values);
135  actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
136 
137  BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier );
138  Vector actual_err_wh_stnd = h.whitenedError(values);
139 
140  // cout<<"actual_err_wh: "<<actual_err_wh_inlier[0]<<", "<<actual_err_wh_inlier[1]<<", "<<actual_err_wh_inlier[2]<<endl;
141  // cout<<"actual_err_wh_stnd: "<<actual_err_wh_stnd[0]<<", "<<actual_err_wh_stnd[1]<<", "<<actual_err_wh_stnd[2]<<endl;
142  //
143  // CHECK( assert_equal(actual_err_wh_inlier, actual_err_wh_stnd, 1e-8));
144 }
145 
147 TEST (BetweenFactorEM, jacobian ) {
148 
149  gtsam::Key key1(1);
150  gtsam::Key key2(2);
151 
152  // Inlier test
153  gtsam::Pose2 p1(10.0, 15.0, 0.1);
154  gtsam::Pose2 p2(15.0, 15.0, 0.3);
155  gtsam::Pose2 noise(0.5, 0.4, 0.01);
156  gtsam::Pose2 rel_pose_ideal = p1.between(p2);
157  gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
158 
159  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05)));
160  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0)));
161 
162  gtsam::Values values;
163  values.insert(key1, p1);
164  values.insert(key2, p2);
165 
166  double prior_outlier = 0.0;
167  double prior_inlier = 1.0;
168 
169  BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
170  prior_inlier, prior_outlier);
171 
172  std::vector<gtsam::Matrix> H_actual(2);
173  Vector actual_err_wh = f.whitenedError(values, H_actual);
174 
175  Matrix H1_actual = H_actual[0];
176  Matrix H2_actual = H_actual[1];
177 
178  // compare to standard between factor
179  BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier );
180  Vector actual_err_wh_stnd = h.whitenedError(values);
181  Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
182 // CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
183  std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
184  (void)h.unwhitenedError(values, H_actual_stnd_unwh);
185  Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0];
186  Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1];
187  Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh);
188  Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh);
189 // CHECK( assert_equal(H1_actual_stnd, H1_actual, 1e-8));
190 // CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
191 
192  double stepsize = 1.0e-9;
193  Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize);
194  Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::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<LieVector, Pose2>(boost::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  Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
244  Vector actual_err_wh_outlier = (Vector(3) << 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( (gtsam::Vector(3) << 1.5, 2.5, 4.05)));
267  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 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 /* ************************************************************************* */
CHECK
#define CHECK(condition)
Definition: Test.h:109
main
int main()
Definition: testBetweenFactorEM.cpp:300
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
Values.h
A non-templated config holding any types of Manifold-group elements.
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:140
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
leaf::values
leaf::MyValues values
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
std
Definition: Half.h:150
numericalDerivative.h
Some functions to compute numerical derivatives.
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
g
void g(const string &key, int i)
Definition: testBTree.cpp:43
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:229
key1
const Symbol key1('v', 1)
gtsam::BetweenFactorEM::whitenedError
Vector whitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const
Definition: BetweenFactorEM.h:146
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:47
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:211
debug
static bool debug
Definition: testDiscreteBayesTree.cpp:34
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
predictionError
LieVector predictionError(const LieVector &v1, const LieVector &v2, const GaussMarkovFactor factor)
Definition: testGaussMarkov1stOrderFactor.cpp:33
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
h
const double h
Definition: testSimpleHelicopter.cpp:16
key2
const Symbol key2('v', 2)
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:51
gtsam::NoiseModelFactor::whitenedError
Vector whitenedError(const Values &c) const
Definition: NonlinearFactor.cpp:98
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:52
Pose2.h
2D Pose
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
TEST
#define TEST(testGroup, testName)
Definition: Test.h:63
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::SharedGaussian
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:735

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:46:18