gpc.c
Go to the documentation of this file.
00001 /* GPC: A library for the solution of General Point Correspondence problems.
00002   Copyright (C) 2006 Andrea Censi (andrea at censi dot org)
00003 
00004   This program is free software; you can redistribute it and/or
00005   modify it under the terms of the GNU General Public License
00006   as published by the Free Software Foundation; either version 2
00007   of the License, or (at your option) any later version.
00008 
00009   This program is distributed in the hope that it will be useful,
00010   but WITHOUT ANY WARRANTY; without even the implied warranty of
00011   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012   GNU General Public License for more details.
00013 
00014   You should have received a copy of the GNU General Public License
00015   along with this program; if not, write to the Free Software
00016   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00017 */
00018 
00019 #include <math.h>
00020 #include <gsl/gsl_math.h>
00021 #include "gpc.h"
00022 #include "gpc_utils.h"
00023 
00024 /* Note that we use static values here so we don't need to evaluate that all the time */
00025 #define M(matrix, rows, col) static gsl_matrix*matrix=0; if(!matrix) matrix = gsl_matrix_alloc(rows,col);
00026 #define MF(matrix) /*gsl_matrix_free(matrix)*/
00027 
00028 
00029 int gpc_solve(int K, const struct gpc_corr*c,
00030         const double*x0, const double *cov_x0,
00031         double *x_out) 
00032 {
00033         M(bigM,    4,4); M(g,  4,1); M(bigM_k,2,4);
00034         M(bigM_k_t,4,2); M(C_k,2,2); M(q_k,   2,1);
00035         M(temp41,  4,1); M(temp22,2,2); M(temp22b,2,2);
00036         M(temp42,  4,2); M(temp44,4,4); M(temp21, 2,1);
00037         M(temp22c, 2,2); M(temp12,1,2);
00038         
00039         gsl_matrix_set_zero(bigM);
00040         gsl_matrix_set_zero(g);
00041         gsl_matrix_set_zero(temp42);
00042         
00043         double d_bigM[4][4] = { {0,0,0,0}, {0,0,0,0}, {0,0,0,0}, {0,0,0,0}};
00044         double d_g[4] = {0, 0, 0, 0};
00045         int k;
00046         for(k=0;k<K;k++) {
00047                 if(!c[k].valid) continue;
00048                 
00049                 double C00 = c[k].C[0][0];
00050                 double C01 = c[k].C[0][1];
00051                 double C10 = c[k].C[1][0];
00052                 double C11 = c[k].C[1][1];
00053                 
00054                 if(C01 != C10) {
00055                         fprintf(stderr, "k=%d; I expect C to be a symmetric matrix.\n", k);
00056                         return 0;
00057                 }
00058 #if GPC_CHECK_SEMIDEF
00059                 double det = C00*C11 - C01*C10;
00060                 double trace = C00 + C11;
00061                 int is_semidef_pos = (det >= 0) && (trace>0);
00062                 if(!is_semidef_pos) {
00063                         fprintf(stderr, "k=%d; I expect the matrix to be semidef positive (det>=0 and trace>0), det = %.20f trace= %.10f C = [%.15f,%.15f;%.15f %.15f]\n",
00064                                 k, det, trace,  C00,C01,C10,C11);
00065                         return 0;
00066                 }
00067 #endif
00068                 
00069                 double qx = c[k].q[0];
00070                 double qy = c[k].q[1];
00071                 double px = c[k].p[0];
00072                 double py = c[k].p[1];
00073                 
00074                 /* [ C00,  c01,  px C00 + c01 py , c01 px - py C00 ] */
00075                 d_bigM[0][0] += C00;
00076                 d_bigM[0][1] += C01;
00077                 d_bigM[0][2] += +px * C00 + py * C01;
00078                 d_bigM[0][3] += -py * C00 + px * C01;
00079         
00080                 /*  [ C10 ,  C11 , py C11 + px C10 , px C11 - py C10 ] */
00081                 d_bigM[1][0] += C10;
00082                 d_bigM[1][1] += C11;
00083                 d_bigM[1][2] += +px * C10 + py * C11;
00084                 d_bigM[1][3] += +px * C11 - py * C10;
00085                 
00086                 /*Col 1 = [ py C10 + px C00 ] 
00087                  Col 2 = [ py C11 + c01 px ]
00088                  Col 3 = [ py (py C11 + px C10) + px (px C00 + c01 py) ]
00089                  Col 4 = [ py (px C11 - py C10) + px (c01 px - py C00) ]
00090                 */
00091                 d_bigM[2][0] += px * C00 + py * C10;
00092                 d_bigM[2][1] += px * C01 + py * C11;
00093                 d_bigM[2][2] += (px*px)*(+C00) + (px*py)*(+C10+C01) + (py*py)*(+C11);
00094                 d_bigM[2][3] += (px*px)*(+C01) + (px*py)*(-C00+C11) + (py*py)*(-C10);
00095 
00096                 /*Col 1 = [ px C10 - py C00 ] 
00097                   Col 2 = [ px C11 - c01 py ]
00098                  Col 3 = [ px (py C11 + px C10) - py (px C00 + c01 py) ]
00099                  Col 4 = [ px (px C11 - py C10) - py (c01 px - py C00) ]*/
00100                 d_bigM[3][0] += -py * C00 + px * C10;
00101                 d_bigM[3][1] += -py * C01 + px * C11;
00102                 d_bigM[3][2] += (px*px)*(+C10) + (px*py)*(-C00+C11) + (py*py)*(-C01);
00103                 d_bigM[3][3] += (px*px)*(+C11) + (px*py)*(-C10-C01) + (py*py)*(+C00);
00104                 
00105                 d_g[0] += C00*qx+C10*qy;
00106                 d_g[1] += C01*qx+C11*qy;
00107                 d_g[2] += qx * (C00*px+C01*py) + qy * (C10*px+C11*py);
00108                 d_g[3] += qx * (C00*(-py)+C01*px) + qy * (C10*(-py)+C11*px);
00109 
00110         }
00111 
00112         {
00113         unsigned int a,b;
00114          for(a=0;a<4;a++) 
00115                         *gsl_matrix_ptr(g, a, 0) = -2 * d_g[a];
00116          for(a=0;a<4;a++) 
00117                 for(b=0;b<4;b++)
00118                 gsl_matrix_set(bigM, a, b,  2 * d_bigM[a][b]);
00119         }
00120 
00121         
00122         M(mA,2,2); gms(mA,0,0, gmg(bigM,0,0)); gms(mA,0,1, gmg(bigM,0,1));
00123                    gms(mA,1,0, gmg(bigM,1,0)); gms(mA,1,1, gmg(bigM,1,1));
00124         M(mB,2,2); gms(mB,0,0, gmg(bigM,0,2)); gms(mB,0,1, gmg(bigM,0,3));
00125                    gms(mB,1,0, gmg(bigM,1,2)); gms(mB,1,1, gmg(bigM,1,3));
00126         M(mD,2,2); gms(mD,0,0, gmg(bigM,2,2)); gms(mD,0,1, gmg(bigM,2,3));
00127                    gms(mD,1,0, gmg(bigM,3,2)); gms(mD,1,1, gmg(bigM,3,3));
00128 
00129         M(mS,2,2); M(mSa,2,2);
00130 
00131          
00132         /*      mS = mD - mB.trans * mA.inv * mB;
00133           temp22b = inv(A) */
00134         m_inv(mA, temp22b); 
00135         /* temp22c = inv(A) * mB           */
00136         m_mult(temp22b, mB, temp22c);
00137         /* temp22 = mB'               */
00138         m_trans(mB, temp22); 
00139         m_mult(temp22, temp22c, temp22b); 
00140         m_scale(-1.0,temp22b);
00141         m_add(mD,temp22b,mS);
00142         
00143         /* mSa = mS.inv * mS.det; */
00144         m_inv(mS, mSa);
00145         m_scale(m_det(mS), mSa);
00146         
00147         if(TRACE_ALGO) {
00148                 m_display("mA",mA);
00149                 m_display("mB",mB);
00150                 m_display("mD",mD);
00151                 m_display("mS",mS);
00152                 m_display("mSa",mSa);
00153         }
00154 
00155         M(g1,2,1);      M(g2,2,1);
00156         M(g1t,1,2);     M(g2t,1,2);
00157         M(mAi,2,2);     M(mBt,2,2);
00158         
00159         gms(g1,0,0,gmg(g,0,0));
00160         gms(g1,1,0,gmg(g,1,0));
00161         gms(g2,0,0,gmg(g,2,0));
00162         gms(g2,1,0,gmg(g,3,0));
00163         m_trans(g1, g1t);
00164         m_trans(g2, g2t);
00165         m_trans(mB, mBt);
00166         m_inv(mA, mAi);
00167 
00168         M(m1t,1,2);     M(m1,2,1);
00169         M(m2t,1,2);     M(m2,2,1);
00170         M(m3t,1,2);     M(m3,2,1);
00171 
00172         /* m1t = g1t*mAi*mB */
00173         m_mult(g1t,mAi,temp12);
00174         m_mult(temp12,mB,m1t);
00175 
00176         m_trans(m1t,m1);
00177         /*     m2t = m1t*mSa    */
00178         m_mult(m1t,mSa,m2t);
00179         m_trans(m2t,m2);
00180         /* m3t = g2t*mSa     */
00181         m_mult(g2t,mSa,m3t); 
00182         m_trans(m3t,m3);
00183         
00184         double p[3] = {
00185                   m_dot(m2t,m2) - 2*m_dot(m2t,m3) +   m_dot(m3t,m3),
00186                 4*m_dot(m2t,m1) - 8*m_dot(m2t,g2) + 4*m_dot(g2t,m3),
00187                 4*m_dot(m1t,m1) - 8*m_dot(m1t,g2) + 4*m_dot(g2t,g2)};
00188 
00189         double l[3] = {m_det(mS), 2*gmg(mS,0,0)+2*gmg(mS,1,1), 4};
00190         
00191         /* q = p - l^2        */
00192         double q[5] = {p[0]-(l[0]*l[0]), p[1]-(2*l[1]*l[0]), 
00193                 p[2]-(l[1]*l[1]+2*l[0]*l[2]), -(2*l[2]*l[1]), -(l[2]*l[2])};
00194         
00195         if(TRACE_ALGO) {
00196                 fprintf(stderr, "p = %f %f %f \n", p[2], p[1], p[0]);
00197                 fprintf(stderr, "l = %f %f %f \n", l[2], l[1], l[0]);
00198                 fprintf(stderr, "q = %f %f %f %f %f \n", q[4],  q[3],  q[2], q[1], q[0]);
00199         }
00200 
00201         /*
00202         double lambdas[4];
00203         if(!poly_real_roots(5, q, lambdas)) {
00204                 fprintf(stderr, "Cannot solve polynomial.\n");
00205                 return 0;
00206         }
00207 
00208         double lambdas_error[4];
00209         double lambdas_pose[4][3];
00210         
00211         for(int i=0;i<4;i++) {
00212                 double lambda = lambdas[i];
00213                 
00214                 if(TRACE_ALGO) {
00215                         fprintf(stderr, "lambda = %f \n", lambda);
00216                 }       
00217         
00218                 M(W,4,4); gsl_matrix_set_zero(W); gms(W,2,2,1.0); gms(W,3,3,1.0);
00219                 M(x,4,1);
00220         
00221                 m_scale(2*lambda, W);
00222                 gsl_matrix_add(bigM,W);
00223                 m_inv(bigM, temp44);
00224                 m_mult(temp44, g, x);
00225                 m_scale(-1.0, x);
00226         
00227                 lambdas_pose[i][0] = gmg(x,0,0);
00228                 lambdas_pose[i][1] = gmg(x,1,0);
00229                 lambdas_pose[i][2] = atan2(gmg(x,3,0),gmg(x,2,0));
00230                 
00231                 lambdas_error[i] = gpc_total_error(c, K, lambdas_pose[i]);
00232         }
00233         
00234         int lowest_error = 0;
00235         for(int i=0;i<4;i++) {
00236                 printf("#%d lambda=%lf error=%lf\n",i,lambdas[i],lambdas_error[i]);
00237                 if(lambdas_error[i] < lambdas_error[lowest_error])
00238                         lowest_error = i;
00239         }
00240         
00241         double lr;
00242         poly_greatest_real_root(5,q,&lr);
00243         printf("Choose %d: lambda = %lf   bigger real root = %lf \n",lowest_error,lambdas[lowest_error],lr);
00244         
00245         x_out[0]=lambdas_pose[lowest_error][0];
00246         x_out[1]=lambdas_pose[lowest_error][1];
00247         x_out[2]=lambdas_pose[lowest_error][2];
00248         */
00249         
00250         double lambda;
00251         if(!poly_greatest_real_root(5, q, &lambda)) return 0;
00252         
00253         M(W,4,4); gsl_matrix_set_zero(W); gms(W,2,2,1.0); gms(W,3,3,1.0);
00254         M(x,4,1);
00255 
00256         m_scale(2*lambda, W);
00257         gsl_matrix_add(bigM,W);
00258         m_inv(bigM, temp44);
00259         m_mult(temp44, g, x);
00260         m_scale(-1.0, x);
00261 
00262         x_out[0] = gmg(x,0,0);
00263         x_out[1] = gmg(x,1,0);
00264         x_out[2] = atan2(gmg(x,3,0),gmg(x,2,0));
00265 
00266         if(TRACE_ALGO) {
00267                 fprintf(stderr, "x =  %f  %f %f deg\n", x_out[0], x_out[1],x_out[2]*180/M_PI);
00268         }
00269         
00270         MF(mA); MF(mB); MF(mD); MF(mS); MF(mSa);
00271         MF(m1t);        MF(m1); MF(m2t);        MF(m2);
00272         MF(m3t);        MF(m3); MF(W);  MF(x);
00273         MF(bigM); MF(g); MF(bigM_k);
00274         MF(bigM_k_t); MF(C_k); MF(q_k);
00275         MF(temp42); MF(temp44); MF(temp21);
00276         MF(temp41); MF(temp22); MF(temp22b);
00277         MF(temp22c); MF(temp12);
00278         MF(g1); MF(g2);
00279         MF(g1t);        MF(g2t);
00280         MF(mAi);        MF(mBt);
00281         return 1;
00282 }
00283 
00284 
00285 double gpc_error(const struct gpc_corr*co, const double*x) {
00286         double c = cos(x[2]);
00287         double s = sin(x[2]);
00288         double e[2];
00289         e[0] = c*(co->p[0]) -s*(co->p[1]) + x[0] - co->q[0];
00290         e[1] = s*(co->p[0]) +c*(co->p[1]) + x[1] - co->q[1];
00291         double this_error = e[0]*e[0]*co->C[0][0]+2*e[0]*e[1]*co->C[0][1]+e[1]*e[1]*co->C[1][1];
00292         
00293         if(0) /* due to limited numerical precision, error might be negative */
00294         if(this_error < 0) {
00295                 fprintf(stderr, "Something fishy: error = %lf e = [%lf %lf]  C = [%lf,%lf;%lf,%lf]\n", this_error,
00296                         e[0],e[1],co->C[0][0],co->C[0][1],co->C[1][0],co->C[1][1]);
00297         }
00298         return this_error;
00299 }
00300 
00301 double gpc_total_error(const struct gpc_corr*co, int n, const double*x){
00302         double error = 0;
00303         for(int i=0;i<n;i++) {
00304                 if(!co[i].valid) continue;
00305                 error += gpc_error(co+i,x);
00306         }
00307         if(0) /* due to limited numerical precision, error might be negative */
00308         if(error<0) {
00309                 fprintf(stderr, "Something fishy!\n");
00310         }
00311         return error;
00312 }
00313         
00314 /*
00315                [       C00       ]         [       c01       ]
00316                 [                 ]         [                 ]
00317                 [       C10       ]         [       C11       ]
00318 (%o10)  Col 1 = [                 ] Col 2 = [                 ]
00319                 [ py C10 + px C00 ]         [ py C11 + c01 px ]
00320                 [                 ]         [                 ]
00321                 [ px C10 - py C00 ]         [ px C11 - c01 py ]
00322          [              px C00 + c01 py              ]
00323          [                                           ]
00324          [              py C11 + px C10              ]
00325          [                                           ]
00326  Col 3 = [   2                     2                 ]
00327          [ py  C11 + px py C10 + px  C00 + c01 px py ]
00328          [                                           ]
00329          [               2                         2 ]
00330          [ px py C11 + px  C10 - px py C00 - c01 py  ]
00331          [              c01 px - py C00              ]
00332          [                                           ]
00333          [              px C11 - py C10              ]
00334          [                                           ]
00335  Col 4 = [               2                         2 ]
00336          [ px py C11 - py  C10 - px py C00 + c01 px  ]
00337          [                                           ]
00338          [   2                     2                 ]
00339          [ px  C11 - px py C10 + py  C00 - c01 px py ]*/
00340 
00341 


csm_ros
Author(s): Gaƫl Ecorchard , Ivan Dryanovski, William Morris, Andrea Censi
autogenerated on Sat Jun 8 2019 19:52:42