1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 Kurt Konolige
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 
27 #include "types_sba.h"
28 #include <iostream>
29 
30 #include "g2o/core/factory.h"
31 #include "g2o/stuff/macros.h"
32 
33 namespace g2o {
34 
35   using namespace std;
36 
37   G2O_REGISTER_TYPE_GROUP(sba);
38 
39   G2O_REGISTER_TYPE(VERTEX_CAM, VertexCam);
40   G2O_REGISTER_TYPE(VERTEX_XYZ, VertexSBAPointXYZ);
41   G2O_REGISTER_TYPE(VERTEX_INTRINSICS, VertexIntrinsics);
42 
43   G2O_REGISTER_TYPE(EDGE_PROJECT_P2MC, EdgeProjectP2MC);
44   G2O_REGISTER_TYPE(EDGE_PROJECT_P2SC, EdgeProjectP2SC);
45   G2O_REGISTER_TYPE(EDGE_CAM, EdgeSBACam);
46   G2O_REGISTER_TYPE(EDGE_SCALE, EdgeSBAScale);
47 
48   // constructor
VertexIntrinsics()49   VertexIntrinsics::VertexIntrinsics()
50   {
51     _estimate << cst(1.), cst(1.), cst(.5), cst(.5), cst(.1);
52   }
53 
read(std::istream & is)54   bool VertexIntrinsics::read(std::istream& is) { return internal::readVector(is, _estimate); }
55 
write(std::ostream & os) const56   bool VertexIntrinsics::write(std::ostream& os) const { return internal::writeVector(os, estimate()); }
57 
58   // constructor
VertexCam()59   VertexCam::VertexCam()
60   {
61   }
62 
read(std::istream & is)63   bool VertexCam::read(std::istream& is)
64   {
65     // first the position and orientation (vector3 and quaternion)
66     Vector3 t;
67     internal::readVector(is, t);
68     Quaternion r;
69     internal::readVector(is, r.coeffs());
70     r.normalize(); // recover nummeric precision
71 
72     // form the camera object
73     SBACam cam(r,t);
74 
75     // now fx, fy, cx, cy, baseline
76     number_t fx, fy, cx, cy, tx;
77 
78     // try to read one value
79     is >>  fx;
80     if (is.good()) {
81       is >>  fy >> cx >> cy >> tx;
82       cam.setKcam(fx,fy,cx,cy,tx);
83     } else{
84       is.clear();
85       std::cerr << "cam not defined, using defaults" << std::endl;
86       cam.setKcam(300,300,320,320,cst(0.1));
87     }
88 
89     setEstimate(cam);
90     return true;
91   }
92 
write(std::ostream & os) const93   bool VertexCam::write(std::ostream& os) const
94   {
95     const SBACam &cam = estimate();
96 
97     // first the position and orientation (vector3 and quaternion)
98     internal::writeVector(os, cam.translation());
99     internal::writeVector(os, cam.rotation().coeffs());
100 
101     // now fx, fy, cx, cy, baseline
102     os << cam.Kcam(0,0) << " ";
103     os << cam.Kcam(1,1) << " ";
104     os << cam.Kcam(0,2) << " ";
105     os << cam.Kcam(1,2) << " ";
106     os << cam.baseline << " ";
107     return os.good();
108   }
109 
EdgeSBACam()110   EdgeSBACam::EdgeSBACam() :
111     BaseBinaryEdge<6, SE3Quat, VertexCam, VertexCam>()
112   {
113   }
114 
read(std::istream & is)115   bool EdgeSBACam::read(std::istream& is)
116   {
117     Vector7 meas;
118     internal::readVector(is, meas);
119     setMeasurement(SE3Quat(meas));
120     return readInformationMatrix(is);
121   }
122 
write(std::ostream & os) const123   bool EdgeSBACam::write(std::ostream& os) const
124   {
125     internal::writeVector(os, measurement().toVector());
126     return writeInformationMatrix(os);
127   }
128 
initialEstimate(const OptimizableGraph::VertexSet & from_,OptimizableGraph::Vertex *)129   void EdgeSBACam::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/)
130   {
131     VertexCam* from = static_cast<VertexCam*>(_vertices[0]);
132     VertexCam* to = static_cast<VertexCam*>(_vertices[1]);
133     if (from_.count(from) > 0)
134       to->setEstimate((SE3Quat) from->estimate() * _measurement);
135     else
136       from->setEstimate((SE3Quat) to->estimate() * _inverseMeasurement);
137   }
138 
139 
VertexSBAPointXYZ()140   VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3>()
141   {
142   }
143 
read(std::istream & is)144   bool VertexSBAPointXYZ::read(std::istream& is)
145   {
146     return internal::readVector(is, _estimate);
147   }
148 
write(std::ostream & os) const149   bool VertexSBAPointXYZ::write(std::ostream& os) const
150   {
151     return internal::writeVector(os, estimate());
152   }
153 
154   // point to camera projection, monocular
EdgeProjectP2MC()155   EdgeProjectP2MC::EdgeProjectP2MC() :
156   BaseBinaryEdge<2, Vector2, VertexSBAPointXYZ, VertexCam>()
157   {
158     information().setIdentity();
159   }
160 
read(std::istream & is)161   bool EdgeProjectP2MC::read(std::istream& is)
162   {
163     // measured keypoint
164     internal::readVector(is, _measurement);
165     return readInformationMatrix(is);
166   }
167 
write(std::ostream & os) const168   bool EdgeProjectP2MC::write(std::ostream& os) const
169   {
170     internal::writeVector(os, measurement());
171     writeInformationMatrix(os);
172     return os.good();
173   }
174 
175   // point to camera projection, stereo
EdgeProjectP2SC()176   EdgeProjectP2SC::EdgeProjectP2SC() :
177     BaseBinaryEdge<3, Vector3, VertexSBAPointXYZ, VertexCam>()
178   {
179   }
180 
read(std::istream & is)181   bool EdgeProjectP2SC::read(std::istream& is)
182   {
183     internal::readVector(is, _measurement);
184     return readInformationMatrix(is);
185   }
186 
write(std::ostream & os) const187   bool EdgeProjectP2SC::write(std::ostream& os) const
188   {
189     internal::writeVector(os, measurement());
190     writeInformationMatrix(os);
191     return os.good();
192   }
193 
194 /**
195  * \brief Jacobian for stereo projection
196  */
linearizeOplus()197   void EdgeProjectP2SC::linearizeOplus()
198   {
199     VertexCam *vc = static_cast<VertexCam *>(_vertices[1]);
200     const SBACam &cam = vc->estimate();
201 
202     VertexSBAPointXYZ *vp = static_cast<VertexSBAPointXYZ *>(_vertices[0]);
203     Vector4 pt, trans;
204     pt.head<3>() = vp->estimate();
205     pt(3) = 1.0;
206     trans.head<3>() = cam.translation();
207     trans(3) = 1.0;
208 
209     // first get the world point in camera coords
210     Vector3 pc = cam.w2n * pt;
211 
212     // Jacobians wrt camera parameters
213     // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
214     number_t px = pc(0);
215     number_t py = pc(1);
216     number_t pz = pc(2);
217     number_t ipz2 = 1.0/(pz*pz);
218     if (g2o_isnan(ipz2) ) {
219       std::cout << "[SetJac] infinite jac" << std::endl;
220       abort();
221     }
222 
223     number_t ipz2fx = ipz2*cam.Kcam(0,0); // Fx
224     number_t ipz2fy = ipz2*cam.Kcam(1,1); // Fy
225     number_t b      = cam.baseline; // stereo baseline
226 
227     // check for local vars
228     Vector3 pwt = (pt-trans).head<3>(); // transform translations, use differential rotation
229 
230     // dx
231     Vector3 dp = cam.dRdx * pwt; // dR'/dq * [pw - t]
232     _jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx;
233     _jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy;
234     _jacobianOplusXj(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
235     // dy
236     dp = cam.dRdy * pwt; // dR'/dq * [pw - t]
237     _jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx;
238     _jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy;
239     _jacobianOplusXj(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
240     // dz
241     dp = cam.dRdz * pwt; // dR'/dq * [pw - t]
242     _jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx;
243     _jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy;
244     _jacobianOplusXj(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
245 
246     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
247     dp = -cam.w2n.col(0);        // dpc / dx
248     _jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
249     _jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
250     _jacobianOplusXj(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
251     dp = -cam.w2n.col(1);        // dpc / dy
252     _jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
253     _jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
254     _jacobianOplusXj(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
255     dp = -cam.w2n.col(2);        // dpc / dz
256     _jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
257     _jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
258     _jacobianOplusXj(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
259 
260     // Jacobians wrt point parameters
261     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
262     dp = cam.w2n.col(0); // dpc / dx
263     _jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
264     _jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
265     _jacobianOplusXi(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
266     dp = cam.w2n.col(1); // dpc / dy
267     _jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
268     _jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
269     _jacobianOplusXi(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
270     dp = cam.w2n.col(2); // dpc / dz
271     _jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
272     _jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
273     _jacobianOplusXi(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
274   }
275 
276 
277 /**
278  * \brief Jacobian for monocular projection
279  */
linearizeOplus()280   void EdgeProjectP2MC::linearizeOplus()
281   {
282     VertexCam *vc = static_cast<VertexCam *>(_vertices[1]);
283     const SBACam &cam = vc->estimate();
284 
285     VertexSBAPointXYZ *vp = static_cast<VertexSBAPointXYZ *>(_vertices[0]);
286     Vector4 pt, trans;
287     pt.head<3>() = vp->estimate();
288     pt(3) = 1.0;
289     trans.head<3>() = cam.translation();
290     trans(3) = 1.0;
291 
292     // first get the world point in camera coords
293     Vector3 pc = cam.w2n * pt;
294 
295     // Jacobians wrt camera parameters
296     // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
297     number_t px = pc(0);
298     number_t py = pc(1);
299     number_t pz = pc(2);
300     number_t ipz2 = 1.0/(pz*pz);
301     if (g2o_isnan(ipz2) ) {
302       std::cout << "[SetJac] infinite jac" << std::endl;
303       abort();
304     }
305 
306     number_t ipz2fx = ipz2*cam.Kcam(0,0); // Fx
307     number_t ipz2fy = ipz2*cam.Kcam(1,1); // Fy
308 
309     // check for local vars
310     Vector3 pwt = (pt-trans).head<3>(); // transform translations, use differential rotation
311 
312     // dx
313     Vector3 dp = cam.dRdx * pwt; // dR'/dq * [pw - t]
314     _jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx;
315     _jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy;
316     // dy
317     dp = cam.dRdy * pwt; // dR'/dq * [pw - t]
318     _jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx;
319     _jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy;
320     // dz
321     dp = cam.dRdz * pwt; // dR'/dq * [pw - t]
322     _jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx;
323     _jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy;
324 
325     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
326     dp = -cam.w2n.col(0);        // dpc / dx
327     _jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
328     _jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
329     dp = -cam.w2n.col(1);        // dpc / dy
330     _jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
331     _jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
332     dp = -cam.w2n.col(2);        // dpc / dz
333     _jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
334     _jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
335 
336     // Jacobians wrt point parameters
337     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
338     dp = cam.w2n.col(0); // dpc / dx
339     _jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
340     _jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
341     dp = cam.w2n.col(1); // dpc / dy
342     _jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
343     _jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
344     dp = cam.w2n.col(2); // dpc / dz
345     _jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
346     _jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
347   }
348 
349   // point to camera projection, stereo
EdgeSBAScale()350   EdgeSBAScale::EdgeSBAScale() :
351     BaseBinaryEdge<1, number_t, VertexCam, VertexCam>()
352   {
353   }
354 
read(std::istream & is)355   bool EdgeSBAScale::read(std::istream& is)
356   {
357     number_t meas;
358     is >> meas;
359     setMeasurement(meas);
360     information().setIdentity();
361     is >> information()(0,0);
362     return true;
363   }
364 
write(std::ostream & os) const365   bool EdgeSBAScale::write(std::ostream& os) const
366   {
367     os  << measurement() << " " << information()(0,0);
368     return os.good();
369   }
370 
initialEstimate(const OptimizableGraph::VertexSet & from_,OptimizableGraph::Vertex *)371   void EdgeSBAScale::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/)
372   {
373     VertexCam* v1 = dynamic_cast<VertexCam*>(_vertices[0]);
374     VertexCam* v2 = dynamic_cast<VertexCam*>(_vertices[1]);
375     //compute the translation vector of v1 w.r.t v2
376     if (from_.count(v1) == 1){
377       SE3Quat delta = (v1->estimate().inverse()*v2->estimate());
378       number_t norm =  delta.translation().norm();
379       number_t alpha = _measurement/norm;
380       delta.setTranslation(delta.translation()*alpha);
381       v2->setEstimate(v1->estimate()*delta);
382     } else {
383       SE3Quat delta = (v2->estimate().inverse()*v1->estimate());
384       number_t norm =  delta.translation().norm();
385       number_t alpha = _measurement/norm;
386       delta.setTranslation(delta.translation()*alpha);
387       v1->setEstimate(v2->estimate()*delta);
388     }
389   }
390 
setMeasurementFromState()391   bool EdgeSBACam::setMeasurementFromState()
392   {
393     const VertexCam* v1 = dynamic_cast<const VertexCam*>(_vertices[0]);
394     const VertexCam* v2 = dynamic_cast<const VertexCam*>(_vertices[1]);
395     _measurement = (v1->estimate().inverse()*v2->estimate());
396     _inverseMeasurement = _measurement.inverse();
397     return true;
398   }
399 
400 } // end namespace
401