CalibCornerPatch.cc
Go to the documentation of this file.
00001 // Copyright 2008 Isis Innovation Limited
00002 #include "ptam/CalibCornerPatch.h"
00003 #include "ptam/OpenGL.h"
00004 #include <TooN/helpers.h>
00005 #include <cvd/vector_image_ref.h>
00006 #include <cvd/vision.h>
00007 #include <cvd/utility.h>
00008 #include <cvd/convolution.h>
00009 #include <cvd/image_interpolate.h>
00010 #include <TooN/Cholesky.h>
00011 #include "ptam/SmallMatrixOpts.h"
00012 
00013 using namespace std;
00014 using namespace CVD;
00015 
00016 Image<float> CalibCornerPatch::mimSharedSourceTemplate;
00017 
00018 CalibCornerPatch::CalibCornerPatch(int nSideSize)
00019 {
00020   mimTemplate.resize(ImageRef(nSideSize, nSideSize));
00021   mimGradients.resize(ImageRef(nSideSize, nSideSize));
00022   mimAngleJacs.resize(ImageRef(nSideSize, nSideSize));
00023   if(mimSharedSourceTemplate.size().x == 0)
00024   {
00025     MakeSharedTemplate();
00026   }
00027 }
00028 
00029 void CalibCornerPatch::MakeTemplateWithCurrentParams()
00030 {
00031   double dBlurSigma = 2.0;
00032   int nExtraPixels = (int) (dBlurSigma * 6.0) + 2;
00033 
00034   Image<float> imToBlur(mimTemplate.size() + ImageRef(nExtraPixels, nExtraPixels));
00035   Image<float> imTwiceToBlur(imToBlur.size() * 2);
00036 
00037   // Make actual template:
00038   int nOffset;
00039   {
00040     Matrix<2> m2Warp = mParams.m2Warp();
00041     CVD::transform(mimSharedSourceTemplate, imTwiceToBlur,
00042                    M2Inverse(m2Warp),
00043                    vec(mimSharedSourceTemplate.size() - ImageRef(1,1)) * 0.5,
00044                    vec(imTwiceToBlur.size() - ImageRef(1,1)) * 0.5);
00045     halfSample(imTwiceToBlur, imToBlur);
00046     convolveGaussian(imToBlur, dBlurSigma);
00047 
00048     nOffset = (imToBlur.size().x - mimTemplate.size().x) / 2;
00049     copy(imToBlur, mimTemplate, mimTemplate.size(), ImageRef(nOffset, nOffset));
00050   };
00051 
00052   // Make numerical angle jac images:
00053   for(int dof=0; dof<2; dof++)
00054   {
00055     Matrix<2> m2Warp;
00056     for(int i=0; i<2; i++)
00057     {
00058       double dAngle = mParams.v2Angles[i];
00059       if(dof == i)
00060         dAngle += 0.01;
00061       m2Warp[0][i] = cos(dAngle);
00062       m2Warp[1][i] = sin(dAngle);
00063     };
00064 
00065     CVD::transform(mimSharedSourceTemplate, imTwiceToBlur,
00066                    M2Inverse(m2Warp),
00067                    vec(mimSharedSourceTemplate.size() - ImageRef(1,1)) * 0.5,
00068                    vec(imTwiceToBlur.size() - ImageRef(1,1)) * 0.5);
00069     halfSample(imTwiceToBlur, imToBlur);
00070     convolveGaussian(imToBlur, dBlurSigma);
00071     ImageRef ir;
00072     do
00073       mimAngleJacs[ir][dof] = (imToBlur[ir + ImageRef(nOffset, nOffset)] - mimTemplate[ir]) / 0.01;
00074     while(ir.next(mimTemplate.size()));
00075   };
00076 
00077   // Make the image of image gradients here too (while we have the bigger template to work from)
00078   ImageRef ir;
00079   do
00080   {
00081     mimGradients[ir][0] = 0.5 *
00082         (imToBlur[ir + ImageRef(nOffset + 1, nOffset)] -
00083             imToBlur[ir + ImageRef(nOffset - 1, nOffset)]);
00084     mimGradients[ir][1] = 0.5 *
00085         (imToBlur[ir + ImageRef(nOffset, nOffset + 1 )] -
00086             imToBlur[ir + ImageRef(nOffset, nOffset - 1 )]);
00087   }
00088   while(ir.next(mimGradients.size()));
00089 }
00090 
00091 bool CalibCornerPatch::IterateOnImageWithDrawing(CalibCornerPatch::Params &params, Image<byte> &im)
00092 {
00093   bool bReturn = IterateOnImage(params, im);
00094   if(!bReturn)
00095   {
00096     glPointSize(3);
00097     glColor3f(1,0,0);
00098     glBegin(GL_POINTS);
00099     glVertex(params.v2Pos);
00100     glEnd();
00101   }
00102   return bReturn;
00103 }
00104 
00105 bool CalibCornerPatch::IterateOnImage(CalibCornerPatch::Params &params, Image<byte> &im)
00106 {
00107   mParams = params;
00108   double dLastUpdate = 0.0;
00109   for(int i=0; i<20; i++)
00110   {
00111     MakeTemplateWithCurrentParams();
00112     dLastUpdate = Iterate(im);
00113 
00114     if(dLastUpdate < 0)
00115       return false;
00116     if(dLastUpdate < 0.00001)
00117       break;
00118   }
00119   if(dLastUpdate > 0.001)
00120     return false;
00121   if(fabs(sin(mParams.v2Angles[0] - mParams.v2Angles[1])) < sin(M_PI / 6.0))
00122     return false;
00123   if(fabs(mParams.dGain) < 20.0)
00124     return false;
00125   if(mdLastError > 25.0)
00126     return false;
00127 
00128   params = mParams;
00129   return true;
00130 }
00131 
00132 double CalibCornerPatch::Iterate(Image<byte> &im)
00133 { 
00134   Vector<2> v2TL = mParams.v2Pos - vec(mimTemplate.size() - ImageRef(1,1)) / 2.0;
00135   if(v2TL[0] < 0.0 || v2TL[1] < 0.0)
00136     return -1.0;
00137   Vector<2> v2BR = mParams.v2Pos + vec(mimTemplate.size()) + vec(mimTemplate.size() - ImageRef(1,1)) / 2.0;
00138   if(v2BR[0] > (im.size().x - 1.0) || v2BR[1] > (im.size().y - 1.0))
00139     return -1.0;
00140 
00141   image_interpolate<Interpolate::Bilinear, byte> imInterp(im);
00142   Matrix<6> m6JTJ = Zeros;
00143   Vector<6> v6JTD = Zeros;
00144 
00145 
00146 
00147   ImageRef ir;
00148   double dSum = 0.0;
00149   do
00150   {
00151     Vector<2> v2Pos_Template = vec(ir) - vec(mimTemplate.size() - ImageRef(1,1)) / 2.0;
00152     Vector<2> v2Pos_Image = mParams.v2Pos + v2Pos_Template;
00153     double dDiff = imInterp[v2Pos_Image] - (mParams.dGain * mimTemplate[ir] + mParams.dMean);
00154     dSum += fabs(dDiff);
00155     Vector<6> v6Jac;
00156     // Jac for center pos: Minus sign because +pos equates to sliding template -
00157     v6Jac.slice<0,2>() = -1.0 * mParams.dGain * mimGradients[ir];
00158     // Jac for angles: dPos/dAngle needs finishing by multiplying by pos..
00159     v6Jac[2] =  mimAngleJacs[ir][0] * mParams.dGain;
00160     v6Jac[3] =  mimAngleJacs[ir][1] * mParams.dGain;
00161     // Jac for mean:
00162     v6Jac[4] = 1.0;
00163     // Jac for gain:
00164     v6Jac[5] = mimTemplate[ir];
00165 
00166     m6JTJ += v6Jac.as_col() * v6Jac.as_row();
00167     v6JTD += dDiff * v6Jac;
00168   }
00169   while(ir.next(mimTemplate.size()));
00170 
00171   Cholesky<6> chol(m6JTJ);
00172   Vector<6> v6Update = 0.7 * chol.backsub(v6JTD);
00173   
00174     //reject bogus updates
00175   if(std::isnan(v6Update[4]) || std::isnan(v6Update[5])){
00176     return -1.0;
00177   } 
00178   
00179   mParams.v2Pos += v6Update.slice<0,2>();
00180   mParams.v2Angles += v6Update.slice<2,2>();
00181   mParams.dMean += v6Update[4];
00182   mParams.dGain += v6Update[5];
00183   mdLastError = dSum / mimTemplate.size().area();
00184   return sqrt(v6Update.slice<0,2>() * v6Update.slice<0,2>());
00185 }
00186 
00187 void CalibCornerPatch::MakeSharedTemplate()
00188 {
00189   const int nSideSize = 100;
00190   const int nHalf = nSideSize / 2;
00191 
00192   mimSharedSourceTemplate.resize(ImageRef(nSideSize, nSideSize));
00193 
00194   ImageRef ir;
00195 
00196   do
00197   {
00198     float fX = (ir.x < nHalf) ? 1.0 : -1.0;
00199     float fY = (ir.y < nHalf) ? 1.0 : -1.0;
00200     mimSharedSourceTemplate[ir] = fX * fY;
00201   }
00202   while(ir.next(mimSharedSourceTemplate.size()));
00203 }
00204 
00205 CalibCornerPatch::Params::Params()
00206 {
00207   v2Angles[0] = 0.0;
00208   v2Angles[1] = M_PI / 2.0;
00209   dMean = 0.0;
00210   dGain = 1.0;
00211 }
00212 
00213 Matrix<2> CalibCornerPatch::Params::m2Warp()
00214 {
00215   Matrix<2> m2Warp;
00216   for(int i=0; i<2; i++)
00217   {
00218     m2Warp[0][i] = cos(v2Angles[i]);
00219     m2Warp[1][i] = sin(v2Angles[i]);
00220   };
00221   return m2Warp;
00222 }
00223 
00224 
00225 
00226 
00227 
00228 
00229 
00230 
00231 
00232 


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