Skip to content
Snippets Groups Projects
Commit 23ae66f7 authored by Andrea Censi's avatar Andrea Censi
Browse files

No commit message

No commit message
parent db9df642
No related branches found
No related tags found
No related merge requests found
......@@ -33,7 +33,7 @@ int main(int argc, const char*argv[]) {
params.clusteringThreshold = 0.05;
params.orientationNeighbourhood = 3;
params.doAlphaTest = 0;
params.doAlphaTest = 1;
params.outliers_maxPerc = 0.85;
params.doVisibilityTest = 1;
......
......@@ -179,6 +179,13 @@ val egsl_zeros(size_t rows, size_t columns) {
return v;
}
val egsl_ones(size_t rows, size_t columns) {
val v = egsl_alloc(rows,columns);
gsl_matrix * m = egsl_gslm(v);
gsl_matrix_set_all(m,1.0);
return v;
}
val egsl_copy_val(val v1) {
gsl_matrix * m1 = egsl_gslm(v1);
val v2 = egsl_alloc(m1->size1,m1->size2);
......
......@@ -12,12 +12,15 @@ void egsl_print(const char*str, val);
val egsl_zeros(size_t rows, size_t columns);
val egsl_ones(size_t rows, size_t columns);
val egsl_scale(double, val);
val egsl_sum(val, val);
val egsl_sum3(val, val, val);
val egsl_mult(val, val);
val egsl_transpose(val);
val egsl_inverse(val);
double* egsl_atmp(val v, size_t i, size_t j);
double egsl_norm(val);
double egsl_atv(val, size_t i);
......
#ifndef H_EGSL_MACROS
#define H_EGSL_MACROS
#include "easy_gsl.h"
#define atv(v,i) egsl_atv(v,i)
#define atm(v,i,j) egsl_atm(v,i,j)
......@@ -9,10 +12,12 @@
#define tr(v) egsl_transpose(v)
#define m(v1,v2) egsl_mult(v1,v2)
#define m3(v1,v2,v3) egsl_mult(v1,egsl_mult(v2,v3))
#define m4(v1,v2,v3,v4) egsl_mult(v1,egsl_mult(v2,egsl_mult(v3,v4)))
#define comp_col(v1,v2) egsl_compose_col(v1,v2)
#define comp_row(v1,v2) egsl_compose_row(v1,v2)
#define zeros(rows,cols) egsl_zeros(rows,cols)
#define ones(rows,cols) egsl_ones(rows,cols)
#define vers(th) egsl_vers(th)
#define rot(theta) egsl_rot(theta)
......@@ -22,3 +27,4 @@
#define inv(v) egsl_inverse(v)
#endif
\ No newline at end of file
......@@ -88,8 +88,8 @@ void compute_covariance_exact(
add_to_col(d2J_dxdy1, (size_t)j1, comp_col(d2Jk_dt_drho_j1, d2Jk_dtheta_drho_j1));
// for measurement rho_j2
val d2Jk_dt_drho_j2 = sc(2, m( dC_drho_j2,v2));
val d2Jk_dtheta_drho_j2 = sc(2, m3( tr(v2), dC_drho_j2, v1));
val d2Jk_dt_drho_j2 = sc(2.0, m( dC_drho_j2,v2));
val d2Jk_dtheta_drho_j2 = sc(2.0, m3( tr(v2), dC_drho_j2, v1));
add_to_col(d2J_dxdy1, (size_t)j2, comp_col(d2Jk_dt_drho_j2, d2Jk_dtheta_drho_j2));
egsl_pop();
......
......@@ -51,9 +51,68 @@ void ld_compute_orientation(LDP ld, int size_neighbourhood, double sigma) {
}
}
#include "easy_gsl_macros.h"
void filter_orientation(double theta0, double rho0, size_t n,
const double*thetas, const double*rhos, double *alpha, double*cov0_alpha ) {
egsl_push();
// Y = L x + R epsilon
val Y = zeros(n,1);
val L = ones(n,1);
val R = zeros(n,n+1);
size_t i; for(i=0;i<n;i++) {
*egsl_atmp(Y, i, 0) = (rhos[i]-rho0)/(thetas[i]-theta0);
*egsl_atmp(R, i, 0) = -1/(thetas[i]-theta0);
*egsl_atmp(R, i, i+1) = +1/(thetas[i]-theta0);
}
val eRinv = inv(m(R, tr(R)));
val vcov_f1 = inv(m3(tr(L),eRinv, L));
val vf1 = m4(vcov_f1, tr(L), eRinv, Y);
double cov_f1 = *egsl_atmp(vcov_f1,0,0);
double f1 = *egsl_atmp(vf1,0,0);
*alpha = theta0 - atan(f1/rho0);
if(cos(*alpha)*cos(theta0)+sin(*alpha)*sin(theta0)>0)
*alpha = *alpha + M_PI;
double dalpha_df1 = rho0 / (square(rho0) + square(f1));
double dalpha_drho = -f1 / (square(rho0) + square(f1));
*cov0_alpha = square(dalpha_df1) * cov_f1 + square(dalpha_drho);
egsl_pop();
// printf("dalpha_df1 = %f dalpha_drho = %f\n",dalpha_df1,dalpha_drho);
// printf("f1 = %f covf1 = %f alpha = %f cov_alpha = %f\n ",f1,cov_f1,*alpha,*cov0_alpha);
// printf("sotto = %f\n ",(square(rho0) + square(f1)));
/* printf(" f1 = %f cov =%f \n", f1,cov_f1);
printf(" f1/rho = %f \n", f1/rho0);
printf(" atan = %f \n", atan(f1/rho0));
printf(" theta0= %f \n", theta0);*/
// printf(" alpha = %f sigma= %f°\n", *alpha, rad2deg(0.01*sqrt(*cov0_alpha)));
/*
printf("l= ");
gsl_matrix_fprintf(stdout, l, "%f");
printf("\ny= ");
gsl_matrix_fprintf(stdout, y, "%f");
printf("\nr= ");
gsl_matrix_fprintf(stdout, r, "%f");
printf("\ninv(r*r)= ");
gsl_matrix_fprintf(stdout, Rinv, "%f");
printf("\nf1 = %lf ",f1);
printf("\ncov_f1 = %lf ",cov_f1);
*/
}
#if 0
void filter_orientation(double theta0, double rho0, size_t n,
const double*thetas, const double*rhos, double *alpha, double*cov0_alpha ) {
// y = l x + r epsilon
gsl_matrix * y = gsl_matrix_alloc(n,1);
......@@ -68,7 +127,7 @@ void filter_orientation(double theta0, double rho0, size_t n,
gms(r,i,0, -1/(thetas[i]-theta0) );
gms(r,i,i+1, 1/(thetas[i]-theta0) );
}
int m;
gsls_set(r);
gsls_trans();
......@@ -89,14 +148,16 @@ void filter_orientation(double theta0, double rho0, size_t n,
gsls_mult(y);
double f1 = gsls_get_at(0,0);
/* printf("l= ");
printf("l= ");
gsl_matrix_fprintf(stdout, l, "%f");
printf("\ny= ");
gsl_matrix_fprintf(stdout, y, "%f");
printf("\nr= ");
gsl_matrix_fprintf(stdout, r, "%f");
printf("\ninv(r*r)= ");
gsl_matrix_fprintf(stdout, Rinv, "%f");*/
gsl_matrix_fprintf(stdout, Rinv, "%f");
printf("\nf1 = %lf ",f1);
printf("\ncov_f1 = %lf ",cov_f1);
gsl_matrix_free(Rinv);
gsl_matrix_free(y);
......@@ -113,18 +174,9 @@ void filter_orientation(double theta0, double rho0, size_t n,
double dalpha_drho = -f1 / (square(rho0) + square(f1));
*cov0_alpha = square(dalpha_df1) * cov_f1 + square(dalpha_drho);
// printf("dalpha_df1 = %f dalpha_drho = %f\n",dalpha_df1,dalpha_drho);
// printf("f1 = %f covf1 = %f alpha = %f cov_alpha = %f\n ",f1,cov_f1,*alpha,*cov0_alpha);
// printf("sotto = %f\n ",(square(rho0) + square(f1)));
/* printf(" f1 = %f cov =%f \n", f1,cov_f1);
printf(" f1/rho = %f \n", f1/rho0);
printf(" atan = %f \n", atan(f1/rho0));
printf(" theta0= %f \n", theta0);*/
// printf(" alpha = %f sigma= %f°\n", *alpha, rad2deg(0.01*sqrt(*cov0_alpha)));
}
#endif
// indexes: an array of size "max_num*2"
void find_neighbours(LDP ld, int i, int max_num, int*indexes, size_t*num_found) {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment