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 / tri.c < prev    next >
C/C++ Source or Header  |  1996-04-23  |  18KB  |  838 lines
  1. #include <stdio.h>
  2. #include "nrutil.h"
  3. #include <math.h>
  4. #include "rh_util.h"
  5. #include "marquardt.h"
  6. #include "fbip.h"
  7.  
  8.  
  9. /*
  10. #define old_version 1
  11. */
  12. #define print_rms 1
  13. /*
  14. #define print_init 1
  15. #define state_output 1
  16. #define point_output 1
  17. #define dist_output 1
  18. #define hesse_output 1
  19. #define full_debug 1
  20. */
  21.  
  22.  
  23. #define EPSILON 1.0e-4
  24. #define MAX_ANGLE_STEP 0.2
  25. #define MAX_HEIGTH_STEP 50
  26.  
  27. #define MAX_INITIAL_RMS 1.0e6
  28.  
  29. /* global data structures */
  30.  
  31. int **index_array;
  32. double ***image_2d_lines;
  33. double ***pose_matrices;
  34. int num_3d_lines;
  35. double **initial_3d_lines;
  36. double *curr_sol;
  37. double *initial_sol;
  38. double old_phi;
  39. double old_height;
  40. FBIP *fbips;
  41. int line_orient;
  42. int bail_out_status = 0;
  43.  
  44.  
  45. void project_to_two_d(double **matrix, double *from, double *into)
  46. {
  47.  
  48.   int i,j;
  49.  
  50.   for (i = 0; i < 4; i++) {
  51.     into[i] = 0.0;
  52.  
  53.     for (j = 0; j < 4; j++)
  54.       into[i] += matrix[i+1][j+1] * from[j];
  55.   };
  56.  
  57.   for (i = 0; i < 3; i++) 
  58.     into[i] /= into[3]; 
  59.  
  60. };
  61.  
  62.  
  63. double compute_rms(double vector[], double goal_vector[], int n)
  64. {
  65.   int i;
  66.   double rms = 0.0;
  67.  
  68.   for (i = 1; i <= n; i++)
  69.     rms += ((vector[i] - goal_vector[i]) * (vector[i] - goal_vector[i]));
  70.   return(rms);
  71. };
  72.  
  73.  
  74. void ComputeDistance(int ln_no_3d, double *mparam,
  75.                      int view_no, 
  76.                      double *image_line, double *result)
  77.  
  78. /* returns the sum of the 
  79.    distance of the endpoints of the 2d line to the 
  80.    2d line which is the result of the projection of the 3d line */
  81.  
  82. {
  83.  
  84.   int i,j;
  85.  
  86.   double pt_1[5];
  87.   double pt_2[5];
  88.  
  89.   double hd;
  90.   double hvectord[7];
  91.   double a,b,c;
  92.  
  93.   COORDINATE worldv[4];
  94.   COORDINATE viewv[4];
  95.   COORDINATE *viewv_ptr;
  96.  
  97.  
  98.  i = ln_no_3d;
  99.  j = ln_no_3d + 1;
  100.  if (j > num_3d_lines)
  101.    j = 1;
  102.  
  103.  c = (cos(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2) * 
  104.       sin(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2)) -
  105.      (cos(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2) * 
  106.       sin(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2));
  107.  
  108.   if (c == 0.0) { 
  109.     fprintf(stdout,"error intersecting the lines \n");
  110.     bail_out_status = 1;
  111.   }
  112.   else 
  113.   {
  114.     a = (sin(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2) *
  115.          mparam[j+2]) -
  116.         (sin(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2) *
  117.          mparam[i+2]);
  118.     b = (mparam[i+2] * 
  119.          cos(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2)) -
  120.         (mparam[j+2] *
  121.          cos(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2));
  122.     pt_1[1] = a/c;
  123.     pt_1[2] = b/c;
  124.     pt_1[3] = mparam[2];
  125.     pt_1[4] = 1.0;
  126.   };
  127.  
  128.  
  129.  j = ln_no_3d - 1;
  130.  if (j < 1)
  131.    j = num_3d_lines;
  132.  
  133.  c = (cos(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2) * 
  134.       sin(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2)) -
  135.      (cos(mparam[1] + (j % 2 == line_orient ? 1 : 0) * M_PI_2) * 
  136.       sin(mparam[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2));
  137.  
  138.   if (c == 0.0) { 
  139.     fprintf(stdout,"error intersecting the lines \n");
  140.     bail_out_status = 1;
  141.   }
  142.   else 
  143.   {
  144.     a = (sin(mparam[1] + (i % 2 == line_orient  ? 1 : 0) * M_PI_2) *
  145.          mparam[j+2]) -
  146.         (sin(mparam[1] + (j % 2 == line_orient  ? 1 : 0) * M_PI_2) *
  147.          mparam[i+2]);
  148.     b = (mparam[i+2] * 
  149.          cos(mparam[1] + (j % 2 == line_orient  ? 1 : 0) * M_PI_2)) -
  150.         (mparam[j+2] *
  151.          cos(mparam[1] + (i % 2 == line_orient  ? 1 : 0) * M_PI_2));
  152.     pt_2[1] = a/c;
  153.     pt_2[2] = b/c;
  154.     pt_2[3] = mparam[2];
  155.     pt_2[4] = 1.0;
  156.   };
  157.  
  158.  
  159. #ifdef point_output
  160.   fprintf(stdout,"3d point 1: %lf %lf %lf \n",pt_1[1],pt_1[2],pt_1[3]);
  161.   fprintf(stdout,"3d point 2: %lf %lf %lf \n",pt_2[1],pt_2[2],pt_2[3]);
  162. #endif
  163.  
  164.   /* project into 2d */
  165.  
  166.  worldv[0] = pt_1[1];
  167.  worldv[1] = pt_1[2];
  168.  worldv[2] = pt_1[3];
  169.  worldv[3] = 1.0;
  170.  
  171.  
  172.  if (pose_matrices[view_no] != 0)
  173.    project_to_two_d(pose_matrices[view_no],worldv,viewv);
  174.  
  175.  if (fbips[view_no] != 0)
  176.    viewv_ptr = fbip_project_to_view(fbips[view_no], worldv, viewv); 
  177.   
  178.  hvectord[1] = viewv[0]; 
  179.  hvectord[2] = viewv[1]; 
  180.  
  181.  worldv[0] = pt_2[1];
  182.  worldv[1] = pt_2[2];
  183.  worldv[2] = pt_2[3];
  184.  worldv[3] = 1.0;
  185.  
  186.  if (pose_matrices[view_no] != 0)
  187.    project_to_two_d(pose_matrices[view_no],worldv,viewv);
  188.  
  189.  if (fbips[view_no] != 0)
  190.    viewv_ptr = fbip_project_to_view(fbips[view_no], worldv, viewv); 
  191.  
  192.  hvectord[3] = viewv[0]; 
  193.  hvectord[4] = viewv[1]; 
  194.  
  195. /* the 2d points are now in hvectord[1..2] and hvectord[3..4] */
  196.  
  197. #ifdef point_output 
  198.   fprintf(stdout,"2d points %d %lf %lf %lf %lf \n",view_no,hvectord[1],
  199.                                       hvectord[2],
  200.                                       hvectord[3],
  201.                                       hvectord[4]);
  202.   fscanf(stdin,"%*c");
  203. #endif
  204.  
  205.   /* compute the distance */ 
  206.  
  207.   hvectord[1] = hvectord[1] * cos(image_line[6]) 
  208.               + hvectord[2] * sin(image_line[6])   
  209.               - image_line[7];
  210.  
  211.   hvectord[2] = hvectord[3] * cos(image_line[6]) 
  212.               + hvectord[4] * sin(image_line[6])   
  213.               - image_line[7];
  214.  
  215.   *result = (hvectord[1]*hvectord[1]) + (hvectord[2]*hvectord[2]);
  216.  
  217. #ifdef hesse_output
  218.   fprintf(stdout,"hesse image line: %lf %lf %lf \n",image_line[1],
  219.                   image_line[6],image_line[7]);
  220. #endif
  221.  
  222. #ifdef dist_output
  223.   fprintf(stdout,"distance %lf: \n",*result);
  224. #endif
  225.  
  226. };
  227.  
  228.  
  229. /* the function which we try to minimize the parameters for */
  230.  
  231. void function(double a[], double y[], int nrows, int ncols) {
  232.   
  233.   int i;
  234.   int image_no; 
  235.   int view_no;
  236.   int line_no;
  237.   double phi;
  238.   double h;
  239.  
  240. #ifdef full_debug
  241.   fprintf(stdout,"in function \n");
  242. #endif
  243.  
  244.   for (i = 1; i <= nrows; i++) { 
  245.  
  246.     image_no = index_array[i][1]; 
  247.     view_no = index_array[i][2]; 
  248.     line_no = index_array[i][3]; 
  249.  
  250. #ifdef full_debug
  251.     fprintf(stdout,"%d %d %d\n",image_no,view_no,line_no);
  252. #endif
  253.  
  254.  
  255.     ComputeDistance((int)(image_2d_lines[image_no][line_no][1]),
  256.                   a,
  257.                   view_no,
  258.                   image_2d_lines[image_no][line_no],
  259.                   &h); 
  260.     y[i] = h;
  261. #ifdef debug
  262.     fprintf(stdout,"%lf \n",y[i]);
  263. #endif
  264.   };
  265. #ifdef full_debug
  266.   for (i = 1; i <= nrows; i++)
  267.     fprintf(stdout,"%lf ",y[i]);
  268.   fprintf(stdout,"\n");
  269. #endif
  270. };
  271.  
  272. void new_position_action(double *mparam, int ncols) {
  273. /* gets called when parameters are changed */
  274.  
  275. int i;
  276.  
  277.   /* check if the quadrants have changed */
  278.  
  279.   if   ((old_phi > M_PI_2) && (mparam[1] <= M_PI_2)
  280.      || (old_phi <= M_PI_2) && (mparam[1] > M_PI_2)
  281.      || (old_phi > 0.0) && (mparam[1] <= 0.0)
  282.      || (old_phi <= 0.0) && (mparam[1] > 0.0)) {
  283. #ifdef state_output
  284.     fprintf(stdout,"wraparound \n");
  285. #endif
  286.   };
  287.  
  288.   if (fabs(old_phi - curr_sol[1]) > MAX_ANGLE_STEP)
  289.     curr_sol[1] = old_phi;
  290.  
  291.   if (fabs(old_height - curr_sol[2]) > MAX_HEIGTH_STEP)
  292.     curr_sol[2] = old_height;
  293.  
  294.   old_phi = curr_sol[1];
  295.   old_height = curr_sol[2];
  296.  
  297. #ifdef state_output
  298.   for (i = 1; i <=ncols; i++) 
  299.     fprintf(stdout,"%lf ",mparam[i]);
  300.   fprintf(stdout,"\n");
  301. #endif
  302.  
  303. }
  304.  
  305.  
  306. /* function to triangulate, takes three arguments  */ 
  307.  
  308. void Triangulation(char * polyfile, char * outfile)
  309. {
  310.   int num_image_lines;
  311.   int num_horizontal_lines;
  312.   int num_constraints;
  313.   int num_images;
  314.   int num_inits;
  315.   int i,j,k;
  316.   int h;
  317.  
  318.   FILE  *poly, *out;
  319.  
  320.   int *num_image_2d_lines;
  321.   double *init_phi;
  322.  
  323.   double hvectord[7];
  324.  
  325.   double dummyd;
  326.   int dummyi;
  327.   char dummyc;
  328.   char hstr[255];
  329.   double hd;
  330.   double hd1;
  331.   double hd2;
  332.   double xm, ym;
  333.   double start_phi;
  334.   double a,b,c,sq;
  335.  
  336.   int num_2d_lines;
  337.   int view_no;
  338.   int *view_numbers;
  339.  
  340.   double *lm_y;
  341.   double *dist_vec;
  342.  
  343.   double old_rms, new_rms;
  344.  
  345.   Marquardt_info minfo;
  346.  
  347.   fprintf(stdout,"Running triangulator \n"); 
  348.  
  349. /* start to read in the values     */
  350.  
  351.   poly = fopen(polyfile,"r");
  352.   if (poly == 0) {
  353.     fprintf(stdout,"file [%s] not existing, exiting \n",polyfile);
  354.     return;
  355.   }
  356.   out = fopen(outfile,"w");
  357.  
  358. /* read the initial guess          */
  359.  
  360.   fscanf(poly,"%d",&num_3d_lines);
  361.  
  362.   if (num_3d_lines < 2) {
  363.     fprintf(stdout,"not enough lines \n");
  364.     close(poly);
  365.     close(out); 
  366.     return;
  367.   }
  368.  
  369. /* first intermediate result = initial guess */
  370.  
  371.   init_phi = dvector(1,num_3d_lines);
  372.   curr_sol = dvector(1,num_3d_lines+2);
  373.   initial_sol = dvector(1,num_3d_lines+2);
  374.   initial_3d_lines = dmatrix(1,num_3d_lines,1,6);
  375.  
  376. #ifdef full_debug
  377.   fprintf(stdout,"before init guess read \n"); 
  378. #endif
  379.   
  380.   for(i=1; i <= num_3d_lines; i++) {
  381.  
  382.     for(j=1; j <= 6; j++) {
  383.       fscanf(poly,"%lf",&(initial_3d_lines[i][j]));
  384.     };
  385.  
  386.     a = initial_3d_lines[i][2] - initial_3d_lines[i][5];
  387.  
  388.     b = initial_3d_lines[i][4] - initial_3d_lines[i][1];
  389.  
  390.     c  = initial_3d_lines[i][1] * initial_3d_lines[i][5] -
  391.          initial_3d_lines[i][4] * initial_3d_lines[i][2];
  392.  
  393.     sq = sqrt(a*a + b*b);
  394.  
  395.     if (b < 0.0)
  396.       sq = -sq;
  397.  
  398.     init_phi[i] = acos(a/sq); 
  399.     curr_sol[i+2] = c/sq; 
  400.  
  401. #ifdef print_init
  402.     fprintf(stdout,"3d ln ");
  403.     fprintf(stdout,"%lf %lf \n", init_phi[i],curr_sol[i+2]);
  404. #endif 
  405.   };
  406.  
  407.  
  408.   /* average the angles */
  409.  
  410.   hd = 0.0;
  411.   hd1 = 0.0;
  412.  
  413.   for (i = 1; i <= num_3d_lines; i = i+2) {
  414.     hd += cos(2.0 * init_phi[i]);
  415.     hd1 += sin(2.0 * init_phi[i]);
  416.   }
  417.  
  418.   for (i = 2; i <= num_3d_lines; i = i+2) {
  419.     hd += cos(2.0 * (init_phi[i] + M_PI_2));  
  420.     hd1 += sin(2.0 * (init_phi[i] + M_PI_2));
  421.   }
  422.  
  423.   hd /= (num_3d_lines);
  424.   hd1 /= (num_3d_lines);
  425.  
  426.   hd =  acos(hd);
  427.  
  428.   if (hd1 < 0.0)                /* the angle we are averaging is > pi/2 */
  429.     hd = 2 * M_PI - hd;
  430.     
  431.   hd *= 0.5;
  432.  
  433.   line_orient = 0;
  434.  
  435.   if (hd > M_PI_2)             /* correct if the angle if > PI/2 */
  436.   {
  437.     hd -= M_PI;
  438.   }
  439.  
  440. #ifdef print_init
  441.   fprintf(stderr,"angle, line orientation %lf %d \n",hd,line_orient);
  442. #endif
  443.  
  444.   curr_sol[1] = hd;
  445.   start_phi = hd;              /* the initial angle guess */
  446.  
  447.   /* we have a new angle, recompute the lines */
  448.  
  449.   for (i=1 ; i <= num_3d_lines; i++) {
  450.  
  451.     hd = curr_sol[1] + (i % 2 == line_orient ? 1 : 0) * M_PI_2;    
  452.  
  453. /*
  454.     compute the middle point 
  455. */
  456.  
  457.     xm = (initial_3d_lines[i][1] + initial_3d_lines[i][4]) / 2.0; 
  458.     ym = (initial_3d_lines[i][2] + initial_3d_lines[i][5]) / 2.0; 
  459.  
  460. /*
  461.     recompute the dist with new angle 
  462. */
  463.  
  464.     curr_sol[i+2] = - ((cos(hd) * xm) + (sin(hd) * ym)); 
  465.  
  466.  
  467. #ifdef print_init
  468.     fprintf(stdout,"3d ln ");
  469.     fprintf(stdout,"%lf %lf \n", hd, curr_sol[i+2]);
  470. #endif
  471.  
  472.   }  
  473.  
  474.   /* average the heigth */    
  475.  
  476.   hd = 0.0; 
  477.   for (i=1; i <= num_3d_lines; i++) {
  478.     hd += initial_3d_lines[i][3];
  479.     hd += initial_3d_lines[i][6];
  480.   };
  481.   hd /= (num_3d_lines * 2.0);
  482.  
  483.   curr_sol[2] = hd;
  484.   
  485. /* curr_sol has now in 1. component the angle, in 2. height and in 
  486.    the following components the r value for each line 
  487. */
  488.  
  489.   /* we save this info in case we are unable to triangulate */
  490.  
  491.   for (i=1; i <= num_3d_lines + 2; i++)
  492.     initial_sol[i] = curr_sol[i];
  493.   
  494. /* read the number of images       */
  495.    
  496.   fscanf(poly,"%d",&num_images);
  497.  
  498.   if (num_images == 0) {
  499.     printf("to less images \n");
  500.     close(poly);
  501.     close(out);
  502.     return;
  503.   }
  504.  
  505. /* get a vector of numbers of 2d lines per image */
  506.  
  507.   num_image_2d_lines = ivector(1,num_images);
  508.  
  509. /* get a vector of view numbers for each image */
  510.  
  511.   view_numbers = ivector(1,num_images); 
  512.  
  513. /* get a vector of projection data structures for */
  514.  
  515.   fbips = (FBIP *)malloc((num_images+1) * sizeof(FBIP));
  516.  
  517. /* get a vector of 2d image data per image */
  518.  
  519.   image_2d_lines = (double***)malloc(sizeof(double *) * (num_images+1));
  520.  
  521. /* get a vector of pointers to pose matrices */
  522.  
  523.   pose_matrices = (double***)malloc(sizeof(double *) * (num_images+1));
  524.  
  525. /*
  526.   for (i = 0; i <= num_images; i++) 
  527.     pose_matrices[i] = 0;
  528. */
  529.  
  530.   num_2d_lines = 0;
  531.  
  532. /* start a loop to read in the line data */
  533.   
  534.   for(i=1; i <= num_images; i++) {
  535.  
  536. #ifdef full_debug
  537.   fprintf(stdout,"num images %d\n",num_images);
  538. #endif
  539.  
  540. /* do we have fbip ? */
  541.  
  542. #ifdef old_version
  543.     h = 1;
  544. #else
  545.     fscanf(poly,"%d",&h);
  546. #endif
  547.  
  548.     view_no = i;
  549.  
  550.     view_numbers[i] = i;   
  551.  
  552.     if (h != 0) {
  553.  
  554. /* read the image projection string */
  555.  
  556.       fscanf(poly,"%s",hstr);
  557.  
  558. /* here you could give the images numbers based on their filename or 
  559.    something else */
  560.  
  561. /* get the fbip projection data from file */
  562.  
  563.       fbips[view_no] = read_fbip_file(hstr);
  564.       
  565.     }
  566.     else
  567.     {  
  568.       fbips[view_no] = 0; 
  569.     }
  570.  
  571. /* do we have a projection matrix ? */    
  572.  
  573. #ifdef old_version
  574.     h = 0;
  575. #else
  576.     fscanf(poly,"%d",&h);
  577. #endif
  578.  
  579. /* if so, read it */
  580.  
  581.     if (h != 0) {
  582.       
  583.       pose_matrices[view_no] = dmatrix(1,4,1,4);    
  584.  
  585.       for (j = 1; j <= 4; j++)
  586.         for (k = 1; k <= 4; k++)
  587.           fscanf(poly,"%lf",&(pose_matrices[view_no][j][k])); 
  588.  
  589.     }
  590.     else
  591.     {  
  592.       pose_matrices[view_no] = 0; 
  593.     }
  594.  
  595.  
  596. /* read the number of lines and start a loop to read the data */
  597.   
  598.     fscanf(poly,"%d",&num_image_lines);
  599.  
  600. #ifdef full_debug
  601.     fprintf(stdout,"%d \n",num_image_lines);
  602. #endif
  603.  
  604.       num_2d_lines += num_image_lines;
  605.      
  606. /* read the line data        */
  607.  
  608.     num_image_2d_lines[i] = num_image_lines;
  609.  
  610.     image_2d_lines[i] = dmatrix(1,num_image_lines,1,7);
  611.     
  612.     for(j=1; j <= num_image_lines; j++) {
  613.  
  614.       for (k=1; k <= 5; k++) 
  615.         fscanf(poly,"%lf",&(image_2d_lines[i][j][k]));
  616.      
  617. /* compute the hesse normal form */
  618.  
  619.       a = image_2d_lines[i][j][3] - image_2d_lines[i][j][5];
  620.       
  621.       b = image_2d_lines[i][j][4] - image_2d_lines[i][j][2];
  622.  
  623.       c = image_2d_lines[i][j][2] * image_2d_lines[i][j][5] -
  624.           image_2d_lines[i][j][4] * image_2d_lines[i][j][3];
  625.       
  626.       sq = sqrt(a*a + b*b);
  627.  
  628.       if (b < 0.0)
  629.         sq = -sq;
  630.  
  631.       image_2d_lines[i][j][6] = acos(a/sq);
  632.  
  633.       image_2d_lines[i][j][7] = -c/sq;
  634.  
  635. #ifdef print_init
  636.       fprintf(stdout,"img ln ");
  637.       for (k=1; k <= 7; k++) 
  638.         fprintf(stdout,"%lf ",image_2d_lines[i][j][k]);
  639.       fprintf(stdout,"\n");
  640. #endif
  641.  
  642.     };
  643.   };
  644.  
  645. /* build up the index array */
  646.  
  647.   index_array = imatrix(1,num_2d_lines,1,3);
  648.  
  649.   i = 1;
  650.   for (j=1; j <= num_images; j++)
  651.     for (k=1; k <= num_image_2d_lines[j]; k++) {
  652.       index_array[i][1] = j;
  653.       index_array[i][2] = view_numbers[j];
  654.       index_array[i][3] = k;
  655.       i++;
  656.     };
  657.  
  658. /* start the iteration             */
  659.  
  660. /* set up a vector of goal solutions */
  661.  
  662.   lm_y        = dvector(1,num_2d_lines);
  663.   dist_vec    = dvector(1,num_2d_lines);
  664.  
  665.   for (i=1; i <= num_2d_lines; i++) {
  666.     lm_y[i] = 0.0;
  667.   };
  668.  
  669. #ifdef full_debug
  670.   fprintf(stdout,"before iteration \n");
  671. #endif
  672.  
  673.   minfo.Num_Loops = 100;
  674.   minfo.Min_Incr = EPSILON;
  675.   minfo.diag = 1;
  676.   minfo.nrows = num_2d_lines;
  677.   minfo.ncols = num_3d_lines+2;
  678.   minfo.weight = NULL;
  679.   minfo.covar = NULL;
  680.   minfo.param = curr_sol;
  681.   minfo.goal = lm_y;
  682.   minfo.gen_jacobian = NULL;
  683.   minfo.function = &function;
  684.   minfo.new_position_action = &new_position_action;
  685.  
  686.   old_phi = curr_sol[1];
  687.   old_height = curr_sol[2];
  688.  
  689. /* do Levenberg Marquardt */   
  690.  
  691. #ifdef print_init
  692.   for (i=1; i <= num_3d_lines+2 ; i++) 
  693.     fprintf(stdout,"%lf  ",curr_sol[i]);
  694.   fprintf(stdout,"\n");
  695. #endif
  696.  
  697.   function(curr_sol, dist_vec, num_2d_lines, num_3d_lines+2); 
  698.   old_rms = compute_rms(dist_vec, lm_y, num_2d_lines); 
  699.  
  700.   if (old_rms < MAX_INITIAL_RMS) {
  701.  
  702.     marquardt(&minfo);
  703.  
  704.     if (bail_out_status != 0)
  705.       goto bail_out;
  706.  
  707.     function(curr_sol, dist_vec, num_2d_lines, num_3d_lines+2); 
  708.     new_rms = compute_rms(dist_vec, lm_y, num_2d_lines); 
  709.  
  710.   }
  711.   else
  712.     new_rms = old_rms + 1.0;   
  713.    
  714.   if (new_rms > old_rms) { 
  715.  
  716.     /* something must have gone wrong */
  717.  
  718.     fprintf(stdout,"Unable to triangulate, using initial guess \n"); 
  719.  
  720.     fprintf(out,"%d\n",num_3d_lines);
  721.  
  722.     for (i = 1; i <= num_3d_lines; i++) {
  723.       for (j = 1; j <= 3; j++)
  724.         fprintf(out,"%lf ",initial_3d_lines[i][j]);
  725.       fprintf(out,"\n");  
  726.     }
  727.     printf("OUTPUT OLD\n");
  728.     fprintf(out,"%lf\n",old_rms);
  729.   }
  730.   else 
  731.   {
  732.  
  733. /* output the result               */
  734.  
  735.     fprintf(out,"%d\n",num_3d_lines);
  736.  
  737. /* compute the vertices and output them */
  738.  
  739.     for (i = 1; i <= num_3d_lines; i++) {
  740.       c = (cos(curr_sol[1] + (i % 2 == line_orient  ? 1 : 0) * M_PI_2) * 
  741.            sin(curr_sol[1] + (i % 2 != line_orient  ? 1 : 0) * M_PI_2)) -
  742.           (cos(curr_sol[1] + (i % 2 != line_orient  ? 1 : 0) * M_PI_2) * 
  743.            sin(curr_sol[1] + (i % 2 == line_orient  ? 1 : 0) * M_PI_2));
  744.  
  745.       if (c == 0.0) { 
  746.         fprintf(stdout,"error intersecting the lines \n");
  747.         bail_out_status = 1;
  748.         goto bail_out;
  749.       }
  750.       else 
  751.       {
  752.         a = (sin(curr_sol[1] + (i % 2 == line_orient  ? 1 : 0) * M_PI_2) *
  753.              curr_sol[(i < num_3d_lines ? i+3 : 3)]) -
  754.             (sin(curr_sol[1] + (i % 2 != line_orient  ? 1 : 0) * M_PI_2) *
  755.              curr_sol[i+2]);
  756.         b = (curr_sol[i+2] * 
  757.              cos(curr_sol[1] + (i % 2 != line_orient  ? 1 : 0) * M_PI_2)) -
  758.             (curr_sol[(i < num_3d_lines ? i+3 : 3)] *
  759.              cos(curr_sol[1] + (i % 2 == line_orient  ? 1 : 0) * M_PI_2));
  760.  
  761.         fprintf(out,"%lf %lf %lf \n",a/c,b/c,curr_sol[2]);
  762.       };
  763.     }; 
  764.  
  765.  
  766. /* output the lines                     */
  767.  
  768.     for (i=1; i <= num_3d_lines+2 ; i++) 
  769.       fprintf(out,"%lf  \n",curr_sol[i]);
  770.  
  771. /* free everything                 */
  772.  
  773.     printf("OUTPUT NEW\n");
  774.     fprintf(out,"%lf\n",new_rms);
  775.   }
  776.  
  777. #ifdef print_rms
  778.   printf("old / new rms: %lf %lf \n",old_rms,new_rms);
  779. #endif
  780.  
  781. #ifdef full_debug
  782.   fprintf(stdout,"before freeing \n"); 
  783. #endif
  784.  
  785. bail_out: ;
  786.  
  787.   if (bail_out_status != 0) { 
  788.     fclose(out);
  789.     out = fopen(outfile,"w");
  790.     fprintf(out,"0 \n");
  791.   };
  792.  
  793.   free_dvector(curr_sol,1,num_3d_lines+2); 
  794.  
  795.   free_dvector(initial_sol,1,num_3d_lines+2); 
  796.   
  797.   free_dmatrix(initial_3d_lines,1,num_3d_lines,1,6); 
  798.   
  799.   free_dvector(init_phi,1,num_3d_lines); 
  800.  
  801.   for(i=1; i<=num_images; i++)
  802.     free_dmatrix(image_2d_lines[i],1,num_image_2d_lines[i],1,7);
  803.  
  804.   free(image_2d_lines);
  805.   
  806.   for(i=1; i<=num_images; i++)
  807.     if (pose_matrices[i] != 0)
  808.       free_dmatrix(pose_matrices[i],1,4,1,4);
  809.  
  810.   free(pose_matrices);
  811.   
  812.   free_ivector(num_image_2d_lines,1,num_images); 
  813.  
  814.   free_ivector(num_image_2d_lines,1,num_images); 
  815.    
  816.   free_ivector(view_numbers,1,num_images); 
  817.  
  818.   free_imatrix(index_array,1,num_2d_lines,1,3);
  819.  
  820.   free_dvector(lm_y,1,num_2d_lines);
  821.   free_dvector(dist_vec,1,num_2d_lines);
  822.  
  823.   for(i=1; i<=num_images; i++)
  824.     if (fbips[i] != 0)
  825.       free(fbips[i]);
  826.   free(fbips);
  827.  
  828.   fclose(poly);
  829.   fclose(out);
  830. }
  831.  
  832. main(int argc, char *argv[])
  833. {
  834.  
  835.   Triangulation(argv[1],argv[2]);
  836. }
  837.  
  838.