g2o
edge_se3_pointxyz_depth.cpp
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
28 
29 namespace g2o {
30 namespace deprecated {
31 
32  using namespace std;
33 
34 
35  // point to camera projection, monocular
39  information().setIdentity();
40  information()(2,2)=100;
41  J.fill(0);
42  J.block<3,3>(0,0) = -Eigen::Matrix3d::Identity();
43  }
44 
46  ParameterVector pv(1);
47  pv[0]=params;
48  resolveCache(cache, (OptimizableGraph::Vertex*)_vertices[0],"CACHE_CAMERA",pv);
49  return cache != 0;
50  }
51 
52  bool EdgeSE3PointXYZDepth::read(std::istream& is) {
53  int pid;
54  is >> pid;
55  setParameterId(0,pid);
56 
57  // measured keypoint
58  Eigen::Vector3d meas;
59  for (int i=0; i<3; i++) is >> meas[i];
60  setMeasurement(meas);
61  // don't need this if we don't use it in error calculation (???)
62  // information matrix is the identity for features, could be changed to allow arbitrary covariances
63  if (is.bad()) {
64  return false;
65  }
66  for ( int i=0; i<information().rows() && is.good(); i++)
67  for (int j=i; j<information().cols() && is.good(); j++){
68  is >> information()(i,j);
69  if (i!=j)
70  information()(j,i)=information()(i,j);
71  }
72  if (is.bad()) {
73  // we overwrite the information matrix
74  information().setIdentity();
75  information()(2,2)=10/_measurement(2); // scale the info by the inverse of the measured depth
76  }
77  return true;
78  }
79 
80  bool EdgeSE3PointXYZDepth::write(std::ostream& os) const {
81  os << params->id() << " ";
82  for (int i=0; i<3; i++) os << measurement()[i] << " ";
83  for (int i=0; i<information().rows(); i++)
84  for (int j=i; j<information().cols(); j++) {
85  os << information()(i,j) << " ";
86  }
87  return os.good();
88  }
89 
90 
92  // from cam to point (track)
93  //VertexSE3 *cam = static_cast<VertexSE3*>(_vertices[0]);
94  VertexPointXYZ *point = static_cast<VertexPointXYZ*>(_vertices[1]);
95 
96  Eigen::Vector3d p = cache->w2i() * point->estimate();
97  Eigen::Vector3d perr;
98  perr.head<2>() = p.head<2>()/p(2);
99  perr(2) = p(2);
100 
101  // error, which is backwards from the normal observed - calculated
102  // _measurement is the measured projection
103  _error = perr - _measurement;
104  // std::cout << _error << std::endl << std::endl;
105  }
106 
108  //VertexSE3 *cam = static_cast<VertexSE3 *>(_vertices[0]);
109  VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[1]);
110 
111  const Eigen::Vector3d& pt = vp->estimate();
112 
113  Eigen::Vector3d Zcam = cache->w2lMatrix() * pt;
114 
115  // J(0,3) = -0.0;
116  J(0,4) = -2*Zcam(2);
117  J(0,5) = 2*Zcam(1);
118 
119  J(1,3) = 2*Zcam(2);
120  // J(1,4) = -0.0;
121  J(1,5) = -2*Zcam(0);
122 
123  J(2,3) = -2*Zcam(1);
124  J(2,4) = 2*Zcam(0);
125  // J(2,5) = -0.0;
126 
127  J.block<3,3>(0,6) = cache->w2lMatrix().rotation();
128 
129  Eigen::Matrix<double,3,9> Jprime = params->Kcam_inverseOffsetR() * J;
130  Eigen::Vector3d Zprime = cache->w2i() * pt;
131 
132  Eigen::Matrix<double, 3, 9> Jhom;
133  Jhom.block<2,9>(0,0) = 1/(Zprime(2)*Zprime(2)) * (Jprime.block<2,9>(0,0)*Zprime(2) - Zprime.head<2>() * Jprime.block<1,9>(2,0));
134  Jhom.block<1,9>(2,0) = Jprime.block<1,9>(2,0);
135 
136  _jacobianOplusXi = Jhom.block<3,6>(0,0);
137  _jacobianOplusXj = Jhom.block<3,3>(0,6);
138  }
139 
140 
142  //VertexSE3 *cam = static_cast<VertexSE3*>(_vertices[0]);
143  VertexPointXYZ *point = static_cast<VertexPointXYZ*>(_vertices[1]);
144 
145  // calculate the projection
146  const Eigen::Vector3d& pt = point->estimate();
147 
148  Eigen::Vector3d p = cache->w2i() * pt;
149  Eigen::Vector3d perr;
150  perr.head<2>() = p.head<2>()/p(2);
151  perr(2) = p(2);
152  _measurement = perr;
153  return true;
154  }
155 
156 
158  {
159  (void) from;
160  assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");
161 
162  VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
163  VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
164  const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam();
165  Eigen::Vector3d p;
166  p(2) = _measurement(2);
167  p.head<2>() = _measurement.head<2>()*p(2);
168  p=invKcam*p;
169  point->setEstimate(cam->estimate() * (params->offsetMatrix() * p));
170  }
171 
172 }
173 }
bool installParameter(ParameterType *&p, size_t argNo, int paramId=-1)
const Eigen::Matrix3d & invKcam() const
virtual void initialEstimate(const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to)
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:136
const Eigen::Affine3d & w2i() const
return the world to image transform
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3PointXYZDepth()
bool setParameterId(int argNum, int paramId)
const Eigen::Isometry3d & offsetMatrix() const
rotation of the offset as 3x3 rotation matrix
void resolveCache(CacheType *&cache, OptimizableGraph::Vertex *, const std::string &_type, const ParameterVector &parameters)
Definition: cache.h:122
std::vector< Parameter * > ParameterVector
Definition: parameter.h:52
int id() const
Definition: parameter.h:45
const Eigen::Matrix3d & Kcam_inverseOffsetR() const
3D pose Vertex, (x,y,z,qw,qx,qy,qz) the parameterization for the increments constructed is a 6d vecto...
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.
void resizeParameters(size_t newSize)
const Eigen::Isometry3d & w2lMatrix() const
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
virtual bool read(std::istream &is)
read the vertex from a stream, i.e., the internal state of the vertex
virtual bool write(std::ostream &os) const
write the vertex to a stream
virtual void setMeasurement(const Eigen::Vector3d &m)
EIGEN_STRONG_INLINE const Measurement & measurement() const
accessor functions for the measurement represented by the edge
Definition: base_edge.h:75
VertexContainer _vertices
Definition: hyper_graph.h:202