proj.cpp
Go to the documentation of this file.
00001 #include <sparse_bundle_adjustment/proj.h>
00002 
00003 namespace sba
00004 {
00005   Proj::Proj(int ci, Eigen::Vector3d &q, bool stereo)
00006       : ndi(ci), kp(q), stereo(stereo), 
00007         isValid(true), useCovar(false), pointPlane(false) {}
00008       
00009   Proj::Proj(int ci, Eigen::Vector2d &q) 
00010       : ndi(ci), kp(q(0), q(1), 0), 
00011         stereo(false), isValid(true), useCovar(false), pointPlane(false) {}
00012   
00013   Proj::Proj() 
00014       : ndi(0), kp(0, 0, 0), 
00015         stereo(false), isValid(false), useCovar(false), pointPlane(false) {}
00016 
00017   void Proj::setJacobians(const Node &nd, const Point &pt, JacobProds *jpp)
00018   {
00019     if (stereo)
00020       setJacobiansStereo_(nd, pt, jpp);
00021     else
00022       setJacobiansMono_(nd, pt, jpp);
00023   }
00024   
00025   double Proj::calcErr(const Node &nd, const Point &pt, const double huber)
00026   {
00027     if (stereo)
00028       return calcErrStereo_(nd, pt, huber);
00029     else
00030       return calcErrMono_(nd, pt, huber);
00031   }
00032   
00033   double Proj::getErrNorm()
00034   {
00035     if (stereo)
00036       return err.norm();
00037     else
00038       return err.head<2>().norm();
00039   }
00040   
00041   double Proj::getErrSquaredNorm()
00042   {
00043     if (stereo)
00044       return err.squaredNorm();
00045     else
00046       return err.head<2>().squaredNorm();
00047   }
00048   
00049   void Proj::setCovariance(const Eigen::Matrix3d &covar)
00050   {
00051     useCovar = true;
00052     covarmat = covar;
00053   }
00054   
00055   void Proj::clearCovariance()
00056   {
00057     useCovar = false;
00058   }
00059 
00060   void Proj::setJacobiansMono_(const Node &nd, const Point &pt, JacobProds *jpp)
00061   {
00062     // first get the world point in camera coords
00063     Eigen::Matrix<double,3,1> pc = nd.w2n * pt;
00064 
00066     Eigen::Matrix<double,2,6> jacc;
00067     
00069     Eigen::Matrix<double,2,3> jacp;
00070 
00071     // Jacobians wrt camera parameters
00072     // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00073     double px = pc(0);
00074     double py = pc(1);
00075     double pz = pc(2);
00076     double ipz2 = 1.0/(pz*pz);
00077     if (isnan(ipz2) ) { printf("[SetJac] infinite jac\n");  *(int *)0x0 = 0; }
00078 
00079     double ipz2fx = ipz2*nd.Kcam(0,0); // Fx
00080     double ipz2fy = ipz2*nd.Kcam(1,1); // Fy
00081     // scale quaternion derivative to match the translational ones
00082     double ipz2fxq = qScale*ipz2fx;
00083     double ipz2fyq = qScale*ipz2fy;
00084     Eigen::Matrix<double,3,1> pwt;
00085 
00086     // check for local vars
00087     pwt = (pt-nd.trans).head<3>(); // transform translations, use differential rotation
00088 
00089     // dx
00090     Eigen::Matrix<double,3,1> dp = nd.dRdx * pwt; // dR'/dq * [pw - t]
00091     jacc(0,3) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00092     jacc(1,3) = (pz*dp(1) - py*dp(2))*ipz2fyq;
00093     // dy
00094     dp = nd.dRdy * pwt; // dR'/dq * [pw - t]
00095     jacc(0,4) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00096     jacc(1,4) = (pz*dp(1) - py*dp(2))*ipz2fyq;
00097     // dz
00098     dp = nd.dRdz * pwt; // dR'/dq * [pw - t]
00099     jacc(0,5) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00100     jacc(1,5) = (pz*dp(1) - py*dp(2))*ipz2fyq;
00101 
00102     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00103     dp = -nd.w2n.col(0);        // dpc / dx
00104     jacc(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00105     jacc(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00106     dp = -nd.w2n.col(1);        // dpc / dy
00107     jacc(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00108     jacc(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00109     dp = -nd.w2n.col(2);        // dpc / dz
00110     jacc(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00111     jacc(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00112 
00113     // Jacobians wrt point parameters
00114     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00115     dp = nd.w2n.col(0); // dpc / dx
00116     jacp(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00117     jacp(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00118     dp = nd.w2n.col(1); // dpc / dy
00119     jacp(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00120     jacp(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00121     dp = nd.w2n.col(2); // dpc / dz
00122     jacp(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00123     jacp(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00124 
00125 #ifdef DEBUG
00126     for (int i=0; i<2; i++)
00127       for (int j=0; j<6; j++)
00128         if (isnan(jacc(i,j)) ) { printf("[SetJac] NaN in jacc(%d,%d)\n", i, j);  *(int *)0x0 = 0; }
00129 #endif
00130     
00131     // Set Hessians + extras.
00132     jpp->Hpp = jacp.transpose() * jacp;
00133     jpp->Hcc = jacc.transpose() * jacc;
00134     jpp->Hpc = jacp.transpose() * jacc;
00135     jpp->JcTE = jacc.transpose() * err.head<2>();
00136     jpp->Bp = jacp.transpose() * err.head<2>();
00137 
00138     jp = jpp;
00139   }
00140 
00141   // calculate error of a projection
00142   // we should do something about negative Z
00143   double Proj::calcErrMono_(const Node &nd, const Point &pt, double huber)
00144   {
00145     Eigen::Vector3d p1 = nd.w2i * pt; 
00146     err(2) = 0.0;
00147     if (p1(2) <= 0.0) 
00148     {
00149 #ifdef DEBUG
00150       printf("[CalcErr] negative Z! Node %d \n",ndi);
00151       if (isnan(err[0]) || isnan(err[1]) ) printf("[CalcErr] NaN!\n"); 
00152 #endif
00153       err = Eigen::Vector3d(0.0,0.0,0.0);
00154       return 0.0;
00155     }
00156     else
00157       err.head<2>() = p1.head<2>()/p1(2); 
00158 
00159     err -= kp;
00160 
00161     // pseudo-Huber weighting
00162     // C(e) = 2*s^2*[sqrt(1+(e/s)^2)-1]
00163     // w = sqrt(C(norm(e)))/norm(e)
00164 
00165     if (huber > 0)
00166       {
00167         double b2 = huber*huber; // kernel width
00168         double e2 = err.head<2>().squaredNorm();
00169         if (e2 > b2)
00170           {
00171             double c = 2.0*huber*sqrt(e2) - b2;
00172             double w = sqrt(c/e2);
00173             err.head<2>() *= w; // weight the error
00174             //            std::cout << "Huber weight: " << w << "  Err sq: " << e2 << std::endl;
00175           }
00176 
00177 
00178 //         double b2 = HUBER*HUBER;        // kernel width
00179 //         double e2 = err.squaredNorm();
00180 //         e2 = std::max(e2,1e-22);    // can't have a zero here
00181 //         double w = 2*b2*(sqrt(1+e2/b2)-1);
00182 //         w = sqrt(w/e2);
00183 //         err.head<2>() *= w;         // weight the error
00184       }
00185 
00186     return err.head<2>().squaredNorm(); 
00187   }
00188 
00189 
00190   void Proj::setJacobiansStereo_(const Node &nd, const Point &pt, JacobProds *jpp)
00191   {
00192     // first get the world point in camera coords
00193     Eigen::Matrix<double,3,1> pc = nd.w2n * pt;
00194 
00196     Eigen::Matrix<double,3,3> jacp;
00197     
00199     Eigen::Matrix<double,3,6> jacc;
00200 
00201     // Jacobians wrt camera parameters
00202     // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00203     double px = pc(0);
00204     double py = pc(1);
00205     double pz = pc(2);
00206     double ipz2 = 1.0/(pz*pz);
00207     if (isnan(ipz2) ) { printf("[SetJac] infinite jac\n");  *(int *)0x0 = 0; }
00208 
00209     double ipz2fx = ipz2*nd.Kcam(0,0); // Fx
00210     double ipz2fy = ipz2*nd.Kcam(1,1); // Fy
00211     double b      = nd.baseline; // stereo baseline
00212     // scale quaternion derivative to match the translational ones
00213     double ipz2fxq = qScale*ipz2fx;
00214     double ipz2fyq = qScale*ipz2fy;
00215     Eigen::Matrix<double,3,1> pwt;
00216 
00217     // check for local vars
00218     pwt = (pt-nd.trans).head(3); // transform translations, use differential rotation
00219 
00220     // dx
00221     Eigen::Matrix<double,3,1> dp = nd.dRdx * pwt; // dR'/dq * [pw - t]
00222     jacc(0,3) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00223     jacc(1,3) = (pz*dp(1) - py*dp(2))*ipz2fyq;
00224     jacc(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
00225     // dy
00226     dp = nd.dRdy * pwt; // dR'/dq * [pw - t]
00227     jacc(0,4) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00228     jacc(1,4) = (pz*dp(1) - py*dp(2))*ipz2fyq;
00229     jacc(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
00230     // dz
00231     dp = nd.dRdz * pwt; // dR'/dq * [pw - t]
00232     jacc(0,5) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00233     jacc(1,5) = (pz*dp(1) - py*dp(2))*ipz2fyq;
00234     jacc(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
00235 
00236     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00237     dp = -nd.w2n.col(0);        // dpc / dx
00238     jacc(0,0) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00239     jacc(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00240     jacc(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
00241     dp = -nd.w2n.col(1);        // dpc / dy
00242     jacc(0,1) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00243     jacc(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00244     jacc(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
00245     dp = -nd.w2n.col(2);        // dpc / dz
00246     jacc(0,2) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00247     jacc(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00248     jacc(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
00249 
00250     // Jacobians wrt point parameters
00251     // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
00252     dp = nd.w2n.col(0); // dpc / dx
00253     jacp(0,0) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00254     jacp(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00255     jacp(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
00256     dp = nd.w2n.col(1); // dpc / dy
00257     jacp(0,1) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00258     jacp(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00259     jacp(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
00260     dp = nd.w2n.col(2); // dpc / dz
00261     jacp(0,2) = (pz*dp(0) - px*dp(2))*ipz2fxq;
00262     jacp(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00263     jacp(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
00264 
00265 #ifdef DEBUG
00266     for (int i=0; i<2; i++)
00267       for (int j=0; j<6; j++)
00268         if (isnan(jacc(i,j)) ) { printf("[SetJac] NaN in jacc(%d,%d)\n", i, j);  *(int *)0x0 = 0; }
00269 #endif
00270     if (useCovar)
00271     {
00272       jacc = covarmat * jacc;
00273       jacp = covarmat * jacp;
00274     }
00275 
00276     // Set Hessians + extras.
00277     jpp->Hpp = jacp.transpose() * jacp;
00278     jpp->Hcc = jacc.transpose() * jacc;
00279     jpp->Hpc = jacp.transpose() * jacc;
00280     jpp->JcTE = jacc.transpose() * err;
00281     jpp->Bp = jacp.transpose() * err;
00282     
00283     jp = jpp;
00284   }
00285 
00286   // calculate error of a projection
00287   // we should do something about negative Z
00288   double Proj::calcErrStereo_(const Node &nd, const Point &pt, double huber)
00289   { 
00290     Eigen::Vector3d p1 = nd.w2i * pt; 
00291     Eigen::Vector3d p2 = nd.w2n * pt; 
00292     Eigen::Vector3d pb(nd.baseline,0,0);
00293     
00294     // TODO: Clean this up a bit. 
00295     if (pointPlane)
00296     {
00297       // Project point onto plane.
00298       Eigen::Vector3d w = pt.head<3>()-plane_point;
00299 
00300       //printf("w: %f %f %f\n", w.x(), w.y(), w.z());
00301       //Eigen::Vector3d projpt = pt.head<3>()+(w.dot(plane_normal))*plane_normal;
00302       Eigen::Vector3d projpt = plane_point+(w.dot(plane_normal))*plane_normal;
00303       //      Eigen::Vector3d projpt = pt.head<3>()+(w.dot(plane_normal))*plane_normal;
00304       //printf("[Proj] Distance to plane: %f\n", w.dot(plane_normal));
00305       p1 = nd.w2i*Eigen::Vector4d(projpt.x(), projpt.y(), projpt.z(), 1.0);
00306       p2 = nd.w2n*Eigen::Vector4d(projpt.x(), projpt.y(), projpt.z(), 1.0);
00307     }
00308     
00309     double invp1 = 1.0/p1(2);
00310     
00311     err.head<2>() = p1.head<2>()*invp1;
00312     // right camera px
00313     p2 = nd.Kcam*(p2-pb);
00314  
00315     err(2) = p2(0)/p2(2);
00316     if (p1(2) <= 0.0) 
00317     {
00318 #ifdef DEBUG
00319       printf("[CalcErr] negative Z! Node %d\n",ndi);
00320       if (isnan(err[0]) || isnan(err[1]) ) printf("[CalcErr] NaN!\n"); 
00321 #endif
00322       err.setZero();
00323       
00324       return 0.0;
00325     }
00326     err -= kp;
00327     
00328     if (abs(err(0)) > 1e6 || abs(err(1)) > 1e6 || abs(err(2)) > 1e6)
00329     {
00330       printf("\n\n[CalcErr] Excessive error.\n");
00331       
00332       isValid = false;
00333     }
00334     
00335     if (useCovar)
00336       err = covarmat*err;
00337      
00338     // Huber kernel weighting
00339     if (huber > 0.0)
00340       {
00341         double b2 = huber*huber; // kernel width
00342         double e2 = err.squaredNorm();
00343         if (e2 > b2)
00344           {
00345             double c = 2.0*huber*sqrt(e2) - b2;
00346             double w = sqrt(c/e2);
00347             err *= w;
00348             //            std::cout << "Huber weight: " << w << "  Err sq: " << e2 << std::endl;
00349           }
00350       }
00351 
00352     return err.squaredNorm();
00353   }
00354   
00355   // Constructors for track.
00356   Track::Track() : point() { }
00357   Track::Track(Point p) : point(p) { }
00358       
00359 } // sba
00360 


sparse_bundle_adjustment
Author(s):
autogenerated on Wed Aug 24 2016 03:37:37