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