edge_se2Switchable.cpp
Go to the documentation of this file.
1 /*
2  * edge_se2Switchable.cpp
3  *
4  * Created on: 13.07.2011
5  * Author: niko
6  *
7  * Updated on: 14.01.2013
8  * Author: Christian Kerl <christian.kerl@in.tum.de>
9  */
10 
11 #include "edge_se2Switchable.h"
12 #include "vertex_switchLinear.h"
13 
14 using namespace std;
15 using namespace Eigen;
16 
17 
18 // ================================================
19 EdgeSE2Switchable::EdgeSE2Switchable() : g2o::BaseMultiEdge<3, g2o::SE2>()
20 {
21  resize(3);
22  _jacobianOplus.clear();
23  _jacobianOplus.push_back(JacobianType(0, 3, 3));
24  _jacobianOplus.push_back(JacobianType(0, 3, 3));
25  _jacobianOplus.push_back(JacobianType(0, 3, 1));
26 
27 }
28 // ================================================
29 bool EdgeSE2Switchable::read(std::istream& is)
30  {
31  Vector3d p;
32  is >> p[0] >> p[1] >> p[2];
33  setMeasurement(g2o::SE2(p));
34  _inverseMeasurement = measurement().inverse();
35 
36  for (int i = 0; i < 3; ++i)
37  for (int j = i; j < 3; ++j) {
38  is >> information()(i, j);
39  if (i != j)
40  information()(j, i) = information()(i, j);
41  }
42  return true;
43  }
44 // ================================================
45 bool EdgeSE2Switchable::write(std::ostream& os) const
46 {
47  Vector3d p = measurement().toVector();
48  os << p.x() << " " << p.y() << " " << p.z();
49  for (int i = 0; i < 3; ++i)
50  for (int j = i; j < 3; ++j)
51  os << " " << information()(i, j);
52  return os.good();
53 }
54 
55 // ================================================
57 {
58 
59  const g2o::VertexSE2* vi = static_cast<const g2o::VertexSE2*>(_vertices[0]);
60  const g2o::VertexSE2* vj = static_cast<const g2o::VertexSE2*>(_vertices[1]);
61  const VertexSwitchLinear* vSwitch = static_cast<const VertexSwitchLinear*>(_vertices[2]);
62 
63  double thetai = vi->estimate().rotation().angle();
64 
65  Vector2d dt = vj->estimate().translation() - vi->estimate().translation();
66  double si=sin(thetai), ci=cos(thetai);
67 
68  _jacobianOplus[0](0, 0) = -ci; _jacobianOplus[0](0, 1) = -si; _jacobianOplus[0](0, 2) = -si*dt.x()+ci*dt.y();
69  _jacobianOplus[0](1, 0) = si; _jacobianOplus[0](1, 1) = -ci; _jacobianOplus[0](1, 2) = -ci*dt.x()-si*dt.y();
70  _jacobianOplus[0](2, 0) = 0; _jacobianOplus[0](2, 1) = 0; _jacobianOplus[0](2, 2) = -1;
71 
72  _jacobianOplus[1](0, 0) = ci; _jacobianOplus[1](0, 1)= si; _jacobianOplus[1](0, 2)= 0;
73  _jacobianOplus[1](1, 0) =-si; _jacobianOplus[1](1, 1)= ci; _jacobianOplus[1](1, 2)= 0;
74  _jacobianOplus[1](2, 0) = 0; _jacobianOplus[1](2, 1)= 0; _jacobianOplus[1](2, 2)= 1;
75 
76  const g2o::SE2& rmean = _inverseMeasurement;
77  Matrix3d z = Matrix3d::Zero();
78  z.block<2, 2>(0, 0) = rmean.rotation().toRotationMatrix();
79  z(2, 2) = 1.;
80  _jacobianOplus[0] = z * _jacobianOplus[0];
81  _jacobianOplus[1] = z * _jacobianOplus[1];
82 
83 
84  _jacobianOplus[0]*=vSwitch->estimate();
85  _jacobianOplus[1]*=vSwitch->estimate();
86 
87 
88  // derivative w.r.t switch vertex
89  _jacobianOplus[2].setZero();
90  g2o::SE2 delta = _inverseMeasurement * (vi->estimate().inverse()*vj->estimate());
91  _jacobianOplus[2] = delta.toVector() * vSwitch->gradient();
92 }
93 
94 // ================================================
96 {
97  const g2o::VertexSE2* v1 = static_cast<const g2o::VertexSE2*>(_vertices[0]);
98  const g2o::VertexSE2* v2 = static_cast<const g2o::VertexSE2*>(_vertices[1]);
99  const VertexSwitchLinear* v3 = static_cast<const VertexSwitchLinear*>(_vertices[2]);
100 
101  g2o::SE2 delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
102  _error = delta.toVector() * v3->estimate();
103 }
104 
105 /*
106 #include <GL/gl.h>
107 #ifdef G2O_HAVE_OPENGL
108  EdgeSE2SwitchableDrawAction::EdgeSE2SwitchableDrawAction(): DrawAction(typeid(EdgeSE2Switchable).name()){}
109 
110  g2o::HyperGraphElementAction* EdgeSE2SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
111  g2o::HyperGraphElementAction::Parameters* ){
112  if (typeid(*element).name()!=_typeName)
113  return 0;
114  EdgeSE2Switchable* e = static_cast<EdgeSE2Switchable*>(element);
115 
116 
117  g2o::VertexSE2* fromEdge = static_cast<g2o::VertexSE2*>(e->vertices()[0]);
118  g2o::VertexSE2* toEdge = static_cast<g2o::VertexSE2*>(e->vertices()[1]);
119  VertexSwitchLinear* s = static_cast<VertexSwitchLinear*>(e->vertices()[2]);
120 
121  glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1);
122  glPushAttrib(GL_ENABLE_BIT);
123  glDisable(GL_LIGHTING);
124  glBegin(GL_LINES);
125  glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
126  glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
127  glEnd();
128  glPopAttrib();
129  return this;
130  }
131 #endif
132 */
virtual bool write(std::ostream &os) const
double gradient() const
The gradient at the current estimate is always 1;.
GLM_FUNC_DECL genType cos(genType const &angle)
GLM_FUNC_DECL genType sin(genType const &angle)
virtual bool read(std::istream &is)
virtual void setMeasurement(const g2o::SE2 &m)


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