Пример #1
0
void compute_ctm( struct pt *points, int cnt, int width, int height,  float *ctm_return )
{
    double xy[ 4 * cnt ];
    double ctm[9];
    normalize_points(points,cnt,width,height,xy);
    ctm_fitting(xy,cnt,ctm);
    for(int i=0;i<9;i++) ctm_return[i] = ctm[i];
}
Пример #2
0
void stitcher::compute_H(const vector<Vec3d>& chosen_p_points, const vector<Vec3d>& chosen_p_prime_points, Mat& dst_mat)
{   
   unsigned int curr_vec_pos = 0;
   double curr_x_prime = 0.0;
   double curr_y_prime = 0.0;
   SVD svd;

   num_points = chosen_p_points.size();
   num_points_div_2 = num_points / 2.0;
   
   p_raw_points.resize( num_points );
   p_prime_raw_points.resize( num_points );
   
   A_matrix.create( num_points * 2, 9, CV_64FC1 );

   for(unsigned int curr_point = 0; curr_point < num_points; curr_point++ )
   {
      p_raw_points[ curr_point ] = chosen_p_points[ curr_point ];
      p_prime_raw_points[ curr_point ] = chosen_p_prime_points[ curr_point ];
   }

   //Normalize points to make algorithm numerically stable
   normalize_points();

   //Fill the A matrix with each corresponding pair of points. 
   for( unsigned int curr_pair_points = 0, curr_mat_row = 0; curr_pair_points < num_points; curr_pair_points++, curr_mat_row += 2 )
   {
      curr_x_prime = p_prime_normalized_points[ curr_pair_points ][0];
      curr_y_prime = p_prime_normalized_points[ curr_pair_points ][1];
      
      A_matrix.at<double>( curr_mat_row, 0 ) = 0;
      A_matrix.at<double>( curr_mat_row, 1 ) = 0;
      A_matrix.at<double>( curr_mat_row, 2 ) = 0;
      
      A_matrix.at<double>( curr_mat_row, 3 ) = -p_normalized_points[ curr_pair_points ][0];
      A_matrix.at<double>( curr_mat_row, 4 ) = -p_normalized_points[ curr_pair_points ][1];
      A_matrix.at<double>( curr_mat_row, 5 ) = -1;
      
      A_matrix.at<double>( curr_mat_row, 6 ) =  p_normalized_points[ curr_pair_points ][0] * curr_y_prime;
      A_matrix.at<double>( curr_mat_row, 7 ) =  p_normalized_points[ curr_pair_points ][1] * curr_y_prime;
      A_matrix.at<double>( curr_mat_row, 8 ) =  curr_y_prime;   
      
      A_matrix.at<double>( curr_mat_row + 1, 0 ) = p_normalized_points[ curr_pair_points ][0];
      A_matrix.at<double>( curr_mat_row + 1, 1 ) = p_normalized_points[ curr_pair_points ][1];
      A_matrix.at<double>( curr_mat_row + 1, 2 ) = 1;
      
      A_matrix.at<double>( curr_mat_row + 1, 3 ) = 0;
      A_matrix.at<double>( curr_mat_row + 1, 4 ) = 0;
      A_matrix.at<double>( curr_mat_row + 1, 5 ) = 0;
      
      A_matrix.at<double>( curr_mat_row + 1, 6 ) =  p_normalized_points[ curr_pair_points ][0] * -curr_x_prime;
      A_matrix.at<double>( curr_mat_row + 1, 7 ) =  p_normalized_points[ curr_pair_points ][1] * -curr_x_prime;
      A_matrix.at<double>( curr_mat_row + 1, 8 ) =  -curr_x_prime;                                                
   }

   //Perform the SVD on the A_matrix, use full flag to get null space vector
   svd(A_matrix, SVD::FULL_UV);

   //Turn the null space vector into the 3x3 matrix H matrix
   for( unsigned int curr_row = 0; curr_row < 3; curr_row++ )
   {
      for( unsigned int curr_col = 0; curr_col < 3; curr_col++ )
      {
         //Divide through by last element to get proper scale
         H_matrix.at<double>( curr_row, curr_col ) = svd.vt.at<double>( 8, curr_vec_pos ) / svd.vt.at<double>( 8, 8 );
         curr_vec_pos++;
      }
   }
   
   //Unnormalize the H matrix so it will be correct in terms of original coordinates
   unnormalize_points(); 

   H_matrix.copyTo(dst_mat);
}
Пример #3
0
int main(int argc, char **argv)
{
    char *str_alpha_p = argv[1], *str_alpha_q = argv[2];
    char *cdf = argv[3];

    unsigned nprior = atoi(argv[4]);
    unsigned npost = atoi(argv[5]);
    unsigned ngold = atoi(argv[6]);
    /* double step_mult = atof(argv[7]); */

    gsl_set_error_handler_off();

    FILE *cdf_fh = fopen(cdf, "w");

    double alpha_p[4], alpha_q[4];
    sscanf(str_alpha_p, "%lf,%lf,%lf,%lf", &alpha_p[0], &alpha_p[1], &alpha_p[2], &alpha_p[3]);
    sscanf(str_alpha_q, "%lf,%lf,%lf,%lf", &alpha_q[0], &alpha_q[1], &alpha_q[2], &alpha_q[3]);

    double alpha_combined[] = { 
        alpha_p[0] + alpha_q[0] - 1,
        alpha_p[1] + alpha_q[1] - 1,
        alpha_p[2] + alpha_q[2] - 1,
        alpha_p[3] + alpha_q[3] - 1
    };

    unsigned ntotal = nprior + npost + ngold;
    struct wpoint *buf = (struct wpoint *)malloc(ntotal * sizeof(struct wpoint));
    struct wpoint *p_points = buf, *q_points = buf + nprior, *g_points = q_points + npost;
    struct wpoint **tmp_points = (struct wpoint **)malloc((nprior + npost) * sizeof(struct wpoint *));
    struct wpoint **int_smooth_points = (struct wpoint **)malloc(npost * sizeof(struct wpoint *));
    struct wpoint **ext_points = (struct wpoint **)malloc(nprior * sizeof(struct wpoint *));
    struct wpoint **ext_smooth_points = (struct wpoint **)malloc(nprior * sizeof(struct wpoint *));
    struct wpoint **ext_rough_points = (struct wpoint **)malloc(nprior * sizeof(struct wpoint *));
    struct wpoint **all_smooth_points = (struct wpoint **)malloc((nprior + npost) * sizeof(struct wpoint *));

    struct wpoint **pg = (struct wpoint **)malloc(ngold * sizeof(struct wpoint *));

    struct wpoint **pp, **pp2, **pe;
    struct wpoint *s, *p_end = p_points + nprior, *q_end = q_points + npost;

    gsl_rng *rand = gsl_rng_alloc(gsl_rng_taus);

    // populate P(*)
    double p_norm;
    populate_points(p_points, nprior, alpha_p, alpha_q, rand, "prior", &p_norm);

    // populate Q(*)
    double q_norm;
    populate_points(q_points, npost, alpha_q, alpha_p, rand, "post", &q_norm);

    int i;
    for (i = 0; i != 10; ++i)
    {
        populate_points(q_points, npost, alpha_q, alpha_p, rand, "post", &q_norm);
        fprintf(stderr, "q_norm = %g\n", q_norm);
    }        


    /* populate gold(*) */
    double g_norm;
    double alpha_uniform[] = { 1, 1, 1, 1 };
    populate_points(g_points, ngold, alpha_combined, alpha_uniform, rand, "gold", &g_norm);

    /* normalize */
    normalize_points(p_points, nprior, p_norm, q_norm);
    normalize_points(q_points, npost, q_norm, p_norm);
    normalize_points(g_points, ngold, g_norm, g_norm);

    unsigned n;
    
    /* gather all Q(*) points (use int_smooth_points temporarily) */
    for (s = q_points, pp = int_smooth_points; s != q_end; ++s)
        *pp++ = s;
    n = pp - int_smooth_points;

    /* collect Q(*) smooth (interior) points */
    // double ln_p_bound = smooth_threshold(int_smooth_points, step_mult, &n);
    /* for (s = q_points, pp = int_smooth_points; s != q_end; ++s) */
    /*     if (s->ln_o <= ln_p_bound) */
    /*         *pp++ = s; */
    for (s = q_points, pp = int_smooth_points; s != q_end; ++s)
        if (s->ln_d > s->ln_o)
            *pp++ = s;
    unsigned n_int_smooth_q = pp - int_smooth_points;

    /* double ma_ratio = max_to_avg_ratio(int_smooth_points,); */
    double int_mass_on_q = sum_of_weights(int_smooth_points, n_int_smooth_q);

    /* collect exterior P(*) points */
    /* for (s = p_points, pp = ext_points; s != p_end; ++s) */
    /*     if (s->ln_d > ln_p_bound) */
    /*         *pp++ = s; */
    for (s = p_points, pp = ext_smooth_points; s != p_end; ++s)
        if (s->ln_d > s->ln_o)
            *pp++ = s;
    /* unsigned n_ext_p = n = pp - ext_points; */
    /* double ln_q_bound = smooth_threshold(ext_points, step_mult, &n); */

    /* collect exterior P(*) smooth points */
    /* for (s = p_points, pp = ext_smooth_points; s != p_end; ++s) */
    /*     if (s->ln_d > ln_p_bound && s->ln_o < ln_q_bound) */
    /*         *pp++ = s; */
    unsigned n_ext_smooth_p = pp - ext_smooth_points;

    
    /* double ma2_ratio = max_to_avg_ratio(ext_smooth_points, n_ext_smooth_p); */
    double ext_mass_on_p = sum_of_weights(ext_smooth_points, n_ext_smooth_p);
    double total_mixed_mass = int_mass_on_q + ext_mass_on_p;
    /* fprintf(stderr, "Number of points unaccounted for: %u\n", n_ext_p - n_ext_smooth_p); */

    // print out numerical CDFs
    char hdr[] = "Dist\tBase\tComp\tPointIndex\tCDF\tln_d\tln_o\tBaseDist\n";
    fprintf(cdf_fh, hdr);

    print_cdf(int_smooth_points, n_int_smooth_q, total_mixed_mass, buf, "int_smooth_q", cdf_fh);
    print_cdf(ext_smooth_points, n_ext_smooth_p, total_mixed_mass, buf, "ext_smooth_p", cdf_fh);

   
    pe = int_smooth_points + n_int_smooth_q;
    for (pp = int_smooth_points, pp2 = all_smooth_points; pp != pe; ++pp, ++pp2)
        *pp2 = *pp;

    pe = ext_smooth_points + n_ext_smooth_p;
    for (pp = ext_smooth_points; pp != pe; ++pp, ++pp2)
        *pp2 = *pp;

    unsigned n_smooth = n_int_smooth_q + n_ext_smooth_p;
    print_cdf(all_smooth_points, n_smooth, total_mixed_mass, p_points, "smooth_combined", cdf_fh);

    /* all points in the P(*) sampling */
    for (s = p_points, pp = tmp_points; s != p_end; ++s)
        *pp++ = s;
    print_cdf(tmp_points, nprior, total_mixed_mass, p_points, "all_p", cdf_fh);

    /* all points in the Q(*) sampling */
    for (s = q_points, pp = tmp_points; s != q_end; ++s)
        *pp++ = s;

    print_cdf(tmp_points, npost, total_mixed_mass, q_points, "all_q", cdf_fh);


    /* print out the gold standard */
    struct wpoint *gold_end = g_points + ngold;
    for (s = g_points, pp = pg; s != gold_end; ++s)
        *pp++ = s;
    
    double gold_mass = sum_of_weights(pg, ngold);
    print_cdf(pg, ngold, gold_mass, g_points, "dgold", cdf_fh);

    /* compute component-wise marginal estimates. */
    /* double q[] = { 0.0001, 0.001, 0.01, 0.1, 0.5, 0.9, 0.99, 0.999, 0.9999 }; */
    /* double qv[NUCS][sizeof(q) / sizeof(double)]; */
    
    // print out marginal estimates
    /* unsigned iq; */
    /* for (dim = 0; dim != NUCS; ++dim) */
    /* { */
        /* for (iq = 0; iq != sizeof(qv[dim]) / sizeof(double); ++iq) */
        /*     printf(iq ? "\t%f" : "%f", qv[dim][iq]); */
        /* printf("\n"); */
    /* } */

    free(buf);
    free(tmp_points);
    free(int_smooth_points);
    free(ext_points);
    free(ext_smooth_points);
    free(ext_rough_points);
    free(all_smooth_points);
    free(pg);
    gsl_rng_free(rand);

    fclose(cdf_fh);

    return 0;
}
Пример #4
0
void build_sir_policy(double *policy, size_t policy_size,
                      double *bounds, size_t ndims,
		      size_t strategy,
                      gp_posterior_mean_fn_t gp_posterior_mean_fn,
		      acq_maximizer_fn_t acq_maximizer_fn,
		      gp_maximizer_local_fn_t gp_maximizer_local_fn,
		      sample_gp_posterior_mean_fn_t sample_gp_posterior_mean_fn)
{
    // strategy: 	0 Nim's Resampling
    //			1 l-bfgs-b + M_resampling
    //			2 threshholding
    //			3 sampling + direct
    //			4 sampling + multiple l-bfgs-b
    
    printf("build_sir_policy(): START\n");
    
    if (strategy > 0) {
	const size_t n_amcmc_dims = 8;
	double *amcmc_params;
	double *amcmc_params_max;
	size_t counter = 0;
	amcmc_params = malloc(sizeof(double) * (n_amcmc_dims+1));
	
	size_t num_loops = policy_size;
	double max_val = -100.0;
	
	if (strategy == 1) {
	    num_loops = 100;
	    amcmc_params_max = malloc(sizeof(double) * (n_amcmc_dims));
	} else if (strategy == 2) {
	    num_loops = 2;
	    amcmc_params_max = malloc(sizeof(double) * (n_amcmc_dims));
	}
	
	while (counter < num_loops) {
	    gp_maximizer_local_fn(bounds, amcmc_params, n_amcmc_dims);
	    
	    if (strategy == 1 || strategy == 2) {
		if (amcmc_params[n_amcmc_dims] > max_val) {
		    max_val = amcmc_params[n_amcmc_dims];
		    for (size_t k = 0 ; k < n_amcmc_dims ; k++) {
			amcmc_params_max[k] = amcmc_params[k];
		    }
		}
		counter = counter + 1;
	    } else if (strategy == 3) {
		int constraints_violated = check_violated_sard_constraints(amcmc_params, n_amcmc_dims);
		if (!constraints_violated)
		{
		    amcmc_params[1] = round(amcmc_params[1]);
		    amcmc_params[2] = round(amcmc_params[2]);
		    
		    for (size_t d = 0; d < ndims; ++d)
		    {
			policy[counter*ndims+d] = amcmc_params[d];
		    }
		    counter = counter + 1;
		} else {
		    printf("Constraint Violated!\n");
		}    
	    }
	}
	
	if (strategy == 1 || strategy == 2) {
	    sample_gp_posterior_mean_fn(amcmc_params_max, n_amcmc_dims, bounds, policy, policy_size);
	    free(amcmc_params_max);
	}
    } else {
    
	// Do not put any sets of parameters that violate constraints into the
	// policy. It just makes things more complicated.

	// Compute number of points per dimension. This will probably result in way
	// too many points, so it would probably be a good idea to construct the
	// points in a more intelligent way.

	printf("1\n");
	size_t *npoints_per_dim = malloc(sizeof(size_t) * ndims);
	{
	    for (size_t i = 0; i < 3; ++i)
	    {
		npoints_per_dim[i] = (size_t)(bounds[2*i+1] - bounds[2*i] + 1);
	    }

	    for (size_t i = 3; i < ndims; ++i)
	    {
		npoints_per_dim[i] = 6;
	    }
	}

	printf("2\n");
	// Make grid of points deterministically.
	size_t n_points = 30000;
	double *grid_points =
	    make_uniform_grid_points_nd(bounds, npoints_per_dim, ndims, n_points);

	printf("3\n");
	// Compute w(x) = f(\mu(x)) for each point. Assign w(x) = 0 to the points
	// that violate constraints.
	double *w = malloc(sizeof(double) * n_points);

	printf("4\n");

	printf("computing gp posterior for %lu points\n", n_points);
	size_t n_violated_points = 0, n_valid_points = 0;
	for (size_t i = 0; i < n_points; ++i)
	{
	    double *curr_point = grid_points+(ndims*i);

	    if (check_violated_sard_constraints(curr_point, ndims))
	    {
		w[i] = 0.0;
		++n_violated_points;
		continue;
	    }

	    ++n_valid_points;

	    w[i] = gp_posterior_mean_fn(curr_point, ndims);
	    w[i] = exp(10*w[i]);

	    if (i % 1000 == 0)
	    {
		printf("n_violated = %lu, n_valid = %lu, total = %lu\n",
		    n_violated_points, n_valid_points,
		    n_violated_points + n_valid_points);
	    }
	}

	// Normalize weights.
	printf("5\n");
	normalize_points(w, n_points);

	printf("6\n");
	// Resample points according to weights to construct policy.
	size_t *policy_idx = malloc(sizeof(size_t) * policy_size);
	kitagawa_resampling(w, n_points, policy_idx, policy_size);
	for (size_t i = 0; i < policy_size; ++i)
	{
	    size_t curr_point_idx = ndims * i;
	    size_t curr_policy_point_idx = ndims * policy_idx[i];

	    for (size_t d = 0; d < ndims; ++d)
	    {
		policy[curr_point_idx+d] = grid_points[curr_policy_point_idx+d];
	    }

	    // Normalize mixture weights P_LL, P_LH, and P_HL.
	    normalize_points(policy+curr_point_idx+3, 3);
	}

	printf("7\n");
	// Free memory that needs to be freed.
	free(policy_idx);
	free(w);
	free(grid_points);
	free(npoints_per_dim);
    }
    printf("build_sir_policy(): END\n");
}