thermalvis: thermalvis: geometry.cpp Source File

00001 
00005 #include "geometry.hpp"
00006 
00007 double normalizedGRICdifference(vector<Point2f>& pts1, vector<Point2f>& pts2, Mat& F, Mat& H, Mat& mask_F, Mat& mask_H, double& F_GRIC, double& H_GRIC) {
00008         
00009         int d, k;
00010         double lambda_3 = 2.00;
00011         double r = 4.00;        // assuming always with 2-frames
00012         int distMethod;
00013         
00014         // determine d & k
00015         d = 2;
00016         k = 8;
00017         distMethod = LOURAKIS_DISTANCE;
00018         H_GRIC = calculateGRIC(pts1, pts2, H, mask_H, d, k, r, lambda_3, distMethod);
00019         
00020         d = 3;
00021         k = 7;
00022         distMethod = SAMPSON_DISTANCE;
00023         F_GRIC = calculateGRIC(pts1, pts2, F, mask_F, d, k, r, lambda_3, distMethod);
00024         
00025         double retVal = abs(F_GRIC - H_GRIC) / H_GRIC;
00026         
00027         return retVal;
00028         
00029         
00030 }
00031 
00032 double calculateGRIC(vector<Point2f>& pts1, vector<Point2f>& pts2, Mat& rel, Mat& mask, int d, double k, double r, double lambda_3, int distMethod) {
00033         
00034         double retVal = 0.00;
00035 
00036         int n = pts1.size(); // countNonZero(mask);
00037         
00038         double lambda_1 = log(r);
00039         double lambda_2 = log(r*((double)n));
00040         
00041         double *e_vals;
00042         e_vals = new double[n];
00043         
00044         // gotta calculate sig and e
00045         
00046         // they calculate 'e' using:
00047         //              for F: point to epipolar line cost
00048         //              for H: symmetric transfer error
00049         
00050         double e_mean = 0.00, sig = 0.00;
00051         
00052         for (unsigned int iii = 0; iii < pts1.size(); iii++) {
00053                 
00054                 e_vals[iii] = calcGeometryDistance(pts1.at(iii), pts2.at(iii), rel, distMethod);
00055                 e_mean += e_vals[iii] / ((double) n);
00056                 
00057         }
00058         
00059         // Calculate variance
00060         
00061         for (unsigned int iii = 0; iii < pts1.size(); iii++) {
00062                 sig += pow((e_vals[iii] - e_mean), 2.0) / ((double) n);
00063         }
00064         
00065         for (unsigned int iii = 0; iii < pts1.size(); iii++) {
00066                 retVal += calculateRho(e_vals[iii], sig, r, lambda_3, d);
00067         }
00068         
00069         retVal +=  lambda_1*((double) d)*n + lambda_2*((double) k);
00070 
00071         return retVal;
00072         
00073 }
00074 
00075 double calculateRho(double e, double sig, double r, double lambda_3, int d) {
00076         
00077         double val1 = pow(e, 2.0) / pow(sig, 2.0);
00078         double val2 = lambda_3 * (r - ((double) d));
00079         
00080         return min(val1, val2);
00081         
00082 }
00083 
00084 double calcGeometryDistance(Point2f& pt1, Point2f& pt2, Mat& mat, int distMethod) {
00085         
00086         Mat p1, p2;
00087         p1 = Mat::ones(3,1,CV_64FC1);
00088         p2 = Mat::ones(3,1,CV_64FC1);
00089         
00090         p1.at<double>(0,0) = (double) pt1.x;
00091         p1.at<double>(1,0) = (double) pt1.y;
00092         
00093         p2.at<double>(0,0) = (double) pt2.x;
00094         p2.at<double>(1,0) = (double) pt2.y;
00095         
00096         Mat p2t;
00097         transpose(p2, p2t);
00098         
00099         double dist = 0.0;
00100         
00101         double ri;
00102         
00103         Mat A;
00104         
00105         A = p2t * mat * p1; 
00106         ri = A.at<double>(0,0); 
00107         
00108         double r = abs(ri);     // not sure if this is correct and/or needed
00109                         
00110         double rx, ry, rxd, ryd;
00111         
00112         rx = mat.at<double>(0,0) * pt2.x + mat.at<double>(1,0) * pt2.y + mat.at<double>(2,0);
00113         ry = mat.at<double>(0,1) * pt2.x + mat.at<double>(1,1) * pt2.y + mat.at<double>(2,1);
00114         rxd = mat.at<double>(0,0) * pt1.x + mat.at<double>(0,1) * pt1.y + mat.at<double>(0,2);
00115         ryd = mat.at<double>(1,0) * pt1.x + mat.at<double>(1,1) * pt1.y + mat.at<double>(1,2);
00116         
00117         double e1, e2;
00118         
00119         e1 = r / pow( pow(rx, 2.0) + pow(ry, 2.0) , 0.5);
00120         e2 = r / pow( pow(rxd, 2.0) + pow(ryd, 2.0) , 0.5);
00121         
00122         double de;
00123         
00124         de = pow(pow(e1, 2.0) + pow(e2, 2.0), 0.5);
00125         
00126         double ds;
00127         
00128         ds = r * pow((1 / (pow(rx, 2.0) + pow(ry, 2.0) + pow(rxd, 2.0) + pow(ryd, 2.0))), 0.5);
00129         
00130         switch (distMethod) {
00131                 case SAMPSON_DISTANCE:
00132                         dist = ds;
00133                         break;
00134                 case ALGEBRAIC_DISTANCE: 
00135                         dist = r;
00136                         break;
00137                 case EPIPOLAR_DISTANCE:
00138                         dist = de;
00139                         break;
00140                 case LOURAKIS_DISTANCE:
00141                         dist = lourakisSampsonError(pt1, pt2, mat);
00142                         break;
00143                 default:
00144                         break;
00145         }
00146         
00147         return dist;
00148 }
00149 
00150 // http://www.ics.forth.gr/~lourakis/homest/
00151 double lourakisSampsonError(Point2f& pt1, Point2f& pt2, Mat& H) {
00152         
00153         double error = 0.0;
00154         
00155         double t1, t10, t100, t104, t108, t112, t118, t12, t122, t125, t126, t129, t13, t139, t14, t141, t144, t15, t150, t153, t161, t167, t17, t174, t18, t19, t193, t199;
00156         double t2, t20, t201, t202, t21, t213, t219, t22, t220, t222, t225, t23, t236, t24, t243, t250, t253, t26, t260, t27, t271, t273, t28, t29, t296;
00157         double t3, t30, t303, t31, t317, t33, t331, t335, t339, t34, t342, t345, t35, t350, t354, t36, t361, t365, t37, t374, t39;
00158         double t4, t40, t41, t42, t43, t44, t45, t46, t47, t49, t51, t57;
00159         double t6, t65, t66, t68, t69;
00160         double t7, t72, t78;
00161         double t8, t86, t87, t90, t95;
00162         
00163         double h[9];
00164         
00165         for (int iii = 0; iii < 3; iii++) {
00166                 for (int jjj = 0; jjj < 3; jjj++) {
00167                         h[3*iii + jjj] = H.at<double>(iii,jjj);
00168                 }
00169         }
00170         
00171         double m1[2], m2[2];
00172         
00173         m1[0] = (double) pt1.x;
00174         m1[1] = (double) pt1.y;
00175         m2[0] = (double) pt2.x;
00176         m2[1] = (double) pt2.y;
00177         
00178         t1 = m2[0];
00179         t2 = h[6];
00180         t3 = t2*t1;
00181         t4 = m1[0];
00182         t6 = h[7];
00183         t7 = t1*t6;
00184         t8 = m1[1];
00185         t10 = h[8];
00186         t12 = h[0];
00187         t13 = t12*t4;
00188         t14 = h[1];
00189         t15 = t14*t8;
00190         t17 = t3*t4+t7*t8+t1*t10-t13-t15-h[2];
00191         t18 = m2[1];
00192         t19 = t18*t18;
00193         t20 = t2*t2;
00194         t21 = t19*t20;
00195         t22 = t18*t2;
00196         t23 = h[3];
00197         t24 = t23*t22;
00198         t26 = t23*t23;
00199         t27 = t6*t6;
00200         t28 = t19*t27;
00201         t29 = t18*t6;
00202         t30 = h[4];
00203         t31 = t29*t30;
00204         t33 = t30*t30;
00205         t34 = t4*t4;
00206         t35 = t20*t34;
00207         t36 = t2*t4;
00208         t37 = t6*t8;
00209         t39 = 2.0*t36*t37;
00210         t40 = t36*t10;
00211         t41 = 2.0*t40;
00212         t42 = t8*t8;
00213         t43 = t42*t27;
00214         t44 = t37*t10;
00215         t45 = 2.0*t44;
00216         t46 = t10*t10;
00217         t47 = t21-2.0*t24+t26+t28-2.0*t31+t33+t35+t39+t41+t43+t45+t46;
00218         t49 = t12*t12;
00219         t51 = t6*t30;
00220         t57 = t20*t2;
00221         t65 = t1*t1;
00222         t66 = t65*t20;
00223         t68 = t65*t57;
00224         t69 = t4*t10;
00225         t72 = t2*t49;
00226         t78 = t27*t6;
00227         t86 = t65*t78;
00228         t87 = t8*t10;
00229         t90 = t65*t27;
00230         t95 = -2.0*t49*t18*t51-2.0*t3*t12*t46-2.0*t1*t57*t12*t34-2.0*t3*t12*t33+t66
00231         *t43+2.0*t68*t69+2.0*t72*t69-2.0*t7*t14*t46-2.0*t1*t78*t14*t42-2.0*t7*t14*t26+
00232         2.0*t86*t87+t90*t35+2.0*t49*t6*t87;
00233         t100 = t14*t14;
00234         t104 = t100*t2;
00235         t108 = t2*t23;
00236         t112 = t78*t42*t8;
00237         t118 = t57*t34*t4;
00238         t122 = t10*t26;
00239         t125 = t57*t4;
00240         t126 = t10*t19;
00241         t129 = t78*t8;
00242         t139 = -2.0*t57*t34*t18*t23+2.0*t100*t6*t87+2.0*t104*t69-2.0*t100*t18*t108+
00243         4.0*t36*t112+6.0*t43*t35+4.0*t118*t37+t35*t28+2.0*t36*t122+2.0*t125*t126+2.0*
00244         t129*t126+2.0*t37*t122-2.0*t78*t42*t18*t30+t43*t21;
00245         t141 = t10*t33;
00246         t144 = t46*t18;
00247         t150 = t46*t19;
00248         t153 = t46*t10;
00249         t161 = t27*t27;
00250         t167 = 2.0*t36*t141-2.0*t144*t108+2.0*t37*t141+t66*t33+t150*t27+t150*t20+
00251         4.0*t37*t153+6.0*t43*t46+4.0*t112*t10+t43*t33+t161*t42*t19+t43*t26+4.0*t36*t153
00252         ;
00253         t174 = t20*t20;
00254         t193 = 6.0*t35*t46+4.0*t10*t118+t35*t33+t35*t26+t174*t34*t19+t100*t27*t42+
00255         t100*t20*t34+t100*t19*t20+t90*t46+t65*t161*t42+t90*t26+t49*t27*t42+t49*t20*t34+
00256         t49*t19*t27;
00257         t199 = t34*t34;
00258         t201 = t12*t23;
00259         t202 = t14*t30;
00260         t213 = t42*t42;
00261         t219 = t66*t46+t100*t26+t46*t100+t174*t199-2.0*t201*t202-2.0*t144*t51+t46*
00262         t26+t65*t174*t34+t49*t33+t49*t46+t46*t33+t161*t213-2.0*t7*t14*t20*t34;
00263         t220 = t1*t27;
00264         t222 = t36*t8;
00265         t225 = t7*t14;
00266         t236 = t4*t6*t8;
00267         t243 = t3*t12;
00268         t250 = t46*t46;
00269         t253 = t1*t20;
00270         t260 = -4.0*t220*t14*t222-4.0*t225*t40-4.0*t220*t15*t10+2.0*t90*t40+2.0*
00271         t225*t24+2.0*t72*t236-2.0*t3*t12*t27*t42-4.0*t243*t44+2.0*t66*t44+2.0*t243*t31+
00272         t250+2.0*t68*t236-4.0*t253*t12*t236-4.0*t253*t13*t10;
00273         t271 = t4*t20;
00274         t273 = t8*t18;
00275         t296 = t10*t18;
00276         t303 = 2.0*t104*t236-2.0*t35*t31+12.0*t35*t44+2.0*t125*t37*t19-4.0*t271*t6*
00277         t273*t23+2.0*t36*t37*t26+2.0*t36*t129*t19-4.0*t36*t27*t273*t30+2.0*t36*t37*t33+
00278         12.0*t36*t43*t10+12.0*t36*t37*t46-4.0*t271*t296*t23+2.0*t36*t126*t27;
00279         t317 = t18*t14;
00280         t331 = t14*t2;
00281         t335 = t12*t18;
00282         t339 = t220*t18;
00283         t342 = t7*t30;
00284         t345 = t317*t6;
00285         t350 = -4.0*t31*t40-2.0*t43*t24+2.0*t37*t126*t20-4.0*t44*t24-4.0*t27*t8*
00286         t296*t30-2.0*t253*t317*t30-2.0*t65*t2*t23*t6*t30+2.0*t3*t23*t14*t30-2.0*t12*t19
00287         *t331*t6+2.0*t335*t331*t30-2.0*t201*t339+2.0*t201*t342+2.0*t201*t345+2.0*t86*
00288         t222;
00289         t354 = 1/(t95+t139+t167+t193+t219+t260+t303+t350);
00290         t361 = t22*t4+t29*t8+t296-t23*t4-t30*t8-h[5];
00291         t365 = t253*t18-t3*t23-t335*t2+t201+t339-t342-t345+t202;
00292         t374 = t66-2.0*t243+t49+t90-2.0*t225+t100+t35+t39+t41+t43+t45+t46;
00293 
00294         error = sqrt((t17*t47*t354-t361*t365*t354)*t17+(-t17*t365*t354+t361*t374*
00295         t354)*t361);
00296 
00297         return error;
00298 }       
00299         
00300