edge_se2MaxMixture.cpp
Go to the documentation of this file.
1 /*
2  * edge_se2MaxMixture.cpp
3  *
4  * Created on: 12.06.2012
5  * Author: niko
6  */
7 
8 
9 #include "edge_se2MaxMixture.h"
10 
11 using namespace std;
12 using namespace Eigen;
13 
14 // ================================================
16 {
17  nullHypothesisMoreLikely = false;
18 }
19 
20 // ================================================
21 bool EdgeSE2MaxMixture::read(std::istream& is)
22  {
23  Vector3d p;
24  is >> weight >> p[0] >> p[1] >> p[2];
25  setMeasurement(g2o::SE2(p));
26  _inverseMeasurement = measurement().inverse();
27  //measurement().fromVector(p);
28  //inverseMeasurement() = measurement().inverse();
29  for (int i = 0; i < 3; ++i)
30  for (int j = i; j < 3; ++j) {
31  is >> information()(i, j);
32  if (i != j)
33  information()(j, i) = information()(i, j);
34  }
35 
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());
40 
41  return true;
42  }
43 // ================================================
44 bool EdgeSE2MaxMixture::write(std::ostream& os) const
45 {
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);
51  return os.good();
52 }
53 
54 // ================================================
56 {
57  g2o::EdgeSE2::linearizeOplus();
58  if (nullHypothesisMoreLikely) {
59  _jacobianOplusXi *= weight;
60  _jacobianOplusXj *= weight;
61  }
62 }
63 
64 // ================================================
66 {
67  // calculate the error for this constraint
68  g2o::EdgeSE2::computeError();
69 
70  // determine the likelihood for constraint and null hypothesis
71  double mahal_constraint = _error.transpose() * information_constraint * _error;
72  double likelihood_constraint = nu_constraint * exp(-mahal_constraint);
73 
74  double mahal_nullHypothesis = _error.transpose() * (information_nullHypothesis) * _error;
75  double likelihood_nullHypothesis = nu_nullHypothesis * exp(-mahal_nullHypothesis);
76 
77  // if the nullHypothesis is more likely ...
78  if (likelihood_nullHypothesis > likelihood_constraint) {
79  _information = information_nullHypothesis;
80  nullHypothesisMoreLikely = true;
81  }
82  else {
83  _information = information_constraint;
84  nullHypothesisMoreLikely = false;
85  }
86 
87 
88 }
89 
90 /*
91 #include <GL/gl.h>
92 // ================================================
93 #ifdef G2O_HAVE_OPENGL
94  EdgeSE2MaxMixtureDrawAction::EdgeSE2MaxMixtureDrawAction(): DrawAction(typeid(EdgeSE2MaxMixture).name()){}
95 
96  g2o::HyperGraphElementAction* EdgeSE2MaxMixtureDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
97  g2o::HyperGraphElementAction::Parameters* ){
98  if (typeid(*element).name()!=_typeName)
99  return 0;
100  EdgeSE2MaxMixture* e = static_cast<EdgeSE2MaxMixture*>(element);
101 
102 
103  g2o::VertexSE2* fromEdge = static_cast<g2o::VertexSE2*>(e->vertices()[0]);
104  g2o::VertexSE2* toEdge = static_cast<g2o::VertexSE2*>(e->vertices()[1]);
105 
106 
107  if (e->nullHypothesisMoreLikely) glColor3f(0.0,0.0,0.0);
108  else glColor3f(1.0,0.5,0.2);
109 
110  glPushAttrib(GL_ENABLE_BIT);
111  glDisable(GL_LIGHTING);
112  glBegin(GL_LINES);
113  glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
114  glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
115  glEnd();
116  glPopAttrib();
117  return this;
118  }
119 #endif
120 */
121 
122 
virtual bool write(std::ostream &os) const
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
GLM_FUNC_DECL genType exp(genType const &x)
virtual bool read(std::istream &is)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58