types_sba.cpp
Go to the documentation of this file.
00001 // g2o - General Graph Optimization
00002 // Copyright (C) 2011 Kurt Konolige
00003 // 
00004 // g2o is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // g2o is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #include "types_sba.h"
00018 #include <iostream>
00019 
00020 #include "g2o/core/factory.h"
00021 
00022 namespace g2o {
00023 
00024   using namespace std;
00025   void __attribute__ ((constructor)) init_sba_types(void)
00026   {
00027     //cerr << "Calling " << __FILE__ << " " << __PRETTY_FUNCTION__ << endl;
00028     Factory* factory = Factory::instance();
00029 
00030     factory->registerType("VERTEX_CAM", new HyperGraphElementCreator<VertexCam>);
00031     factory->registerType("VERTEX_XYZ", new HyperGraphElementCreator<VertexPointXYZ>);
00032     factory->registerType("VERTEX_INTRINSICS", new HyperGraphElementCreator<VertexIntrinsics>);
00033 
00034     factory->registerType("EDGE_PROJECT_P2MC", new HyperGraphElementCreator<EdgeProjectP2MC>);
00035     factory->registerType("EDGE_PROJECT_P2MC_INTRINSICS", new HyperGraphElementCreator<EdgeProjectP2MC_Intrinsics>);
00036     factory->registerType("EDGE_PROJECT_P2SC", new HyperGraphElementCreator<EdgeProjectP2SC>);
00037     factory->registerType("EDGE_CAM", new HyperGraphElementCreator<EdgeSBACam>);
00038     factory->registerType("EDGE_SCALE", new HyperGraphElementCreator<EdgeSBAScale>);
00039   }
00040 
00041   // constructor
00042   VertexIntrinsics::VertexIntrinsics() 
00043   {
00044     estimate() << 1. , 1. , .5 , .5 , .1;
00045   }
00046 
00047   bool VertexIntrinsics::read(std::istream& is)
00048   {
00049     for (int i=0; i<5; i++)
00050       is >> estimate()[i];
00051     return true;
00052   }
00053 
00054   bool VertexIntrinsics::write(std::ostream& os) const
00055   {
00056     for (int i=0; i<5; i++)
00057       os << estimate()[i] << " ";
00058     return os.good();
00059   }
00060 
00061   // constructor
00062   VertexCam::VertexCam() 
00063   {
00064     _intrinsics = 0;
00065   }
00066 
00067   bool VertexCam::read(std::istream& is)
00068   {
00069     // first the position and orientation (vector3 and quaternion)
00070     Vector3d t;
00071     for (int i=0; i<3; i++){
00072       is >> t[i];
00073     }
00074     Vector4d rc;
00075     for (int i=0; i<4; i++) {
00076       is >> rc[i];
00077     }
00078     Quaterniond r;
00079     r.coeffs() = rc;
00080     r.normalize();
00081 
00082 
00083     // form the camera object
00084     SBACam cam(r,t);
00085 
00086     // now fx, fy, cx, cy, baseline
00087     double fx, fy, cx, cy, tx;
00088 
00089     // try to read one value
00090     is >>  fx;
00091     if (is.good()) {
00092       is >>  fy >> cx >> cy >> tx;
00093       cam.setKcam(fx,fy,cx,cy,tx);
00094     } else{
00095       is.clear();
00096       std::cerr << "cam not defined, using defaults" << std::endl;
00097       cam.setKcam(300,300,320,320,0.1);
00098     }
00099 
00100     // set the object
00101     estimate() = cam;
00102 
00103     //    std::cout << cam << std::endl;
00104 
00105     return true;
00106   }
00107 
00108   bool VertexCam::write(std::ostream& os) const
00109   {
00110     const SBACam &cam = estimate();
00111 
00112     // first the position and orientation (vector3 and quaternion)
00113     for (int i=0; i<3; i++)
00114       os << cam.translation()[i] << " ";
00115     for (int i=0; i<4; i++)
00116       os << cam.rotation().coeffs()[i] << " ";
00117 
00118     // now fx, fy, cx, cy, baseline
00119     os << cam.Kcam(0,0) << " ";
00120     os << cam.Kcam(1,1) << " ";
00121     os << cam.Kcam(0,2) << " ";
00122     os << cam.Kcam(1,2) << " ";
00123     os << cam.baseline << " ";
00124     return os.good();
00125   }
00126 
00127   EdgeSBACam::EdgeSBACam() :
00128     BaseBinaryEdge<6, SE3Quat, VertexCam, VertexCam>()
00129   {
00130   }
00131 
00132   bool EdgeSBACam::read(std::istream& is)
00133   {
00134     for (int i=0; i<7; i++)
00135       is >> measurement()[i];
00136     measurement().rotation().normalize();
00137     inverseMeasurement() = measurement().inverse();
00138     
00139     for (int i=0; i<6; i++)
00140       for (int j=i; j<6; j++) {
00141         is >> information()(i,j);
00142         if (i!=j)
00143           information()(j,i)=information()(i,j);
00144       }
00145     return true;
00146   }
00147 
00148   bool EdgeSBACam::write(std::ostream& os) const
00149   {
00150     for (int i=0; i<7; i++)
00151       os << measurement()[i] << " ";
00152     for (int i=0; i<6; i++)
00153       for (int j=i; j<6; j++){
00154         os << " " <<  information()(i,j);
00155       }
00156     return os.good();
00157   }
00158 
00159   void EdgeSBACam::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/)
00160   {
00161     VertexCam* from = static_cast<VertexCam*>(_vertices[0]);
00162     VertexCam* to = static_cast<VertexCam*>(_vertices[1]);
00163     if (from_.count(from) > 0)
00164       to->estimate() = ((SE3Quat) from->estimate()) * _measurement;
00165     else
00166       from->estimate() = ((SE3Quat) to->estimate() * _inverseMeasurement);
00167   }
00168 
00169 
00170   VertexPointXYZ::VertexPointXYZ() : BaseVertex<3, Vector3d>()
00171   {
00172   }
00173 
00174   bool VertexPointXYZ::read(std::istream& is)
00175   {
00176     Vector3d lv;
00177     for (int i=0; i<3; i++)
00178       is >> lv[i];
00179     estimate() = lv;
00180     return true;
00181   }
00182 
00183   bool VertexPointXYZ::write(std::ostream& os) const
00184   {
00185     Vector3d lv=estimate();
00186     for (int i=0; i<3; i++){
00187       os << lv[i] << " ";
00188     }
00189     return os.good();
00190   }
00191 
00192   // point to camera projection, monocular
00193   EdgeProjectP2MC::EdgeProjectP2MC() :
00194   BaseBinaryEdge<2, Vector2d, VertexPointXYZ, VertexCam>()
00195   {
00196     information().setIdentity();
00197   }
00198 
00199   bool EdgeProjectP2MC::read(std::istream& is)
00200   {
00201     // measured keypoint
00202     for (int i=0; i<2; i++)
00203       is >> measurement()[i];
00204     // don't need this if we don't use it in error calculation (???)
00205     inverseMeasurement() = -measurement();
00206     // information matrix is the identity for features, could be changed to allow arbitrary covariances
00207     information().setIdentity();
00208     return true;
00209   }
00210 
00211   bool EdgeProjectP2MC::write(std::ostream& os) const
00212   {
00213     for (int i=0; i<2; i++)
00214       os  << measurement()[i] << " ";
00215     return os.good();
00216   }
00217 
00218   // point to camera projection, stereo
00219   EdgeProjectP2SC::EdgeProjectP2SC() :
00220     BaseBinaryEdge<3, Vector3d, VertexPointXYZ, VertexCam>()
00221   {
00222   }
00223 
00224   bool EdgeProjectP2SC::read(std::istream& is)
00225   {
00226     // measured keypoint
00227     for (int i=0; i<3; i++)
00228       is >> measurement()[i];
00229     // don't need this if we don't use it in error calculation (???)
00230     inverseMeasurement() = -measurement();
00231     // information matrix is the identity for features, could be changed to allow arbitrary covariances
00232     information().setIdentity();
00233     return true;
00234   }
00235 
00236   bool EdgeProjectP2SC::write(std::ostream& os) const
00237   {
00238     for (int i=0; i<3; i++)
00239       os  << measurement()[i] << " ";
00240     return os.good();
00241   }
00242 
00246   void EdgeProjectP2SC::linearizeOplus()
00247   {
00248     VertexCam *vc = static_cast<VertexCam *>(_vertices[1]);
00249     SBACam &cam = vc->estimate();
00250 
00251     VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[0]);
00252     Vector4d pt, trans;
00253     pt.head<3>() = vp->estimate();
00254     pt(3) = 1.0;
00255     trans.head<3>() = cam.translation();
00256     trans(3) = 1.0;
00257 
00258     // first get the world point in camera coords
00259     Eigen::Matrix<double,3,1> pc = cam.w2n * pt;
00260 
00261     // Jacobians wrt camera parameters
00262     // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00263     double px = pc(0);
00264     double py = pc(1);
00265     double pz = pc(2);
00266     double ipz2 = 1.0/(pz*pz);
00267     if (isnan(ipz2) ) 
00268       { 
00269         std::cout << "[SetJac] infinite jac" << std::endl;
00270         *(int *)0x0 = 0; 
00271       }
00272 
00273     double ipz2fx = ipz2*cam.Kcam(0,0); // Fx
00274     double ipz2fy = ipz2*cam.Kcam(1,1); // Fy
00275     double b      = cam.baseline; // stereo baseline
00276 
00277     Eigen::Matrix<double,3,1> pwt;
00278 
00279     // check for local vars
00280     pwt = (pt-trans).head<3>(); // transform translations, use differential rotation
00281 
00282     // dx
00283     Eigen::Matrix<double,3,1> dp = cam.dRdx * pwt; // dR'/dq * [pw - t]
00284     _jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx;
00285     _jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy;
00286     _jacobianOplusXj(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00287     // dy
00288     dp = cam.dRdy * pwt; // dR'/dq * [pw - t]
00289     _jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx;
00290     _jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy;
00291     _jacobianOplusXj(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00292     // dz
00293     dp = cam.dRdz * pwt; // dR'/dq * [pw - t]
00294     _jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx;
00295     _jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy;
00296     _jacobianOplusXj(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00297 
00298     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00299     dp = -cam.w2n.col(0);        // dpc / dx
00300     _jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00301     _jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00302     _jacobianOplusXj(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00303     dp = -cam.w2n.col(1);        // dpc / dy
00304     _jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00305     _jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00306     _jacobianOplusXj(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00307     dp = -cam.w2n.col(2);        // dpc / dz
00308     _jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00309     _jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00310     _jacobianOplusXj(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00311 
00312     // Jacobians wrt point parameters
00313     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00314     dp = cam.w2n.col(0); // dpc / dx
00315     _jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00316     _jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00317     _jacobianOplusXi(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00318     dp = cam.w2n.col(1); // dpc / dy
00319     _jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00320     _jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00321     _jacobianOplusXi(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00322     dp = cam.w2n.col(2); // dpc / dz
00323     _jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00324     _jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00325     _jacobianOplusXi(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00326   }
00327 
00328 
00332   void EdgeProjectP2MC::linearizeOplus()
00333   {
00334     VertexCam *vc = static_cast<VertexCam *>(_vertices[1]);
00335     SBACam &cam = vc->estimate();
00336 
00337     VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[0]);
00338     Vector4d pt, trans;
00339     pt.head<3>() = vp->estimate();
00340     pt(3) = 1.0;
00341     trans.head<3>() = cam.translation();
00342     trans(3) = 1.0;
00343 
00344     // first get the world point in camera coords
00345     Eigen::Matrix<double,3,1> pc = cam.w2n * pt;
00346 
00347     // Jacobians wrt camera parameters
00348     // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00349     double px = pc(0);
00350     double py = pc(1);
00351     double pz = pc(2);
00352     double ipz2 = 1.0/(pz*pz);
00353     if (isnan(ipz2) ) 
00354       { 
00355         std::cout << "[SetJac] infinite jac" << std::endl;
00356         *(int *)0x0 = 0; 
00357       }
00358 
00359     double ipz2fx = ipz2*cam.Kcam(0,0); // Fx
00360     double ipz2fy = ipz2*cam.Kcam(1,1); // Fy
00361 
00362     Eigen::Matrix<double,3,1> pwt;
00363 
00364     // check for local vars
00365     pwt = (pt-trans).head<3>(); // transform translations, use differential rotation
00366 
00367     // dx
00368     Eigen::Matrix<double,3,1> dp = cam.dRdx * pwt; // dR'/dq * [pw - t]
00369     _jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx;
00370     _jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy;
00371     // dy
00372     dp = cam.dRdy * pwt; // dR'/dq * [pw - t]
00373     _jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx;
00374     _jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy;
00375     // dz
00376     dp = cam.dRdz * pwt; // dR'/dq * [pw - t]
00377     _jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx;
00378     _jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy;
00379 
00380     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00381     dp = -cam.w2n.col(0);        // dpc / dx
00382     _jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00383     _jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00384     dp = -cam.w2n.col(1);        // dpc / dy
00385     _jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00386     _jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00387     dp = -cam.w2n.col(2);        // dpc / dz
00388     _jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00389     _jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00390 
00391     // Jacobians wrt point parameters
00392     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00393     dp = cam.w2n.col(0); // dpc / dx
00394     _jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00395     _jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00396     dp = cam.w2n.col(1); // dpc / dy
00397     _jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00398     _jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00399     dp = cam.w2n.col(2); // dpc / dz
00400     _jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00401     _jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00402   }
00403 
00404 
00405 
00406   // point to camera projection, monocular
00407   EdgeProjectP2MC_Intrinsics::EdgeProjectP2MC_Intrinsics() :
00408     BaseMultiEdge<2, Vector2d>()
00409   {
00410     information().setIdentity();
00411     resize(3);
00412     _jacobianOplus[0].resize(2,3);
00413     _jacobianOplus[1].resize(2,6);
00414     _jacobianOplus[2].resize(2,4);
00415   }
00416 
00420   void EdgeProjectP2MC_Intrinsics::linearizeOplus()
00421   {
00422     VertexCam *vc = static_cast<VertexCam *>(_vertices[1]);
00423     SBACam &cam = vc->estimate();
00424 
00425     VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[0]);
00426 
00427     //VertexIntrinsics *intr = static_cast<VertexIntrinsics *>(_vertices[2]);
00428 
00429     Vector4d pt, trans;
00430     pt.head<3>() = vp->estimate();
00431     pt(3) = 1.0;
00432     trans.head<3>() = cam.translation();
00433     trans(3) = 1.0;
00434 
00435     // first get the world point in camera coords
00436     Eigen::Matrix<double,3,1> pc = cam.w2n * pt;
00437 
00438     // Jacobians wrt camera parameters
00439     // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00440     double px = pc(0);
00441     double py = pc(1);
00442     double pz = pc(2);
00443     double ipz2 = 1.0/(pz*pz);
00444     if (isnan(ipz2) ) 
00445       { 
00446         std::cout << "[SetJac] infinite jac" << std::endl;
00447         *(int *)0x0 = 0; 
00448       }
00449 
00450     double ipz2fx = ipz2*cam.Kcam(0,0); // Fx
00451     double ipz2fy = ipz2*cam.Kcam(1,1); // Fy
00452 
00453     Eigen::Matrix<double,3,1> pwt;
00454 
00455     // check for local vars
00456     pwt = (pt-trans).head<3>(); // transform translations, use differential rotation
00457 
00458     // dx
00459     Eigen::Matrix<double,3,1> dp = cam.dRdx * pwt; // dR'/dq * [pw - t]
00460     _jacobianOplus[1](0,3) = (pz*dp(0) - px*dp(2))*ipz2fx;
00461     _jacobianOplus[1](1,3) = (pz*dp(1) - py*dp(2))*ipz2fy;
00462     // dy
00463     dp = cam.dRdy * pwt; // dR'/dq * [pw - t]
00464     _jacobianOplus[1](0,4) = (pz*dp(0) - px*dp(2))*ipz2fx;
00465     _jacobianOplus[1](1,4) = (pz*dp(1) - py*dp(2))*ipz2fy;
00466     // dz
00467     dp = cam.dRdz * pwt; // dR'/dq * [pw - t]
00468     _jacobianOplus[1](0,5) = (pz*dp(0) - px*dp(2))*ipz2fx;
00469     _jacobianOplus[1](1,5) = (pz*dp(1) - py*dp(2))*ipz2fy;
00470 
00471     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00472     dp = -cam.w2n.col(0);        // dpc / dx
00473     _jacobianOplus[1](0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00474     _jacobianOplus[1](1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00475     dp = -cam.w2n.col(1);        // dpc / dy
00476     _jacobianOplus[1](0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00477     _jacobianOplus[1](1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00478     dp = -cam.w2n.col(2);        // dpc / dz
00479     _jacobianOplus[1](0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00480     _jacobianOplus[1](1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00481 
00482     // Jacobians wrt point parameters
00483     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00484     dp = cam.w2n.col(0); // dpc / dx
00485     _jacobianOplus[0](0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00486     _jacobianOplus[0](1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00487     dp = cam.w2n.col(1); // dpc / dy
00488     _jacobianOplus[0](0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00489     _jacobianOplus[0](1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00490     dp = cam.w2n.col(2); // dpc / dz
00491     _jacobianOplus[0](0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00492     _jacobianOplus[0](1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00493 
00494     // Jacobians w.r.t the intrinsics
00495     _jacobianOplus[2].setZero();
00496     _jacobianOplus[2](0,0) = px/pz; // dx/dfx
00497     _jacobianOplus[2](1,1) = py/pz; // dy/dfy
00498     _jacobianOplus[2](0,2) = 1.;    // dx/dcx
00499     _jacobianOplus[2](1,3) = 1.;    // dy/dcy
00500   }
00501 
00502   bool EdgeProjectP2MC_Intrinsics::read(std::istream& is)
00503   {
00504     // measured keypoint
00505     for (int i=0; i<2; i++)
00506       is >> measurement()[i];
00507     // don't need this if we don't use it in error calculation (???)
00508     inverseMeasurement() = -measurement();
00509     // information matrix is the identity for features, could be changed to allow arbitrary covariances
00510     information().setIdentity();
00511     return true;
00512   }
00513 
00514   bool EdgeProjectP2MC_Intrinsics::write(std::ostream& os) const
00515   {
00516     for (int i=0; i<2; i++)
00517       os  << measurement()[i] << " ";
00518     return os.good();
00519   }
00520 
00521 
00522   // point to camera projection, stereo
00523   EdgeSBAScale::EdgeSBAScale() :
00524     BaseBinaryEdge<1, double, VertexCam, VertexCam>()
00525   {
00526   }
00527   
00528   bool EdgeSBAScale::read(std::istream& is)
00529   {
00530     is >> measurement();
00531     inverseMeasurement() = -measurement();
00532     information().setIdentity();
00533     is >> information()(0,0);
00534     return true;
00535   }
00536 
00537   bool EdgeSBAScale::write(std::ostream& os) const
00538   {
00539     os  << measurement() << " " << information()(0,0);
00540     return os.good();
00541   }
00542 
00543   void EdgeSBAScale::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/)
00544   {
00545     VertexCam* v1 = dynamic_cast<VertexCam*>(_vertices[0]);
00546     VertexCam* v2 = dynamic_cast<VertexCam*>(_vertices[1]);
00547     //compute the translation vector of v1 w.r.t v2
00548     if (from_.count(v1) == 1){
00549       SE3Quat delta = (v1->estimate().inverse()*v2->estimate());
00550       double norm =  delta.translation().norm();
00551       double alpha = _measurement/norm;
00552       delta.translation()*=alpha;
00553       v2->estimate()=v1->estimate()*delta;
00554     } else {
00555       SE3Quat delta = (v2->estimate().inverse()*v1->estimate());
00556       double norm =  delta.translation().norm();
00557       double alpha = _measurement/norm;
00558       delta.translation()*=alpha;
00559       v1->estimate()=v2->estimate()*delta;
00560     }
00561   }
00562 
00563 } // end namespace


re_vision
Author(s): Dorian Galvez-Lopez
autogenerated on Sun Jan 5 2014 11:33:29