types_icp.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_icp.h"
00018 #include "g2o/core/factory.h"
00019 
00020 #include <iostream>
00021 
00022 namespace g2o {
00023 
00024   using namespace std;
00025   using namespace Eigen;
00026   typedef  Matrix<double, 6, 1> Vector6d;
00027 
00028   Matrix3d Edge_V_V_GICP::dRidx; // differential quat matrices
00029   Matrix3d Edge_V_V_GICP::dRidy; // differential quat matrices
00030   Matrix3d Edge_V_V_GICP::dRidz; // differential quat matrices
00031   Matrix3d VertexSCam::dRidx; // differential quat matrices
00032   Matrix3d VertexSCam::dRidy; // differential quat matrices
00033   Matrix3d VertexSCam::dRidz; // differential quat matrices
00034   Matrix3d VertexSCam::Kcam;
00035   double VertexSCam::baseline;
00036 
00037   // global initialization
00038   void __attribute__ ((constructor)) init_icp_types(void)
00039   {
00040     //cerr << "Calling " << __FILE__ << " " << __PRETTY_FUNCTION__ << endl;
00041     Factory* factory = Factory::instance();
00042     factory->registerType("EDGE_V_V_GICP", new HyperGraphElementCreator<Edge_V_V_GICP>);
00043 
00044     Edge_V_V_GICP::dRidx << 0.0,0.0,0.0,
00045       0.0,0.0,2.0,
00046       0.0,-2.0,0.0;
00047     Edge_V_V_GICP::dRidy  << 0.0,0.0,-2.0,
00048       0.0,0.0,0.0,
00049       2.0,0.0,0.0;
00050     Edge_V_V_GICP::dRidz  << 0.0,2.0,0.0,
00051       -2.0,0.0,0.0,
00052       0.0,0.0,0.0;
00053 
00054     VertexSCam::dRidx << 0.0,0.0,0.0,
00055       0.0,0.0,2.0,
00056       0.0,-2.0,0.0;
00057     VertexSCam::dRidy  << 0.0,0.0,-2.0,
00058       0.0,0.0,0.0,
00059       2.0,0.0,0.0;
00060     VertexSCam::dRidz  << 0.0,2.0,0.0,
00061       -2.0,0.0,0.0,
00062       0.0,0.0,0.0;
00063 
00064   }
00065 
00066   // Copy constructor
00067   Edge_V_V_GICP::Edge_V_V_GICP(const Edge_V_V_GICP* e)
00068     : BaseBinaryEdge<3, EdgeGICP, VertexSE3, VertexSE3>()
00069   {
00070     vertices()[0] = e->vertices()[0];
00071     vertices()[1] = e->vertices()[1];
00072 
00073     measurement().pos0 = e->measurement().pos0;
00074     measurement().pos1 = e->measurement().pos1;
00075     measurement().normal0 = e->measurement().normal0;
00076     measurement().normal1 = e->measurement().normal1;
00077     measurement().R0 = e->measurement().R0;
00078     measurement().R1 = e->measurement().R1;
00079 
00080     pl_pl = e->pl_pl;
00081     cov0 = e->cov0;
00082     cov1 = e->cov1;
00083 
00084     _robustKernel = e->_robustKernel;
00085     _huberWidth = e->_huberWidth;
00086   }
00087 
00088   //
00089   // Rigid 3D constraint between poses, given fixed point offsets
00090   //
00091 
00092   // input two matched points between the frames
00093   // first point belongs to the first frame, position and normal
00094   // second point belongs to the second frame, position and normal
00095   //
00096   // the measurement variable has type EdgeGICP (see types_icp.h)
00097 
00098   bool Edge_V_V_GICP::read(std::istream& is)
00099   {
00100     // measured point and normal
00101     for (int i=0; i<3; i++)
00102       is >> measurement().pos0[i];
00103     for (int i=0; i<3; i++)
00104       is >> measurement().normal0[i];
00105 
00106     // measured point and normal
00107     for (int i=0; i<3; i++)
00108       is >> measurement().pos1[i];
00109     for (int i=0; i<3; i++)
00110       is >> measurement().normal1[i];
00111 
00112     // don't need this if we don't use it in error calculation (???)
00113     //    inverseMeasurement() = -measurement();
00114 
00115     measurement().makeRot0();  // set up rotation matrices
00116 
00117     // GICP info matrices
00118 
00119     // point-plane only
00120     Matrix3d prec;
00121     double v = .01;
00122     prec << v, 0, 0,
00123             0, v, 0,
00124             0, 0, 1;
00125     Matrix3d &R = measurement().R0; // plane of the point in vp0
00126     information() = R.transpose()*prec*R;
00127 
00128     //    information().setIdentity();
00129 
00130     //    setRobustKernel(true);
00131     setHuberWidth(0.01);      // units? m?
00132 
00133     return true;
00134   }
00135 
00136 
00137   // Jacobian
00138   // [ -R0'*R1 | R0 * dRdx/ddx * 0p1 ]
00139   // [  R0'*R1 | R0 * dR'dx/ddx * 0p1 ]
00140 
00141 #ifdef GICP_ANALYTIC_JACOBIANS
00142 
00143   // jacobian defined as:
00144   //    f(T0,T1) =  dR0.inv() * T0.inv() * (T1 * dR1 * p1 + dt1) - dt0
00145   //    df/dx0 = [-I, d[dR0.inv()]/dq0 * T01 * p1]
00146   //    df/dx1 = [R0, T01 * d[dR1]/dq1 * p1]
00147   void Edge_V_V_GICP::linearizeOplus()
00148   {
00149     VertexSE3* vp0 = static_cast<VertexSE3*>(_vertices[0]);
00150     VertexSE3* vp1 = static_cast<VertexSE3*>(_vertices[1]);
00151 
00152     Matrix3d R0T = vp0->estimate().rotation().toRotationMatrix().transpose();
00153     Vector3d p1 = measurement().pos1;
00154 
00155     // this could be more efficient
00156     if (!vp0->fixed())
00157       {
00158         SE3Quat T01 = vp0->estimate().inverse() *  vp1->estimate();
00159         Vector3d p1t = T01.map(p1);
00160         _jacobianOplusXi.block<3,3>(0,0) = -Matrix3d::Identity();
00161         _jacobianOplusXi.block<3,1>(0,3) = dRidx*p1t;
00162         _jacobianOplusXi.block<3,1>(0,4) = dRidy*p1t;
00163         _jacobianOplusXi.block<3,1>(0,5) = dRidz*p1t;
00164       }
00165 
00166     if (!vp1->fixed())
00167       {
00168         Matrix3d R1 = vp1->estimate().rotation().toRotationMatrix();
00169         R0T = R0T*R1;
00170         _jacobianOplusXj.block<3,3>(0,0) = R0T;
00171         _jacobianOplusXj.block<3,1>(0,3) = R0T*dRidx.transpose()*p1;
00172         _jacobianOplusXj.block<3,1>(0,4) = R0T*dRidy.transpose()*p1;
00173         _jacobianOplusXj.block<3,1>(0,5) = R0T*dRidz.transpose()*p1;
00174       }
00175   }
00176 #endif
00177 
00178 
00179   bool Edge_V_V_GICP::write(std::ostream& os) const
00180   {
00181     // first point
00182     for (int i=0; i<3; i++)
00183       os  << measurement().pos0[i] << " ";
00184     for (int i=0; i<3; i++)
00185       os  << measurement().normal0[i] << " ";
00186 
00187     // second point
00188     for (int i=0; i<3; i++)
00189       os  << measurement().pos1[i] << " ";
00190     for (int i=0; i<3; i++)
00191       os  << measurement().normal1[i] << " ";
00192 
00193 
00194     return os.good();
00195   }
00196 
00197   //
00198   // stereo camera functions
00199   //
00200 
00201 
00202 
00203   VertexSCam::VertexSCam() :
00204     VertexSE3()
00205   {}
00206 
00207 
00208   Edge_XYZ_VSC::Edge_XYZ_VSC()
00209   {}
00210 
00211 #ifdef SCAM_ANALYTIC_JACOBIANS
00212 
00215   void Edge_XYZ_VSC::linearizeOplus()
00216   {
00217     VertexSCam *vc = static_cast<VertexSCam *>(_vertices[1]);
00218 
00219     VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[0]);
00220     Vector4d pt, trans;
00221     pt.head<3>() = vp->estimate();
00222     pt(3) = 1.0;
00223     trans.head<3>() = vc->estimate().translation();
00224     trans(3) = 1.0;
00225 
00226     // first get the world point in camera coords
00227     Eigen::Matrix<double,3,1> pc = vc->w2n * pt;
00228 
00229     // Jacobians wrt camera parameters
00230     // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00231     double px = pc(0);
00232     double py = pc(1);
00233     double pz = pc(2);
00234     double ipz2 = 1.0/(pz*pz);
00235     if (isnan(ipz2) )
00236       {
00237         std::cout << "[SetJac] infinite jac" << std::endl;
00238         *(int *)0x0 = 0;
00239       }
00240 
00241     double ipz2fx = ipz2*vc->Kcam(0,0); // Fx
00242     double ipz2fy = ipz2*vc->Kcam(1,1); // Fy
00243     double b      = vc->baseline; // stereo baseline
00244 
00245     Eigen::Matrix<double,3,1> pwt;
00246 
00247     // check for local vars
00248     pwt = (pt-trans).head<3>(); // transform translations, use differential rotation
00249 
00250     // dx
00251     Eigen::Matrix<double,3,1> dp = vc->dRdx * pwt; // dR'/dq * [pw - t]
00252     _jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx;
00253     _jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy;
00254     _jacobianOplusXj(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00255     // dy
00256     dp = vc->dRdy * pwt; // dR'/dq * [pw - t]
00257     _jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx;
00258     _jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy;
00259     _jacobianOplusXj(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00260     // dz
00261     dp = vc->dRdz * pwt; // dR'/dq * [pw - t]
00262     _jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx;
00263     _jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy;
00264     _jacobianOplusXj(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00265 
00266     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00267     dp = -vc->w2n.col(0);        // dpc / dx
00268     _jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00269     _jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00270     _jacobianOplusXj(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00271     dp = -vc->w2n.col(1);        // dpc / dy
00272     _jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00273     _jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00274     _jacobianOplusXj(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00275     dp = -vc->w2n.col(2);        // dpc / dz
00276     _jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00277     _jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00278     _jacobianOplusXj(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00279 
00280     // Jacobians wrt point parameters
00281     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00282     dp = vc->w2n.col(0); // dpc / dx
00283     _jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00284     _jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00285     _jacobianOplusXi(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00286     dp = vc->w2n.col(1); // dpc / dy
00287     _jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00288     _jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00289     _jacobianOplusXi(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00290     dp = vc->w2n.col(2); // dpc / dz
00291     _jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00292     _jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00293     _jacobianOplusXi(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
00294   }
00295 #endif
00296   bool Edge_XYZ_VSC::read(std::istream&)
00297   { return false; }
00298 
00299   bool Edge_XYZ_VSC::write(std::ostream&) const
00300   { return false; }
00301 
00302   bool VertexSCam::read(std::istream&)
00303   { return false; }
00304 
00305   bool VertexSCam::write(std::ostream&) const
00306   { return false; }
00307 
00308 } // end namespace


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