CVD 0.8
cvd/interpolate.h
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