icp_covariance.c
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <gsl/gsl_math.h>
00003 #include <egsl/egsl_macros.h>
00004 
00005 #include "icp.h"
00006 #include "../csm_all.h"
00007 
00008 
00009 val compute_C_k(val p_j1, val p_j2);
00010 val dC_drho(val p1, val p2);
00011 
00012 
00013 void compute_covariance_exact(
00014         LDP laser_ref, LDP laser_sens, const gsl_vector*x,
00015                 val *cov0_x, val *dx_dy1, val *dx_dy2)
00016 {
00017         egsl_push_named("compute_covariance_exact");
00018         
00019         val d2J_dxdy1 = zeros(3,(size_t)laser_ref ->nrays);
00020         val d2J_dxdy2 = zeros(3,(size_t)laser_sens->nrays);
00021         
00022         /* the three pieces of d2J_dx2 */
00023         val d2J_dt2       = zeros(2,2);
00024         val d2J_dt_dtheta = zeros(2,1);
00025         val d2J_dtheta2   = zeros(1,1);
00026         
00027         double theta = x->data[2];
00028         val t = egsl_vFa(2,x->data);
00029         
00030         int i; 
00031         for(i=0;i<laser_sens->nrays;i++) {
00032                 if(!ld_valid_corr(laser_sens,i)) continue;
00033                 egsl_push_named("compute_covariance_exact iteration");
00034 
00035                 int j1 = laser_sens->corr[i].j1;
00036                 int j2 = laser_sens->corr[i].j2;
00037 
00038                 val p_i  = egsl_vFa(2, laser_sens->points[i].p);
00039                 val p_j1 = egsl_vFa(2, laser_ref ->points[j1].p);
00040                 val p_j2 = egsl_vFa(2, laser_ref ->points[j2].p);
00041                 
00042                 /* v1 := rot(theta+M_PI/2)*p_i */
00043                 val v1 = m(rot(theta+M_PI/2), p_i);             
00044                 /* v2 := (rot(theta)*p_i+t-p_j1) */
00045                 val v2 = sum3( m(rot(theta),p_i), t, minus(p_j1));
00046                 /* v3 := rot(theta)*v_i */
00047                 val v3 = vers(theta + laser_sens->theta[i]);
00048                 /* v4 := rot(theta+PI/2)*v_i; */
00049                 val v4 = vers(theta + laser_sens->theta[i] + M_PI/2);
00050                 
00051                 val C_k = compute_C_k(p_j1, p_j2);
00052                 
00053                 val d2J_dt2_k = sc(2.0, C_k);
00054                 val d2J_dt_dtheta_k = sc(2.0,m(C_k,v1));
00055                 
00056                 val v_new = m(rot(theta+M_PI), p_i);
00057                 val d2J_dtheta2_k = sc(2.0, sum( m3(tr(v2),C_k, v_new), m3(tr(v1),C_k,v1)));
00058                 add_to(d2J_dt2, d2J_dt2_k);
00059                 add_to(d2J_dt_dtheta, d2J_dt_dtheta_k ); 
00060                 add_to(d2J_dtheta2, d2J_dtheta2_k);
00061                 
00062                 /* for measurement rho_i  in the second scan */
00063                 val d2Jk_dtdrho_i = sc(2.0, m(C_k,v3)); 
00064                 val d2Jk_dtheta_drho_i = sc(2.0, sum( m3(tr(v2),C_k,v4),  m3(tr(v3),C_k,v1)));
00065                 add_to_col(d2J_dxdy2, (size_t)i, comp_col(d2Jk_dtdrho_i, d2Jk_dtheta_drho_i));
00066                 
00067                 /* for measurements rho_j1, rho_j2 in the first scan */
00068                 
00069                 val dC_drho_j1 = dC_drho(p_j1, p_j2);
00070                 val dC_drho_j2 = dC_drho(p_j2, p_j1);
00071         
00072                 
00073                 val v_j1 = vers(laser_ref->theta[j1]);
00074                 
00075                 val d2Jk_dt_drho_j1 = sum(sc(-2.0,m(C_k,v_j1)), sc(2.0,m(dC_drho_j1,v2)));
00076                 val d2Jk_dtheta_drho_j1 = sum( sc(-2.0, m3(tr(v_j1),C_k,v1)), m3(tr(v2),dC_drho_j1,v1));
00077                 add_to_col(d2J_dxdy1, (size_t)j1, comp_col(d2Jk_dt_drho_j1, d2Jk_dtheta_drho_j1));
00078                 
00079                 /* for measurement rho_j2*/
00080                 val d2Jk_dt_drho_j2 = sc(2.0, m( dC_drho_j2,v2));
00081                 val d2Jk_dtheta_drho_j2 = sc(2.0, m3( tr(v2), dC_drho_j2, v1));
00082                 add_to_col(d2J_dxdy1, (size_t)j2, comp_col(d2Jk_dt_drho_j2, d2Jk_dtheta_drho_j2));
00083 
00084                 egsl_pop_named("compute_covariance_exact iteration");
00085         }
00086 
00087         /* composes matrix  d2J_dx2  from the pieces*/
00088         val d2J_dx2   = comp_col( comp_row(    d2J_dt2      ,   d2J_dt_dtheta),
00089                                   comp_row(tr(d2J_dt_dtheta),     d2J_dtheta2));
00090         
00091         val edx_dy1 = sc(-1.0, m(inv(d2J_dx2), d2J_dxdy1));
00092         val edx_dy2 = sc(-1.0, m(inv(d2J_dx2), d2J_dxdy2));
00093 
00094         val ecov0_x = sum(m(edx_dy1,tr(edx_dy1)),m(edx_dy2,tr(edx_dy2)) );
00095 
00096         /* With the egsl_promote we save the matrix in the previous
00097            context */
00098         *cov0_x = egsl_promote(ecov0_x);
00099         *dx_dy1 = egsl_promote(edx_dy1);
00100         *dx_dy2 = egsl_promote(edx_dy2);
00101         
00102         egsl_pop_named("compute_covariance_exact");     
00103         /* now edx_dy1 is not valid anymore, but *dx_dy1 is. */
00104 }
00105 
00106 
00107 val compute_C_k(val p_j1, val p_j2)  {  
00108         val d = sub(p_j1, p_j2);
00109         double alpha = M_PI/2 + atan2( atv(d,1), atv(d,0));
00110         double c = cos(alpha); double s = sin(alpha);
00111         double m[2*2] = {
00112                 c*c, c*s,
00113                 c*s, s*s
00114         };
00115         return egsl_vFda(2,2,m);
00116 }
00117 
00118 
00119 val dC_drho(val p1, val p2) {
00120         double eps = 0.001;
00121 
00122         val C_k = compute_C_k(p1, p2);  
00123         val p1b = sum(p1, sc(eps/egsl_norm(p1),p1));
00124         val C_k_eps = compute_C_k(p1b,p2);
00125         return sc(1/eps, sub(C_k_eps,C_k));
00126 }
00127 
00128 


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29