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];
}
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);
}
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;
}
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");
}
