home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
vis-ftp.cs.umass.edu
/
vis-ftp.cs.umass.edu.tar
/
vis-ftp.cs.umass.edu
/
pub
/
Software
/
ASCENDER
/
ascendMar8.tar
/
UMass
/
Triangulate
/
tri.c
< prev
next >
Wrap
C/C++ Source or Header
|
1996-02-27
|
18KB
|
823 lines
#include <stdio.h>
#include "nrutil.h"
#include <math.h>
#include "rh_util.h"
#include "marquardt.h"
#include "fbip.h"
/*
#define old_version 1
*/
#define print_rms 1
/*
#define print_init 1
#define state_output 1
#define point_output 1
#define dist_output 1
#define hesse_output 1
#define full_debug 1
*/
#define EPSILON 1.0e-4
#define MAX_ANGLE_STEP 0.2
#define MAX_HEIGTH_STEP 50
#define MAX_INITIAL_RMS 1.0e6
/* global data structures */
int **index_array;
double ***image_2d_lines;
double ***pose_matrices;
int num_3d_lines;
double **initial_3d_lines;
double *curr_sol;
double *initial_sol;
double old_phi;
double old_height;
FBIP *fbips;
int line_orient;
void project_to_two_d(double **matrix, double *from, double *into)
{
int i,j;
for (i = 0; i < 4; i++) {
into[i] = 0.0;
for (j = 0; j < 4; j++)
into[i] += matrix[i+1][j+1] * from[j];
};
for (i = 0; i < 3; i++)
into[i] /= into[3];
};
double compute_rms(double vector[], double goal_vector[], int n)
{
int i;
double rms = 0.0;
for (i = 1; i <= n; i++)
rms += ((vector[i] - goal_vector[i]) * (vector[i] - goal_vector[i]));
return(rms);
};
void ComputeDistance(int ln_no_3d, double *mparam,
int view_no,
double *image_line, double *result)
/* returns the sum of the
distance of the endpoints of the 2d line to the
2d line which is the result of the projection of the 3d line */
{
int i,j;
double pt_1[5];
double pt_2[5];
double hd;
double hvectord[7];
double a,b,c;
COORDINATE worldv[4];
COORDINATE viewv[4];
COORDINATE *viewv_ptr;
i = ln_no_3d;
j = ln_no_3d + 1;
if (j > num_3d_lines)
j = 1;
c = (cos(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2) *
sin(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2)) -
(cos(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2) *
sin(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2));
if (c == 0.0) {
fprintf(stdout,"error intersecting the lines \n");
}
else
{
a = (sin(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2) *
mparam[j+2]) -
(sin(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2) *
mparam[i+2]);
b = (mparam[i+2] *
cos(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2)) -
(mparam[j+2] *
cos(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2));
pt_1[1] = a/c;
pt_1[2] = b/c;
pt_1[3] = mparam[2];
pt_1[4] = 1.0;
};
j = ln_no_3d - 1;
if (j < 1)
j = num_3d_lines;
c = (cos(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2) *
sin(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2)) -
(cos(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2) *
sin(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2));
if (c == 0.0) {
fprintf(stdout,"error intersecting the lines \n");
}
else
{
a = (sin(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2) *
mparam[j+2]) -
(sin(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2) *
mparam[i+2]);
b = (mparam[i+2] *
cos(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2)) -
(mparam[j+2] *
cos(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2));
pt_2[1] = a/c;
pt_2[2] = b/c;
pt_2[3] = mparam[2];
pt_2[4] = 1.0;
};
#ifdef point_output
fprintf(stdout,"3d point 1: %lf %lf %lf \n",pt_1[1],pt_1[2],pt_1[3]);
fprintf(stdout,"3d point 2: %lf %lf %lf \n",pt_2[1],pt_2[2],pt_2[3]);
#endif
/* project into 2d */
worldv[0] = pt_1[1];
worldv[1] = pt_1[2];
worldv[2] = pt_1[3];
worldv[3] = 1.0;
if (pose_matrices[view_no] != 0)
project_to_two_d(pose_matrices[view_no],worldv,viewv);
if (fbips[view_no] != 0)
viewv_ptr = fbip_project_to_view(fbips[view_no], worldv, viewv);
hvectord[1] = viewv[0];
hvectord[2] = viewv[1];
worldv[0] = pt_2[1];
worldv[1] = pt_2[2];
worldv[2] = pt_2[3];
worldv[3] = 1.0;
if (pose_matrices[view_no] != 0)
project_to_two_d(pose_matrices[view_no],worldv,viewv);
if (fbips[view_no] != 0)
viewv_ptr = fbip_project_to_view(fbips[view_no], worldv, viewv);
hvectord[3] = viewv[0];
hvectord[4] = viewv[1];
/* the 2d points are now in hvectord[1..2] and hvectord[3..4] */
#ifdef point_output
fprintf(stdout,"2d points %d %lf %lf %lf %lf \n",view_no,hvectord[1],
hvectord[2],
hvectord[3],
hvectord[4]);
fscanf(stdin,"%*c");
#endif
/* compute the distance */
hvectord[1] = hvectord[1] * cos(image_line[6])
+ hvectord[2] * sin(image_line[6])
- image_line[7];
hvectord[2] = hvectord[3] * cos(image_line[6])
+ hvectord[4] * sin(image_line[6])
- image_line[7];
*result = (hvectord[1]*hvectord[1]) + (hvectord[2]*hvectord[2]);
#ifdef hesse_output
fprintf(stdout,"hesse image line: %lf %lf %lf \n",image_line[1],
image_line[6],image_line[7]);
#endif
#ifdef dist_output
fprintf(stdout,"distance %lf: \n",*result);
#endif
};
/* the function which we try to minimize the parameters for */
void function(double a[], double y[], int nrows, int ncols) {
int i;
int image_no;
int view_no;
int line_no;
double phi;
double h;
#ifdef full_debug
fprintf(stdout,"in function \n");
#endif
for (i = 1; i <= nrows; i++) {
image_no = index_array[i][1];
view_no = index_array[i][2];
line_no = index_array[i][3];
#ifdef full_debug
fprintf(stdout,"%d %d %d\n",image_no,view_no,line_no);
#endif
ComputeDistance((int)(image_2d_lines[image_no][line_no][1]),
a,
view_no,
image_2d_lines[image_no][line_no],
&h);
y[i] = h;
#ifdef debug
fprintf(stdout,"%lf \n",y[i]);
#endif
};
#ifdef full_debug
for (i = 1; i <= nrows; i++)
fprintf(stdout,"%lf ",y[i]);
fprintf(stdout,"\n");
#endif
};
void new_position_action(double *mparam, int ncols) {
/* gets called when parameters are changed */
int i;
/* check if the quadrants have changed */
if ((old_phi > M_PI_2) && (mparam[1] <= M_PI_2)
|| (old_phi <= M_PI_2) && (mparam[1] > M_PI_2)
|| (old_phi > 0.0) && (mparam[1] <= 0.0)
|| (old_phi <= 0.0) && (mparam[1] > 0.0)) {
#ifdef state_output
fprintf(stdout,"wraparound \n");
#endif
};
if (fabs(old_phi - curr_sol[1]) > MAX_ANGLE_STEP)
curr_sol[1] = old_phi;
if (fabs(old_height - curr_sol[2]) > MAX_HEIGTH_STEP)
curr_sol[2] = old_height;
old_phi = curr_sol[1];
old_height = curr_sol[2];
#ifdef state_output
for (i = 1; i <=ncols; i++)
fprintf(stdout,"%lf ",mparam[i]);
fprintf(stdout,"\n");
#endif
}
/* function to triangulate, takes three arguments */
void Triangulation(char * polyfile, char * outfile)
{
int num_image_lines;
int num_horizontal_lines;
int num_constraints;
int num_images;
int num_inits;
int i,j,k;
int h;
FILE *poly, *out;
int *num_image_2d_lines;
double *init_phi;
double hvectord[7];
double dummyd;
int dummyi;
char dummyc;
char hstr[255];
double hd;
double hd1;
double hd2;
double xm, ym;
double start_phi;
double a,b,c,sq;
int num_2d_lines;
int view_no;
int *view_numbers;
double *lm_y;
double *dist_vec;
double old_rms, new_rms;
Marquardt_info minfo;
fprintf(stdout,"Running triangulator \n");
/* start to read in the values */
poly = fopen(polyfile,"r");
if (poly == 0) {
fprintf(stdout,"file [%s] not existing, exiting \n",polyfile);
return;
}
out = fopen(outfile,"w");
/* read the initial guess */
fscanf(poly,"%d",&num_3d_lines);
if (num_3d_lines < 2) {
fprintf(stdout,"not enough lines \n");
close(poly);
close(out);
return;
}
/* first intermediate result = initial guess */
init_phi = dvector(1,num_3d_lines);
curr_sol = dvector(1,num_3d_lines+2);
initial_sol = dvector(1,num_3d_lines+2);
initial_3d_lines = dmatrix(1,num_3d_lines,1,6);
#ifdef full_debug
fprintf(stdout,"before init guess read \n");
#endif
for(i=1; i <= num_3d_lines; i++) {
for(j=1; j <= 6; j++) {
fscanf(poly,"%lf",&(initial_3d_lines[i][j]));
};
a = initial_3d_lines[i][2] - initial_3d_lines[i][5];
b = initial_3d_lines[i][4] - initial_3d_lines[i][1];
c = initial_3d_lines[i][1] * initial_3d_lines[i][5] -
initial_3d_lines[i][4] * initial_3d_lines[i][2];
sq = sqrt(a*a + b*b);
if (b < 0.0)
sq = -sq;
init_phi[i] = acos(a/sq);
curr_sol[i+2] = c/sq;
#ifdef print_init
fprintf(stdout,"3d ln ");
fprintf(stdout,"%lf %lf \n", init_phi[i],curr_sol[i+2]);
#endif
};
/* average the angles */
hd = 0.0;
hd1 = 0.0;
for (i = 1; i <= num_3d_lines; i = i+2) {
hd += cos(2.0 * init_phi[i]);
hd1 += sin(2.0 * init_phi[i]);
}
for (i = 2; i <= num_3d_lines; i = i+2) {
hd += cos(2.0 * (init_phi[i] + M_PI_2));
hd1 += sin(2.0 * (init_phi[i] + M_PI_2));
}
hd /= (num_3d_lines);
hd1 /= (num_3d_lines);
hd = acos(hd);
if (hd1 < 0.0) /* the angle we are averaging is > pi/2 */
hd = 2 * M_PI - hd;
hd *= 0.5;
line_orient = 0;
if (hd > M_PI_2) /* correct if the angle if > PI/2 */
{
hd -= M_PI;
}
#ifdef print_init
fprintf(stderr,"angle, line orientation %lf %d \n",hd,line_orient);
#endif
curr_sol[1] = hd;
start_phi = hd; /* the initial angle guess */
/* we have a new angle, recompute the lines */
for (i=1 ; i <= num_3d_lines; i++) {
hd = curr_sol[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2;
/*
compute the middle point
*/
xm = (initial_3d_lines[i][1] + initial_3d_lines[i][4]) / 2.0;
ym = (initial_3d_lines[i][2] + initial_3d_lines[i][5]) / 2.0;
/*
recompute the dist with new angle
*/
curr_sol[i+2] = - ((cos(hd) * xm) + (sin(hd) * ym));
#ifdef print_init
fprintf(stdout,"3d ln ");
fprintf(stdout,"%lf %lf \n", hd, curr_sol[i+2]);
#endif
}
/* average the heigth */
hd = 0.0;
for (i=1; i <= num_3d_lines; i++) {
hd += initial_3d_lines[i][3];
hd += initial_3d_lines[i][6];
};
hd /= (num_3d_lines * 2.0);
curr_sol[2] = hd;
/* curr_sol has now in 1. component the angle, in 2. height and in
the following components the r value for each line
*/
/* we save this info in case we are unable to triangulate */
for (i=1; i <= num_3d_lines + 2; i++)
initial_sol[i] = curr_sol[i];
/* read the number of images */
fscanf(poly,"%d",&num_images);
if (num_images == 0) {
printf("to less images \n");
close(poly);
close(out);
return;
}
/* get a vector of numbers of 2d lines per image */
num_image_2d_lines = ivector(1,num_images);
/* get a vector of view numbers for each image */
view_numbers = ivector(1,num_images);
/* get a vector of projection data structures for */
fbips = (FBIP *)malloc((num_images+1) * sizeof(FBIP));
/* get a vector of 2d image data per image */
image_2d_lines = (double***)malloc(sizeof(double *) * (num_images+1));
/* get a vector of pointers to pose matrices */
pose_matrices = (double***)malloc(sizeof(double *) * (num_images+1));
/*
for (i = 0; i <= num_images; i++)
pose_matrices[i] = 0;
*/
num_2d_lines = 0;
/* start a loop to read in the line data */
for(i=1; i <= num_images; i++) {
#ifdef full_debug
fprintf(stdout,"num images %d\n",num_images);
#endif
/* do we have fbip ? */
#ifdef old_version
h = 1;
#else
fscanf(poly,"%d",&h);
#endif
view_no = i;
view_numbers[i] = i;
if (h != 0) {
/* read the image projection string */
fscanf(poly,"%s",hstr);
/* here you could give the images numbers based on their filename or
something else */
/* get the fbip projection data from file */
fbips[view_no] = read_fbip_file(hstr);
}
else
{
fbips[view_no] = 0;
}
/* do we have a projection matrix ? */
#ifdef old_version
h = 0;
#else
fscanf(poly,"%d",&h);
#endif
/* if so, read it */
if (h != 0) {
pose_matrices[view_no] = dmatrix(1,4,1,4);
for (j = 1; j <= 4; j++)
for (k = 1; k <= 4; k++)
fscanf(poly,"%lf",&(pose_matrices[view_no][j][k]));
}
else
{
pose_matrices[view_no] = 0;
}
/* read the number of lines and start a loop to read the data */
fscanf(poly,"%d",&num_image_lines);
#ifdef full_debug
fprintf(stdout,"%d \n",num_image_lines);
#endif
num_2d_lines += num_image_lines;
/* read the line data */
num_image_2d_lines[i] = num_image_lines;
image_2d_lines[i] = dmatrix(1,num_image_lines,1,7);
for(j=1; j <= num_image_lines; j++) {
for (k=1; k <= 5; k++)
fscanf(poly,"%lf",&(image_2d_lines[i][j][k]));
/* compute the hesse normal form */
a = image_2d_lines[i][j][3] - image_2d_lines[i][j][5];
b = image_2d_lines[i][j][4] - image_2d_lines[i][j][2];
c = image_2d_lines[i][j][2] * image_2d_lines[i][j][5] -
image_2d_lines[i][j][4] * image_2d_lines[i][j][3];
sq = sqrt(a*a + b*b);
if (b < 0.0)
sq = -sq;
image_2d_lines[i][j][6] = acos(a/sq);
image_2d_lines[i][j][7] = -c/sq;
#ifdef print_init
fprintf(stdout,"img ln ");
for (k=1; k <= 7; k++)
fprintf(stdout,"%lf ",image_2d_lines[i][j][k]);
fprintf(stdout,"\n");
#endif
};
};
/* build up the index array */
index_array = imatrix(1,num_2d_lines,1,3);
i = 1;
for (j=1; j <= num_images; j++)
for (k=1; k <= num_image_2d_lines[j]; k++) {
index_array[i][1] = j;
index_array[i][2] = view_numbers[j];
index_array[i][3] = k;
i++;
};
/* start the iteration */
/* set up a vector of goal solutions */
lm_y = dvector(1,num_2d_lines);
dist_vec = dvector(1,num_2d_lines);
for (i=1; i <= num_2d_lines; i++) {
lm_y[i] = 0.0;
};
#ifdef full_debug
fprintf(stdout,"before iteration \n");
#endif
minfo.Num_Loops = 100;
minfo.Min_Incr = EPSILON;
minfo.diag = 1;
minfo.nrows = num_2d_lines;
minfo.ncols = num_3d_lines+2;
minfo.weight = NULL;
minfo.covar = NULL;
minfo.param = curr_sol;
minfo.goal = lm_y;
minfo.gen_jacobian = NULL;
minfo.function = &function;
minfo.new_position_action = &new_position_action;
old_phi = curr_sol[1];
old_height = curr_sol[2];
/* do Levenberg Marquardt */
#ifdef print_init
for (i=1; i <= num_3d_lines+2 ; i++)
fprintf(stdout,"%lf ",curr_sol[i]);
fprintf(stdout,"\n");
#endif
function(curr_sol, dist_vec, num_2d_lines, num_3d_lines+2);
old_rms = compute_rms(dist_vec, lm_y, num_2d_lines);
if (old_rms < MAX_INITIAL_RMS) {
marquardt(&minfo);
function(curr_sol, dist_vec, num_2d_lines, num_3d_lines+2);
new_rms = compute_rms(dist_vec, lm_y, num_2d_lines);
}
else
new_rms = old_rms + 1.0;
if (new_rms > old_rms) {
/* something must have gone wrong */
fprintf(stdout,"Unable to triangulate, using initial guess \n");
fprintf(out,"%d\n",num_3d_lines);
for (i = 1; i <= num_3d_lines; i++) {
for (j = 1; j <= 3; j++)
fprintf(out,"%lf ",initial_3d_lines[i][j]);
fprintf(out,"\n");
}
printf("OUTPUT OLD\n");
fprintf(out,"%lf\n",old_rms);
}
else
{
/* output the result */
fprintf(out,"%d\n",num_3d_lines);
/* compute the vertices and output them */
for (i = 1; i <= num_3d_lines; i++) {
c = (cos(curr_sol[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2) *
sin(curr_sol[1] + (i % 2 != line_orient ? 1 : 0) * M_PI_2)) -
(cos(curr_sol[1] + (i % 2 != line_orient ? 1 : 0) * M_PI_2) *
sin(curr_sol[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2));
if (c == 0.0) {
fprintf(stdout,"error intersecting the lines \n");
fprintf(out,"\n");
}
else
{
a = (sin(curr_sol[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2) *
curr_sol[(i < num_3d_lines ? i+3 : 3)]) -
(sin(curr_sol[1] + (i % 2 != line_orient ? 1 : 0) * M_PI_2) *
curr_sol[i+2]);
b = (curr_sol[i+2] *
cos(curr_sol[1] + (i % 2 != line_orient ? 1 : 0) * M_PI_2)) -
(curr_sol[(i < num_3d_lines ? i+3 : 3)] *
cos(curr_sol[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2));
fprintf(out,"%lf %lf %lf \n",a/c,b/c,curr_sol[2]);
};
};
/* output the lines */
for (i=1; i <= num_3d_lines+2 ; i++)
fprintf(out,"%lf \n",curr_sol[i]);
/* free everything */
printf("OUTPUT NEW\n");
fprintf(out,"%lf\n",new_rms);
}
#ifdef print_rms
printf("old / new rms: %lf %lf \n",old_rms,new_rms);
#endif
#ifdef full_debug
fprintf(stdout,"before freeing \n");
#endif
free_dvector(curr_sol,1,num_3d_lines+2);
free_dvector(initial_sol,1,num_3d_lines+2);
free_dmatrix(initial_3d_lines,1,num_3d_lines,1,6);
free_dvector(init_phi,1,num_3d_lines);
for(i=1; i<=num_images; i++)
free_dmatrix(image_2d_lines[i],1,num_image_2d_lines[i],1,7);
free(image_2d_lines);
for(i=1; i<=num_images; i++)
if (pose_matrices[i] != 0)
free_dmatrix(pose_matrices[i],1,4,1,4);
free(pose_matrices);
free_ivector(num_image_2d_lines,1,num_images);
free_ivector(num_image_2d_lines,1,num_images);
free_ivector(view_numbers,1,num_images);
free_imatrix(index_array,1,num_2d_lines,1,3);
free_dvector(lm_y,1,num_2d_lines);
free_dvector(dist_vec,1,num_2d_lines);
for(i=1; i<=num_images; i++)
if (fbips[i] != 0)
free(fbips[i]);
free(fbips);
fclose(poly);
fclose(out);
}
main(int argc, char *argv[])
{
Triangulation(argv[1],argv[2]);
}