Hugintrunk: hugin_base/vigra_ext/FitPolynom.h Source File

00001 // -*- c-basic-offset: 4 -*-
00024 #ifndef _FITPOLYNOM_H
00025 #define _FITPOLYNOM_H
00026 
00027 #include "hugin_math/Matrix3.h"
00028 
00029 namespace vigra_ext
00030 {
00031 
00035 template <typename M>
00036 double calcDeterminant3(const M &m)
00037 {
00038     return    m[0][0] * m[1][1] * m[2][2]
00039             + m[0][1] * m[1][2] * m[2][0]
00040             + m[0][2] * m[1][0] * m[2][1]
00041             - m[2][0] * m[1][1] * m[0][2]
00042             - m[2][1] * m[1][2] * m[0][0]
00043             - m[2][2] * m[1][0] * m[0][1];
00044 }
00045 
00054 template <class T>
00055 void FitPolynom(T x, T xend, T y, double & a, double & b, double & c)
00056     {
00057         size_t n = xend - x;
00058         // calculate various sums.
00059         double sx=0;
00060         double sx2=0;
00061         double sx3=0;
00062         double sx4=0;
00063         double sy=0;
00064         double sxy=0;
00065         double sx2y=0;
00066         T xi,yi;
00067         for (xi=x, yi=y; xi != xend; ++xi, ++yi) {
00068             double tx = *xi;
00069             double ty = *yi;
00070             double t = tx;
00071             sx += tx;
00072             sy += ty;
00073             sxy += tx * ty;
00074             tx = tx * t;
00075             sx2 += tx;
00076             sx2y += tx * ty;
00077             tx = tx * t;
00078             sx3 +=tx;
00079             sx4 += tx * t;
00080         }
00081         
00082         // X*A=Y
00083         Matrix3 X;
00084         X.m[0][0] = n;
00085         X.m[0][1] = sx;
00086         X.m[0][2] = sx2;
00087         X.m[1][0] = sx;
00088         X.m[1][1] = sx2;
00089         X.m[1][2] = sx3;
00090         X.m[2][0] = sx2;
00091         X.m[2][1] = sx3;
00092         X.m[2][2] = sx4;
00093 
00094         // calculate det(X)
00095         double D = X.Determinant(); //calcDeterminant3(X.m);
00096 
00097         // matrix to calculate a;
00098         X.m[0][0] = sy;
00099         X.m[0][1] = sx;
00100         X.m[0][2] = sx2;
00101         X.m[1][0] = sxy;
00102         X.m[1][1] = sx2;
00103         X.m[1][2] = sx3;
00104         X.m[2][0] = sx2y;
00105         X.m[2][1] = sx3;
00106         X.m[2][2] = sx4;
00107         double A = X.Determinant();
00108         a = A / D;
00109 
00110         // matrix to calculate b
00111         X.m[0][0] = n;
00112         X.m[0][1] = sy;
00113         X.m[0][2] = sx2;
00114         X.m[1][0] = sx;
00115         X.m[1][1] = sxy;
00116         X.m[1][2] = sx3;
00117         X.m[2][0] = sx2;
00118         X.m[2][1] = sx2y;
00119         X.m[2][2] = sx4;
00120         b = X.Determinant() / D;
00121 
00122         // matrix to calculate c
00123         X.m[0][0] = n;
00124         X.m[0][1] = sx;
00125         X.m[0][2] = sy;
00126         X.m[1][0] = sx;
00127         X.m[1][1] = sx2;
00128         X.m[1][2] = sxy;
00129         X.m[2][0] = sx2;
00130         X.m[2][1] = sx3;
00131         X.m[2][2] = sx2y;
00132         c = X.Determinant() / D;
00133     }
00134 
00135 }
00136 
00137 #endif // _FITPOLYNOM_H