12 using namespace Eigen;
17 nullHypothesisMoreLikely =
false;
24 is >> weight >>
p[0] >>
p[1] >>
p[2];
25 setMeasurement(g2o::SE2(
p));
26 _inverseMeasurement = measurement().inverse();
29 for (
int i = 0;
i < 3; ++
i)
30 for (
int j =
i;
j < 3; ++
j) {
31 is >> information()(
i,
j);
33 information()(
j,
i) = information()(
i,
j);
36 information_constraint = _information;
37 nu_constraint = 1.0/
sqrt(information_constraint.inverse().determinant());
38 information_nullHypothesis = information_constraint*weight;
39 nu_nullHypothesis = 1.0/
sqrt(information_nullHypothesis.inverse().determinant());
46 Vector3d
p = measurement().toVector();
47 os <<
p.x() <<
" " <<
p.y() <<
" " <<
p.z();
48 for (
int i = 0;
i < 3; ++
i)
49 for (
int j =
i;
j < 3; ++
j)
50 os <<
" " << information()(
i,
j);
57 g2o::EdgeSE2::linearizeOplus();
58 if (nullHypothesisMoreLikely) {
59 _jacobianOplusXi *= weight;
60 _jacobianOplusXj *= weight;
68 g2o::EdgeSE2::computeError();
71 double mahal_constraint = _error.transpose() * information_constraint * _error;
72 double likelihood_constraint = nu_constraint *
exp(-mahal_constraint);
74 double mahal_nullHypothesis = _error.transpose() * (information_nullHypothesis) * _error;
75 double likelihood_nullHypothesis = nu_nullHypothesis *
exp(-mahal_nullHypothesis);
78 if (likelihood_nullHypothesis > likelihood_constraint) {
79 _information = information_nullHypothesis;
80 nullHypothesisMoreLikely =
true;
83 _information = information_constraint;
84 nullHypothesisMoreLikely =
false;