g2o
edge_se3_lotsofxyz.cpp
Go to the documentation of this file.
1 #include "edge_se3_lotsofxyz.h"
2 
3 namespace g2o{
4 
6  resize(0);
7  }
8 
10  VertexSE3 * pose = static_cast<VertexSE3 *> (_vertices[0]);
11 
12  Eigen::Transform<double, 3, 1> poseinv = pose->estimate().inverse();
13 
14  for(unsigned int i=0; i<_observedPoints; i++){
15  VertexPointXYZ * xyz = static_cast<VertexPointXYZ *> (_vertices[1+i]);
16  // const Vector3D &pt = xyz->estimate();
17  Vector3D m = poseinv * xyz->estimate();
18 
19  unsigned int index = 3*i;
20  _measurement[index] = m[0];
21  _measurement[index+1] = m[1];
22  _measurement[index+2] = m[2];
23  }
24  return true;
25  }
26 
28  VertexSE3 * pose = static_cast<VertexSE3 *> (_vertices[0]);
29 
30  for(unsigned int i=0; i<_observedPoints; i++){
31  VertexPointXYZ * xyz = static_cast<VertexPointXYZ *> (_vertices[1+i]);
32  Vector3D m = pose->estimate().inverse() * xyz->estimate();
33 
34  unsigned int index = 3*i;
35  _error[index] = m[0] - _measurement[index];
36  _error[index+1] = m[1] - _measurement[index+1];
37  _error[index+2] = m[2] - _measurement[index+2];
38  }
39  }
40 
42  g2o::VertexSE3 * pose = (g2o::VertexSE3 *) (_vertices[0]);
43 
44  // initialize Ji matrix
45  MatrixXD Ji;
46  unsigned int rows = 3*(_vertices.size()-1);
47  Ji.resize(rows, 6);
48  Ji.fill(0);
49 
50  Matrix3D poseRot = pose->estimate().inverse().rotation();
51 
52  for(unsigned int i=1; i<_vertices.size(); i++){
54  Vector3D Zcam = pose->estimate().inverse() * point->estimate();
55 
56  unsigned int index=3*(i-1);
57 
58  // Ji.block<3,3>(index,0) = -poseRot;
59  Ji.block<3,3>(index,0) = -Matrix3D::Identity();
60 
61  Ji(index, 3) = -0.0;
62  Ji(index, 4) = -2*Zcam(2);
63  Ji(index, 5) = 2*Zcam(1);
64 
65  Ji(index+1, 3) = 2*Zcam(2);
66  Ji(index+1, 4) = -0.0;
67  Ji(index+1, 5) = -2*Zcam(0);
68 
69  Ji(index+2, 3) = -2*Zcam(1);
70  Ji(index+2, 4) = 2*Zcam(0);
71  Ji(index+2, 5) = -0.0;
72 
73  MatrixXD Jj;
74  Jj.resize(rows, 3);
75  Jj.fill(0);
76  Jj.block<3,3>(index,0) = poseRot;
77 
78  _jacobianOplus[i] = Jj;
79  }
80 
81  _jacobianOplus[0] = Ji;
82 
83  }
84 
85  bool EdgeSE3LotsOfXYZ::read(std::istream& is){
86  is >> _observedPoints;
87 
88  setSize(_observedPoints + 1);
89 
90  // read the measurements
91  for(unsigned int i=0; i<_observedPoints; i++){
92  unsigned int index = 3*i;
93  is >> _measurement[index] >> _measurement[index+1] >> _measurement[index+2];
94  }
95 
96  // read the information matrix
97  for(unsigned int i=0; i<_observedPoints*3; i++){
98  // fill the "upper triangle" part of the matrix
99  for(unsigned int j=i; j<_observedPoints*3; j++){
100  is >> information()(i,j);
101  }
102 
103  // fill the lower triangle part
104  for(unsigned int j=0; j<i; j++){
105  information()(i,j) = information()(j,i);
106  }
107 
108  }
109  return true;
110  }
111 
112 
113 
114  bool EdgeSE3LotsOfXYZ::write(std::ostream& os) const{
115  // write number of observed points
116  os << "|| " << _observedPoints;
117 
118  // write measurements
119  for(unsigned int i=0; i<_observedPoints; i++){
120  unsigned int index = 3*i;
121  os << " " << _measurement[index] << " " << _measurement[index+1] << " " << _measurement[index+2];
122  }
123 
124  // write information matrix
125  for(unsigned int i=0; i<_observedPoints*3; i++){
126  for(unsigned int j=i; j<_observedPoints*3; j++){
127  os << " " << information()(i,j);
128  }
129  }
130  return os.good();
131  }
132 
133 
134 
136  (void) toEstimate;
137 
138  assert(initialEstimatePossible(fixed, toEstimate) && "Bad vertices specified");
139 
140  VertexSE3 * pose = static_cast<VertexSE3 *>(_vertices[0]);
141 
142 #ifdef _MSC_VER
143  std::vector<bool> estimate_this(_observedPoints, true);
144 #else
145  bool estimate_this[_observedPoints];
146  for(unsigned int i=0; i<_observedPoints; i++){
147  estimate_this[i] = true;
148  }
149 #endif
150 
151  for(std::set<HyperGraph::Vertex*>::iterator it=fixed.begin(); it!=fixed.end(); it++){
152  for(unsigned int i=1; i<_vertices.size(); i++){
153  VertexPointXYZ * vert = static_cast<VertexPointXYZ *>(_vertices[i]);
154  if(vert->id() == (*it)->id()) estimate_this[i-1] = false;
155  }
156  }
157 
158  for(unsigned int i=1; i<_vertices.size(); i++){
159  if(estimate_this[i-1]){
160  unsigned int index = 3*(i-1);
161  Vector3D submeas(_measurement[index], _measurement[index+1], _measurement[index+2]);
162  VertexPointXYZ * vert = static_cast<VertexPointXYZ *>(_vertices[i]);
163  vert->setEstimate(pose->estimate() * submeas);
164  }
165  }
166  }
167 
168 
169 
171  (void) toEstimate;
172 
173  for(std::set<HyperGraph::Vertex *>::iterator it=fixed.begin(); it!=fixed.end(); it++){
174  if(_vertices[0]->id() == (*it)->id()){
175  return 1.0;
176  }
177  }
178 
179  return -1.0;
180  }
181 }
int id() const
returns the id
Definition: hyper_graph.h:148
Eigen::Matrix< double, 3, 1, Eigen::ColMajor > Vector3D
Definition: eigen_types.h:46
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:136
virtual void initialEstimate(const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
virtual bool setMeasurementFromState()
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
virtual bool write(std::ostream &os) const
write the vertex to a stream
void setSize(int vertices)
Eigen::Matrix< double, 3, 3, Eigen::ColMajor > Matrix3D
Definition: eigen_types.h:61
Vertex for a tracked point in space.
virtual bool read(std::istream &is)
read the vertex from a stream, i.e., the internal state of the vertex
void setEstimate(const EstimateType &et)
set the estimate for the vertex also calls updateCache()
Definition: base_vertex.h:101
A general case Vertex for optimization.
virtual void resize(size_t size)
EIGEN_STRONG_INLINE const InformationType & information() const
information matrix of the constraint
Definition: base_edge.h:67
const EstimateType & estimate() const
return the current estimate of the vertex
Definition: base_vertex.h:99
3D pose Vertex, represented as an Isometry3D
Definition: vertex_se3.h:50
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor > MatrixXD
Definition: eigen_types.h:63
std::vector< JacobianType, Eigen::aligned_allocator< JacobianType > > _jacobianOplus
jacobians of the edge (w.r.t. oplus)
VertexContainer _vertices
Definition: hyper_graph.h:202