edge_se3Switchable.cpp
Go to the documentation of this file.
1 /*
2  * edge_se3Switchable.cpp
3  *
4  * Created on: 17.10.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_se3Switchable.h"
12 #include "vertex_switchLinear.h"
13 #include "g2o/types/slam3d/vertex_se3.h"
14 #include "g2o/types/slam3d/isometry3d_gradients.h"
15 
16 
17 using namespace std;
18 using namespace Eigen;
19 
20 
21 // ================================================
22 EdgeSE3Switchable::EdgeSE3Switchable() : g2o::BaseMultiEdge<6, Eigen::Isometry3d>()
23 {
24  resize(3);
25  _jacobianOplus.clear();
26  _jacobianOplus.push_back(JacobianType(0, 6, 6));
27  _jacobianOplus.push_back(JacobianType(0, 6, 6));
28  _jacobianOplus.push_back(JacobianType(0, 6, 1));
29 
30 }
31 // ================================================
32 bool EdgeSE3Switchable::read(std::istream& is)
33  {
34  /* g2o::Vector7d meas;
35  for (int i=0; i<7; i++)
36  is >> meas[i];
37  // normalize the quaternion to recover numerical precision lost by storing as human readable text
38  Vector4d::MapType(meas.data()+3).normalize();
39  setMeasurement(g2o::internal::fromVectorQT(meas));
40 
41  for (int i=0; i<6; i++)
42  for (int j=i; j<6; j++) {
43  is >> information()(i,j);
44  if (i!=j)
45  information()(j,i) = information()(i,j);
46  }
47  return true;*/
48  return false;
49 
50  }
51 // ================================================
52 bool EdgeSE3Switchable::write(std::ostream& os) const
53 {
54  /* g2o::Vector7d meas = g2o::internal::toVectorQT(measurement());
55  for (int i=0; i<7; i++) os << meas[i] << " ";
56  for (int i = 0; i < 6; ++i)
57  for (int j = i; j < 6; ++j)
58  os << " " << information()(i, j);
59  return os.good();*/
60  return false;
61 }
62 
63 // ================================================
65 {
66 
67  g2o::VertexSE3* from = static_cast<g2o::VertexSE3*>(_vertices[0]);
68  g2o::VertexSE3* to = static_cast<g2o::VertexSE3*>(_vertices[1]);
69  const VertexSwitchLinear* vSwitch = static_cast<const VertexSwitchLinear*>(_vertices[2]);
70 
71  Eigen::Isometry3d E;
72  const Eigen::Isometry3d& Xi=from->estimate();
73  const Eigen::Isometry3d& Xj=to->estimate();
74  const Eigen::Isometry3d& Z=_measurement;
75  g2o::internal::computeEdgeSE3Gradient(E, _jacobianOplus[0], _jacobianOplus[1], Z, Xi, Xj);
76 
77  _jacobianOplus[0]*=vSwitch->estimate();
78  _jacobianOplus[1]*=vSwitch->estimate();
79 
80  // derivative w.r.t switch vertex
81  _jacobianOplus[2].setZero();
82  _jacobianOplus[2] = g2o::internal::toVectorMQT(E) * vSwitch->gradient();
83 }
84 
85 
86 // ================================================
88 {
89  const g2o::VertexSE3* v1 = dynamic_cast<const g2o::VertexSE3*>(_vertices[0]);
90  const g2o::VertexSE3* v2 = dynamic_cast<const g2o::VertexSE3*>(_vertices[1]);
91  const VertexSwitchLinear* v3 = static_cast<const VertexSwitchLinear*>(_vertices[2]);
92 
93  Eigen::Isometry3d delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
94  _error = g2o::internal::toVectorMQT(delta) * v3->estimate();
95 }
96 
97 /*
98 #include <GL/gl.h>
99 #ifdef G2O_HAVE_OPENGL
100  EdgeSE3SwitchableDrawAction::EdgeSE3SwitchableDrawAction(): DrawAction(typeid(EdgeSE3Switchable).name()){}
101 
102  g2o::HyperGraphElementAction* EdgeSE3SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
103  g2o::HyperGraphElementAction::Parameters* ){
104  if (typeid(*element).name()!=_typeName)
105  return 0;
106  EdgeSE3Switchable* e = static_cast<EdgeSE3Switchable*>(element);
107 
108 
109  g2o::VertexSE3* fromEdge = static_cast<g2o::VertexSE3*>(e->vertices()[0]);
110  g2o::VertexSE3* toEdge = static_cast<g2o::VertexSE3*>(e->vertices()[1]);
111  VertexSwitchLinear* s = static_cast<VertexSwitchLinear*>(e->vertices()[2]);
112 
113  glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1);
114  glPushAttrib(GL_ENABLE_BIT);
115  glDisable(GL_LIGHTING);
116  glBegin(GL_LINES);
117  glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),fromEdge->estimate().translation().z());
118  glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),toEdge->estimate().translation().z());
119  glEnd();
120  glPopAttrib();
121  return this;
122  }
123 #endif
124 */
virtual bool write(std::ostream &os) const
double gradient() const
The gradient at the current estimate is always 1;.
virtual bool read(std::istream &is)
Eigen::Isometry3d _inverseMeasurement


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