libcvd: interpolate.h Source File

00001 /*
00002         This file is part of the CVD Library.
00003 
00004         Copyright (C) 2005 The Authors
00005 
00006         This library is free software; you can redistribute it and/or
00007         modify it under the terms of the GNU Lesser General Public
00008         License as published by the Free Software Foundation; either
00009         version 2.1 of the License, or (at your option) any later version.
00010 
00011         This library is distributed in the hope that it will be useful,
00012         but WITHOUT ANY WARRANTY; without even the implied warranty of
00013         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014         Lesser General Public License for more details.
00015 
00016         You should have received a copy of the GNU Lesser General Public
00017         License along with this library; if not, write to the Free Software
00018         Foundation, Inc.,
00019     51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00020 */
00021 
00022 #ifndef CVD_INC_INTERPOLATE_H
00023 #define CVD_INC_INTERPOLATE_H
00024 
00025 #include <cvd/image.h>
00026 #include <cvd/config.h>
00027 #include <cvd/vision.h>
00028 #include <cvd/vector_image_ref.h>
00029 
00030 namespace CVD
00031 {
00043         double interpolate_extremum(double d1, double d2, double d3)
00044         {       
00045                 assert(d2 >= d1 && d2 > d3 || d2 > d1 && d2 >= d3);
00046                 assert(d2 <= d1 && d2 < d3 || d2 < d1 && d2 <= d3);
00047                 //Use Quadratic interpolation to find the peak, position
00048                 //and hence the "real" edge position.
00049                 return -0.5 * (d3 - d1) / (d3 + d1 - 2 * d2);
00050         }
00051 
00052         #if defined CVD_HAVE_TOON || defined DOXYGEN_IGNORE_INTERNAL
00053 
00077         TooN::Vector<2> interpolate_extremum(double I__1__1,
00078                                                  double I__1_0,
00079                                                  double I__1_1,
00080                                                  double I_0__1,
00081                                                  double I_0_0,
00082                                                  double I_0_1,
00083                                                  double I_1__1,
00084                                                  double I_1_0,
00085                                                  double I_1_1)
00086         {
00087                 //Compute the gradient values
00088                 double gx = 0.5 * (I_1_0 - I__1_0);
00089                 double gy = 0.5 * (I_0_1 - I_0__1);
00090 
00091                 //Compute the Hessian values
00092                 double gxx = I_1_0 - 2 * I_0_0 + I__1_0;
00093                 double gyy = I_0_1 - 2 * I_0_0 + I_0__1;
00094                 double gxy = 0.25 * (I_1_1 + I__1__1 - I_1__1 - I__1_1);
00095 
00096                 //Compute -inv(H) * grad
00097 
00098                 double Dinv = 1./(gxx * gyy - gxy * gxy);
00099 
00100                 TooN::Vector<2> v;
00101                 v[0] = -Dinv * (gyy * gx - gxy * gy);
00102                 v[1] = -Dinv * (-gxy * gx + gxx * gy);
00103                 
00104                 return v;
00105         }
00106                                                                                  
00107 
00113         template<class I> TooN::Vector<2> interpolate_extremum(const SubImage<I>& i, ImageRef p)
00114         {
00115             CVD_IMAGE_ASSERT(p.x > 0 && p.y > 0 && p.x < i.size().x-1 && p.y < i.size().y-1, ImageError::AccessOutsideImage);
00116                 
00117                 //Extract and label 9 particular points
00118                 double I__1__1 = i[p + ImageRef(-1, -1)];
00119                 double I__1_0  = i[p + ImageRef(-1,  0)];
00120                 double I__1_1  = i[p + ImageRef(-1,  1)];
00121                 double I_0__1  = i[p + ImageRef( 0, -1)];
00122                 double I_0_0   = i[p + ImageRef( 0,  0)];
00123                 double I_0_1   = i[p + ImageRef( 0,  1)];
00124                 double I_1__1  = i[p + ImageRef( 1, -1)];
00125                 double I_1_0   = i[p + ImageRef( 1,  0)];
00126                 double I_1_1   = i[p + ImageRef( 1,  1)];
00127                 return interpolate_extremum(I__1__1, I__1_0, I__1_1, I_0__1, I_0_0, I_0_1, I_1__1, I_1_0, I_1_1) + vec(p);
00128         }
00129         
00130         #endif
00131 
00156         std::pair<TooN::Vector<2>, double> interpolate_extremum_value(double I__1__1,
00157                                                  double I__1_0,
00158                                                  double I__1_1,
00159                                                  double I_0__1,
00160                                                  double I_0_0,
00161                                                  double I_0_1,
00162                                                  double I_1__1,
00163                                                  double I_1_0,
00164                                                  double I_1_1)
00165         {
00166                 //Compute the gradient values
00167                 double gx = 0.5 * (I_1_0 - I__1_0);
00168                 double gy = 0.5 * (I_0_1 - I_0__1);
00169 
00170                 //Compute the Hessian values
00171                 double gxx = I_1_0 - 2 * I_0_0 + I__1_0;
00172                 double gyy = I_0_1 - 2 * I_0_0 + I_0__1;
00173                 double gxy = 0.25 * (I_1_1 + I__1__1 - I_1__1 - I__1_1);
00174 
00175                 //Compute -inv(H) * grad
00176 
00177                 double Dinv = 1./(gxx * gyy - gxy * gxy);
00178 
00179                 TooN::Vector<2> v;
00180                 v[0] = -Dinv * (gyy * gx - gxy * gy);
00181                 v[1] = -Dinv * (-gxy * gx + gxx * gy);
00182 
00183                 double value = I_0_0 + (gx + 0.5 * gxx * v[0] + gxy * v[1])* v[0] + (gy + 0.5 * gyy * v[1]) * v[1];
00184 
00185                 return std::make_pair(v, value);
00186         }
00187 
00193         template<class I> std::pair<TooN::Vector<2>, double> interpolate_extremum_value(const SubImage<I>& i, ImageRef p)
00194         {
00195             CVD_IMAGE_ASSERT(p.x > 0 && p.y > 0 && p.x < i.size().x-1 && p.y < i.size().y-1, ImageError::AccessOutsideImage);
00196 
00197                 //Extract and label 9 particular points
00198                 double I__1__1 = i[p + ImageRef(-1, -1)];
00199                 double I__1_0  = i[p + ImageRef(-1,  0)];
00200                 double I__1_1  = i[p + ImageRef(-1,  1)];
00201                 double I_0__1  = i[p + ImageRef( 0, -1)];
00202                 double I_0_0   = i[p + ImageRef( 0,  0)];
00203                 double I_0_1   = i[p + ImageRef( 0,  1)];
00204                 double I_1__1  = i[p + ImageRef( 1, -1)];
00205                 double I_1_0   = i[p + ImageRef( 1,  0)];
00206                 double I_1_1   = i[p + ImageRef( 1,  1)];
00207                 std::pair<TooN::Vector<2>, double> result = interpolate_extremum_value(I__1__1, I__1_0, I__1_1, I_0__1, I_0_0, I_0_1, I_1__1, I_1_0, I_1_1);
00208                 result.first += vec(p);
00209                 return result;
00210         }
00211 
00212 }
00213 
00214 #endif
00215