SmallBlurryImage.cc
Go to the documentation of this file.
00001 // Copyright 2008 Isis Innovation Limited
00002 #include "ptam/SmallBlurryImage.h"
00003 #include <cvd/utility.h>
00004 #include <cvd/convolution.h>
00005 #include <cvd/vision.h>
00006 #include <TooN/se2.h>
00007 #include <TooN/Cholesky.h>
00008 #include <TooN/wls.h>
00009 
00010 using namespace CVD;
00011 using namespace std;
00012 
00013 ImageRef SmallBlurryImage::mirSize(-1,-1);
00014 
00015 SmallBlurryImage::SmallBlurryImage(KeyFrame& kf, double dBlur)
00016 {
00017   mbMadeJacs = false;
00018   MakeFromKF(kf, dBlur);
00019 }
00020 
00021 SmallBlurryImage::SmallBlurryImage()
00022 {
00023   mbMadeJacs = false;
00024 }
00025 
00026 // Make a SmallBlurryImage from a KeyFrame This fills in the mimSmall
00027 // image (Which is just a small un-blurred version of the KF) and
00028 // mimTemplate (which is a floating-point, zero-mean blurred version
00029 // of the above)
00030 void SmallBlurryImage::MakeFromKF(KeyFrame& kf, double dBlur)
00031 {
00032   if(mirSize[0] == -1)
00033     mirSize = kf.aLevels[3].im.size() / 2;
00034   mbMadeJacs = false;
00035 
00036   mimSmall.resize(mirSize);
00037   mimTemplate.resize(mirSize);
00038 
00039   mbMadeJacs = false;
00040   halfSample(kf.aLevels[3].im, mimSmall);
00041   ImageRef ir;
00042   unsigned int nSum = 0;
00043   do
00044     nSum += mimSmall[ir];
00045   while(ir.next(mirSize));
00046 
00047   float fMean = ((float) nSum) / mirSize.area();
00048 
00049   ir.home();
00050   do
00051     mimTemplate[ir] = mimSmall[ir] - fMean;
00052   while(ir.next(mirSize));
00053 
00054   convolveGaussian(mimTemplate, dBlur);
00055 }
00056 
00057 // Make the jacobians (actually, no more than a gradient image)
00058 // of the blurred template
00059 void SmallBlurryImage::MakeJacs()
00060 {
00061   mimImageJacs.resize(mirSize);
00062   // Fill in the gradient image
00063   ImageRef ir;
00064   do
00065   {
00066     Vector<2> &v2Grad = mimImageJacs[ir];
00067     if(mimTemplate.in_image_with_border(ir,1))
00068     {
00069       v2Grad[0] = mimTemplate[ir + ImageRef(1,0)] - mimTemplate[ir - ImageRef(1,0)];
00070       v2Grad[1] = mimTemplate[ir + ImageRef(0,1)] - mimTemplate[ir - ImageRef(0,1)];
00071       // N.b. missing 0.5 factor in above, this will be added later.
00072     }
00073     else
00074       v2Grad = Zeros;
00075   }
00076   while(ir.next(mirSize));
00077   mbMadeJacs = true;
00078 };
00079 
00080 // Calculate the zero-mean SSD between one image and the next.
00081 // Since both are zero mean already, just calculate the SSD...
00082 double SmallBlurryImage::ZMSSD(SmallBlurryImage &other)
00083 {
00084   double dSSD = 0.0;
00085   ImageRef ir;
00086   do
00087   {
00088     double dDiff = mimTemplate[ir] - other.mimTemplate[ir];
00089     dSSD += dDiff * dDiff;
00090   }
00091   while(ir.next(mirSize));
00092   return dSSD;
00093 }
00094 
00095 
00096 // Find an SE2 which best aligns an SBI to a target
00097 // Do this by ESM-tracking a la Benhimane & Malis
00098 pair<SE2<>,double> SmallBlurryImage::IteratePosRelToTarget(SmallBlurryImage &other, int nIterations)
00099 {
00100   SE2<> se2CtoC;
00101   SE2<> se2WfromC;
00102   ImageRef irCenter = mirSize / 2;
00103   se2WfromC.get_translation() = vec(irCenter);
00104 
00105   pair<SE2<>, double> result_pair;
00106   if(!other.mbMadeJacs)
00107   {
00108     cerr << "You spanner, you didn't make the jacs for the target." << endl;
00109     assert(other.mbMadeJacs);
00110   };
00111 
00112   double dMeanOffset = 0.0;
00113   Vector<4> v4Accum;
00114 
00115   Vector<10> v10Triangle;
00116   Image<float> imWarped(mirSize);
00117 
00118   double dFinalScore = 0.0;
00119   for(int it = 0; it<nIterations; it++)
00120   {
00121     dFinalScore = 0.0;
00122     v4Accum = Zeros;
00123     v10Triangle = Zeros; // Holds the bottom-left triangle of JTJ
00124     Vector<4> v4Jac;
00125     v4Jac[3] = 1.0;
00126 
00127     SE2<> se2XForm = se2WfromC * se2CtoC * se2WfromC.inverse();
00128 
00129     // Make the warped current image template:
00130     Vector<2> v2Zero = Zeros;
00131     CVD::transform(mimTemplate, imWarped, se2XForm.get_rotation().get_matrix(), se2XForm.get_translation(), v2Zero, -9e20f);
00132 
00133     // Now compare images, calc differences, and current image jacobian:
00134     ImageRef ir;
00135     do
00136     {
00137       if(!imWarped.in_image_with_border(ir,1))
00138         continue;
00139       float l,r,u,d,here;
00140       l = imWarped[ir - ImageRef(1,0)];
00141       r = imWarped[ir + ImageRef(1,0)];
00142       u = imWarped[ir - ImageRef(0,1)];
00143       d = imWarped[ir + ImageRef(0,1)];
00144       here = imWarped[ir];
00145       if(l + r + u + d + here < -9999.9)   // This means it's out of the image; c.f. the -9e20f param to transform.
00146         continue;
00147 
00148       Vector<2> v2CurrentGrad;
00149       v2CurrentGrad[0] = r - l; // Missing 0.5 factor
00150       v2CurrentGrad[1] = d - u;
00151 
00152       Vector<2> v2SumGrad = 0.25 * (v2CurrentGrad  + other.mimImageJacs[ir]);
00153       // Why 0.25? This is from missing 0.5 factors: One for
00154       // the fact we average two gradients, the other from
00155       // each gradient missing a 0.5 factor.
00156 
00157       v4Jac[0] = v2SumGrad[0];
00158       v4Jac[1] = v2SumGrad[1];
00159       v4Jac[2] = -(ir.y - irCenter.y) * v2SumGrad[0] + (ir.x - irCenter.x) * v2SumGrad[1];
00160       //          v4Jac[3] = 1.0;
00161 
00162       double dDiff = imWarped[ir] - other.mimTemplate[ir] + dMeanOffset;
00163       dFinalScore += dDiff * dDiff;
00164 
00165       v4Accum += dDiff * v4Jac;
00166 
00167       // Speedy fill of the LL triangle of JTJ:
00168       double *p = &v10Triangle[0];
00169       *p++ += v4Jac[0] * v4Jac[0];
00170       *p++ += v4Jac[1] * v4Jac[0];
00171       *p++ += v4Jac[1] * v4Jac[1];
00172       *p++ += v4Jac[2] * v4Jac[0];
00173       *p++ += v4Jac[2] * v4Jac[1];
00174       *p++ += v4Jac[2] * v4Jac[2];
00175       *p++ += v4Jac[0];
00176       *p++ += v4Jac[1];
00177       *p++ += v4Jac[2];
00178       *p++ += 1.0;
00179     }
00180     while(ir.next(mirSize));
00181 
00182     Vector<4> v4Update;
00183 
00184     // Solve for JTJ-1JTv;
00185     {
00186       Matrix<4> m4;
00187       int v=0;
00188       for(int j=0; j<4; j++)
00189         for(int i=0; i<=j; i++)
00190           m4[j][i] = m4[i][j] = v10Triangle[v++];
00191       Cholesky<4> chol(m4);
00192       v4Update = chol.backsub(v4Accum);
00193     }
00194 
00195     SE2<> se2Update;
00196     se2Update.get_translation() = -v4Update.slice<0,2>();
00197     se2Update.get_rotation() = SO2<>::exp(-v4Update[2]);
00198     se2CtoC = se2CtoC * se2Update;
00199     dMeanOffset -= v4Update[3];
00200   }
00201 
00202   result_pair.first = se2CtoC;
00203   result_pair.second = dFinalScore;
00204   return result_pair;
00205 }
00206 
00207 
00208 // What is the 3D camera rotation (zero trans) SE3<> which causes an
00209 // input image SO2 rotation?
00210 SE3<> SmallBlurryImage::SE3fromSE2(SE2<> se2, ATANCamera camera) 
00211 {
00212   // Do this by projecting two points, and then iterating the SE3<> (SO3
00213   // actually) until convergence. It might seem stupid doing this so
00214   // precisely when the whole SE2-finding is one big hack, but hey.
00215 
00216   camera.SetImageSize(mirSize);
00217 
00218   Vector<2> av2Turned[2];   // Our two warped points in pixels
00219   av2Turned[0] = vec(mirSize / 2) + se2 * vec(ImageRef(5,0));
00220   av2Turned[1] = vec(mirSize / 2) + se2 * vec(ImageRef(-5,0));
00221 
00222   Vector<3> av3OrigPoints[2];   // 3D versions of these points.
00223   av3OrigPoints[0] = unproject(camera.UnProject(vec(mirSize / 2) + vec(ImageRef(5,0))));
00224   av3OrigPoints[1] = unproject(camera.UnProject(vec(mirSize / 2) + vec(ImageRef(-5,0))));
00225 
00226   SO3<> so3;
00227   for(int it = 0; it<3; it++)
00228   {
00229     WLS<3> wls;  // lazy; no need for the 'W'
00230     wls.add_prior(10.0);
00231     for(int i=0; i<2; i++)
00232     {
00233       // Project into the image to find error
00234       Vector<3> v3Cam = so3 * av3OrigPoints[i];
00235       Vector<2> v2Implane = project(v3Cam);
00236       Vector<2> v2Pixels = camera.Project(v2Implane);
00237       Vector<2> v2Error = av2Turned[i] - v2Pixels;
00238 
00239       Matrix<2> m2CamDerivs = camera.GetProjectionDerivs();
00240       Matrix<2,3> m23Jacobian;
00241       double dOneOverCameraZ = 1.0 / v3Cam[2];
00242       for(int m=0; m<3; m++)
00243       {
00244         const Vector<3> v3Motion = SO3<>::generator_field(m, v3Cam);
00245         Vector<2> v2CamFrameMotion;
00246         v2CamFrameMotion[0] = (v3Motion[0] - v3Cam[0] * v3Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
00247         v2CamFrameMotion[1] = (v3Motion[1] - v3Cam[1] * v3Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
00248         m23Jacobian.T()[m] = m2CamDerivs * v2CamFrameMotion;
00249       };
00250       wls.add_mJ(v2Error[0], m23Jacobian[0], 1.0);
00251       wls.add_mJ(v2Error[1], m23Jacobian[1], 1.0);
00252     };
00253 
00254     wls.compute();
00255     Vector<3> v3Res = wls.get_mu();
00256     so3 = SO3<>::exp(v3Res) * so3;
00257   };
00258 
00259   SE3<> se3Result;
00260   se3Result.get_rotation() = so3;
00261   return se3Result;
00262 }
00263 
00264 
00265 
00266 
00267 
00268 
00269 
00270 
00271 
00272 


ptam
Author(s): Markus Achtelik , Stephan Weiss , Simon Lynen
autogenerated on Sun Oct 5 2014 23:52:33