开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > 3D图形编程 > 查看源码
SDfile.cc
资源名称：scanalyze-1.0.3_source_code.rar　[点击查看]
上传用户：kellyonhid
上传日期：2013-10-12
资源大小：932k
文件大小：38k
源码类别：

3D图形编程

开发平台：

Visual C++

SDfile.cc：源码内容
  1. //############################################################
  2. // 
  3. // SDfile.cc
  4. //
  5. // Kari Pulli
  6. // Tue Feb 23 13:09:27 CET 1999
  7. //
  8. // Read, write, and store the raw data of an *.sd file
  9. //
  10. //############################################################
  12. #include "SDfile.h"
  13. #include "VertexFilter.h"
  14. #include "Random.h"
  15. #include "Median.h"
  16. #include <algorithm>
  19. #ifdef WIN32
  20. # define __median _Median
  21. # define gzclose fclose
  22. int gzread (FILE* file, void* buf, int size)
  23. {
  24. return fread (buf, 1, size, file);
  25. }
  26. #else
  27. # include <zlib.h>
  28. #endif
  29. using std::__median;
  32. #if 1
  33. # include "plvGlobals.h"
  34. #else
  35. bool g_bNoIntensity = false;
  36. bool g_verbose = false;
  37. #endif
  39. #if defined(WIN32) || defined(i386)
  40. #define LITTLE_ENDIAN
  41. #endif
  44. #ifdef LITTLE_ENDIAN
  46. #define swap_short(x) ( (((x) & 0xff) << 8) | ((unsigned short)(x) >> 8) )
  47. #define swap_long(x) ( ((x) << 24) | 
  48.                        (((x) << 8) & 0x00ff0000) | 
  49.        (((x) >> 8) & 0x0000ff00) | 
  50.        ((x) >> 24) )
  52. static short sg_swapbuf_short;
  53. static float sg_swapbuf_float;
  54. static int sg_swapbuf_int;
  56. #define FIX_SHORT(x) (*(unsigned short *)&(x) = 
  57.       swap_short(*(unsigned short *)&(x)))
  58. #define FIX_LONG(x) (*(unsigned *)&(x) = 
  59.      swap_long(*(unsigned *)&(x)))
  60. #define FIX_FLOAT(x) FIX_LONG(x)
  62. #define WRITE_UINT(x) 
  63.  ((sg_swapbuf_int = x), FIX_LONG(sg_swapbuf_int) , (fwrite(&sg_swapbuf_int, sizeof(unsigned int),1,sdfile) != 1))
  64. #define WRITE_FLOAT(x) 
  65.  ((sg_swapbuf_float = x), FIX_FLOAT(sg_swapbuf_float) , (fwrite(&sg_swapbuf_float, sizeof(float),1,sdfile) != 1))
  66. #define WRITE_USHORT(x) 
  67.  ((sg_swapbuf_short = x), FIX_SHORT(sg_swapbuf_short) , (fwrite(&sg_swapbuf_short, sizeof(unsigned short),1,sdfile) != 1))
  69. #else
  71. #define FIX_SHORT(x) (x)
  72. #define FIX_LONG(x) (x)
  73. #define FIX_FLOAT(x) (x)
  75. #define WRITE_UINT(x) 
  76.  (fwrite(&x, sizeof(unsigned int),1,sdfile) != 1)
  77. #define WRITE_FLOAT(x) 
  78.  (fwrite(&x, sizeof(float),1,sdfile) != 1)
  79. #define WRITE_USHORT(x) 
  80.  (fwrite(&x, sizeof(unsigned short),1,sdfile) != 1)
  82. #endif
  85. // These return true on error
  86. #define READ_UINT(x) 
  87.  ((gzread(sdfile, &x, sizeof(unsigned int)) != sizeof(unsigned int)) || (FIX_LONG(x) && 0))
  88. #define READ_FLOAT(x) 
  89.  ((gzread(sdfile, &x, sizeof(float)) != sizeof(float)) || (FIX_FLOAT(x) && 0))
  90. #define READ_USHORT(x) 
  91.  ((gzread(sdfile, &x, sizeof(unsigned short)) != sizeof(unsigned short)) || (FIX_SHORT(x) && 0))
  95. void
  96. SDfile::prepare_axis_proj(void)
  97. {
  98.   if (axis_proj_done) return;
  100.   axis_proj_done = true;
  102.   axis_proj_min = axis_proj_max = 
  103.     xf.axis_project(first_good[0], z_data[0]);
  105.   for (int i=0; i<n_frames; i++) {
  107.     if (first_good[i] >= first_bad[i]) continue;
  109.     float pr = xf.axis_project(first_good[i],
  110.        z_data[row_start[i]]);
  111.     if      (pr < axis_proj_min) axis_proj_min = pr;
  112.     else if (pr > axis_proj_max) axis_proj_max = pr;
  114.     int cnt = first_bad[i] - first_good[i];
  115.     pr = xf.axis_project(first_bad[i]-1,
  116.  z_data[row_start[i]+cnt]);
  117.     if      (pr < axis_proj_min) axis_proj_min = pr;
  118.     else if (pr > axis_proj_max) axis_proj_max = pr;
  120.   }
  121. }    
  125. // The image below describes how the point indices are assumed
  126. // to be arranged. There's two index arrays, both n long.
  127. // 1 *---*---*...*---*
  128. //     /  / ... / 
  129. // 0   *---*---*...*---*
  130. // If the situation looks like 
  131. // 1   *---*...*---*---*
  132. //    /  / ... /  /
  133. // 0 *---*---*...*---*
  134. // the flag upper_row_starts should be set false.
  135. // This routine creates ccw triangles, but the strip can be 
  136. // flipped by setting a flag.
  137. static void
  138. create_strip(vector<int>        &tstrips,
  139.      const vector<int>  &p_inds_0,
  140.      const vector<int>  &p_inds_1,
  141.      bool               upper_row_starts = true,
  142.      bool               flipped = false)
  143. {
  144.   bool str_on = false;
  145.   // 1 *---*
  146.   //    1/2
  147.   // 0   *---*
  148.   int n = p_inds_0.size();
  149.   int end = n-1;
  151.   const int *p0 = &p_inds_0[0];
  152.   const int *p1 = &p_inds_1[0];
  154.   if (flipped) {
  155.     p0 = &p_inds_1[0];
  156.     p1 = &p_inds_0[0];
  157.     upper_row_starts = !upper_row_starts;
  158.   }
  160.   if (upper_row_starts == false) {
  161.     if (p0[0] != -1 &&
  162. p0[1] != -1 &&
  163. p1[0] != -1) {
  164.       tstrips.push_back(p1[0]);
  165.       tstrips.push_back(p0[0]);
  166.       tstrips.push_back(p0[1]);
  167.       tstrips.push_back(-1);
  168.     }
  169.     p0  = p0+1;
  170.     end = n-2;
  171.   }
  173.   for (int i=0; i<end; i++) {
  174.     // triangle 1
  175.     if (str_on) {
  176.       // push the index
  177.       int j = p1[i+1];
  178.       tstrips.push_back(j);
  179.       // still going along a strip?
  180.       str_on = (j != -1);
  181.     } else {
  182.       // should we start a strip?
  183.       if (p1[i  ] != -1 &&
  184.   p1[i+1] != -1 &&
  185.   p0[i  ] != -1) {
  186. tstrips.push_back(p1[i]);
  187. tstrips.push_back(p0[i]);
  188. tstrips.push_back(p1[i+1]);
  189. str_on = true;
  190.       }
  191.     }
  192.     // triangle 2
  193.     if (str_on) {
  194.       // push the index
  195.       int j = p0[i+1];
  196.       tstrips.push_back(j);
  197.       // still going along a strip?
  198.       str_on = (j != -1);
  199.     } else {
  200.       // bad place to start a strip, but a single tri?
  201.       if (p0[i  ] != -1 &&
  202.   p0[i+1] != -1 &&
  203.   p1[i+1] != -1) {
  204. tstrips.push_back(p1[i+1]);
  205. tstrips.push_back(p0[i]);
  206. tstrips.push_back(p0[i+1]);
  207. tstrips.push_back(-1);
  208.       }
  209.     }
  210.   }
  211.   if (upper_row_starts == false) {
  212.     if (p1[n-1] != -1) {
  213.       if (str_on) {
  214. tstrips.push_back(p1[n-1]);
  215.       } else {
  216. p0--; // p0 was shifted, shift it back!
  217. if (p1[n-2] != -1 && p0[n-1] != -1) {
  218.   // single triangle
  219.   tstrips.push_back(p1[n-1]);
  220.   tstrips.push_back(p1[n-2]);
  221.   tstrips.push_back(p0[n-1]);
  222.   tstrips.push_back(-1);
  223. }
  224.       }
  225.     }
  226.   }
  227.   // close strip?
  228.   if (str_on) tstrips.push_back(-1);
  229. }
  230.    
  232. inline void
  233. mark_all_empty(const vector<int> &curr, 
  234.        vector<char>      &bdry)
  235. {
  236.   vector<int>::const_iterator it = curr.begin();
  237.   for (;it != curr.end(); it++)
  238.     if (*it != -1) bdry[*it] = 1;
  239. }
  242. inline void
  243. mark_maybe_empty(const vector<int> &curr,
  244.  const vector<int> &prev, 
  245.  const vector<int> &next,
  246.  vector<char> &bdry) 
  247. {
  248.   int n = curr.size();
  249.   if (curr[0] != -1)   bdry[curr[0]] = 1;
  250.   int end = n-1;
  251.   if (curr[end] != -1) bdry[curr[end]] = 1;
  252.   for (int j=1; j<end; j++) {
  253.     int k = curr[j];
  254.     if (k != -1 && (curr[j-1] == -1 ||
  255.     curr[j+1] == -1 ||
  256.     prev[j]   == -1 ||
  257.     prev[j-1] == -1 ||
  258.     next[j]   == -1 ||
  259.     next[j-1] == -1)) {
  260.       bdry[k] = 1;
  261.     }
  262.   }
  263. }
  266. void
  267. SDfile::make_tstrip_horizontal(vector<int>  &tstrips,
  268.        vector<char> &bdry)
  269. {
  270.   // do horizontal stripping
  272.   int i,j,k;
  273.   assert(pts_per_frame % 2 == 0);
  274.   int n = pts_per_frame / 2;
  275.   // previous, current indices
  276.   vector<int> p_0(n), p_1(n);
  277.   vector<int> c_0(n), c_1(n);
  278.   fill(c_0.begin(), c_0.end(), -1);
  279.   fill(c_1.begin(), c_1.end(), -1);
  281.   int p_idx = 0;
  283.   // initialize the point indices
  284.   int end = first_bad[0] - first_good[0];
  285.   for (i=0; i<end; i++) {
  286.     if (z_data[row_start[0]+i]) {
  287.       j = first_good[0]+i;
  288.       if (j%2) c_1[j/2] = p_idx++;
  289.       else     c_0[j/2] = p_idx++;
  290.     }      
  291.   }
  292.   // first row: part of boundary
  293.   mark_all_empty(c_0, bdry);
  295.   bool flip = (frame_pitch > 0.0);
  296.   bool vscan = scanner_config & 0x00000001;
  297.   if (!vscan) flip = !flip;
  298.   // first frame, within the two fields
  299.   create_strip(tstrips, c_0, c_1, false, flip);
  300.   for (i=1; i<n_frames; i++) {
  301.     
  302.     end = first_bad[i] - first_good[i];
  304.     if (end == 0) {
  305.       if (first_good[i-1] != first_bad[i-1]) {
  306. mark_all_empty(c_1, bdry);
  307.       }
  308.       continue;
  309.     }
  311.     // initialize and copy
  312.     copy(c_0.begin(), c_0.end(), p_0.begin());
  313.     copy(c_1.begin(), c_1.end(), p_1.begin());
  314.     fill(c_0.begin(), c_0.end(), -1);
  315.     fill(c_1.begin(), c_1.end(), -1);
  316.     // fill with indices
  317.     for (j=0; j<end; j++) {
  318.       if (z_data[row_start[i]+j]) {
  319. k = first_good[i]+j;
  320. if (k%2) c_1[k/2] = p_idx++;
  321. else     c_0[k/2] = p_idx++;
  322.       }      
  323.     }
  325.     // between the first field and the previous frame's
  326.     // second field
  327.     if (first_good[i-1] != first_bad[i-1]) {
  328.       create_strip(tstrips, p_1, c_0, true, flip);
  329.       // check boundary status of p_inds[1]
  330.       mark_maybe_empty(p_1, p_0, c_0, bdry);
  331.     }
  332.     
  333.     // between the fields of this frame
  334.     create_strip(tstrips, c_0, c_1, false, flip);
  335.     // check boundary status of c_inds[0]
  336.     if (first_good[i-1] != first_bad[i-1]) {
  337.       mark_maybe_empty(c_0, p_1, c_1, bdry);
  338.     } else {
  339.       mark_all_empty(c_0, bdry);
  340.     }
  341.   }
  342.   
  343.   // last row: all valids are part of boundary
  344.   mark_all_empty(c_1, bdry);
  345. }
  348. static int
  349. get_index(int col, int idx, int n,
  350.   const vector<int> &indices, 
  351.   int good, int bad)
  352. {
  353.   
  354.   if (col < good || col >= bad || idx < 0 || idx >= n)
  355.     return -1;
  356.   return indices[idx];
  357. }
  360. #define IsValidTstripIndex(ind,base,range) 
  361.   (((ind)==-1) || ((ind)>=(base) && (ind)<((base)+(range))))
  363. // the comments make more sense for the make_tstrip_horizontal,
  364. // this was modified from it
  365. void
  366. SDfile::make_tstrip_vertical(vector<int>  &tstrips,
  367.      vector<char> &bdry)
  368. {
  369.   // do vertical stripping
  371.   int i,j,k;
  372.   assert(pts_per_frame % 2 == 0);
  373.   int n = n_frames;
  374.   // previous, current indices
  375.   vector<int> p_0(n), p_1(n);
  376.   vector<int> c_0(n), c_1(n);
  377.   fill(c_0.begin(), c_0.end(), -1);
  378.   fill(c_1.begin(), c_1.end(), -1);
  380.   vector<int> indices(n_pts);
  381.   k = 0;
  382.   int p_idx = 0;
  383.   for (i=0; i<n; i++) {
  384.     int end = first_bad[i] - first_good[i];
  385.     assert(k == row_start[i]);
  386.     for (j=0; j<end; j++) {
  387.       if (z_data[k]) indices[k++] = p_idx++;
  388.       else           indices[k++] = -1;
  389.     }
  390.   }
  392.   // initialize the point indices
  393.   for (i=0; i<n; i++) {
  394.     c_0[i] = get_index(0, row_start[i], n_pts, indices, 
  395.        first_good[i], first_bad[i]);
  396.     c_1[i] = get_index(1, row_start[i]+1, n_pts, indices,
  397.        first_good[i], first_bad[i]);
  398.   }
  400. # ifndef NDEBUG
  401.   for (i=0; i<n_pts; i++) {
  402.     assert (IsValidTstripIndex (indices[i], 0, k));
  403.   }
  404.   for (i=0; i<n; i++) {
  405.     assert (IsValidTstripIndex (c_0[i], 0, k));
  406.     assert (IsValidTstripIndex (c_1[i], 0, k));
  407.   }
  408. # endif
  410.   bool flip = (frame_pitch > 0.0);
  411.   bool vscan = scanner_config & 0x00000001;
  412.   if (!vscan) flip = !flip;
  413.   // first column: part of boundary
  414.   mark_all_empty(c_0, bdry);
  415.   // first frame, within the two fields
  416.   create_strip(tstrips, c_1, c_0, true, flip);
  417.   for (i=2; i<pts_per_frame; i+=2) {
  418.     int oddi = i+1;
  419.     // copy
  420.     copy(c_0.begin(), c_0.end(), p_0.begin());
  421.     copy(c_1.begin(), c_1.end(), p_1.begin());
  422.     // fill with indices
  423.     for (j=0; j<n; j++) {
  424.       int tmp = row_start[j]-first_good[j];
  425.       c_0[j] = get_index(i, tmp+i, n_pts, indices,
  426.  first_good[j], first_bad[j]);
  427.       c_1[j] = get_index(oddi, tmp+oddi, n_pts, indices,
  428.  first_good[j], first_bad[j]);
  429.       assert(IsValidTstripIndex (c_0[j], 0, k));
  430.       assert(IsValidTstripIndex (c_1[j], 0, k));
  431.     }
  433.     // between the first field and the previous frame's
  434.     // second field
  435.     create_strip(tstrips, c_0, p_1, false, flip);
  436.     // check boundary status of p_inds[1]
  437.     mark_maybe_empty(p_1, p_0, c_0, bdry);
  438.     
  439.     // between the fields of this frame
  440.     create_strip(tstrips, c_1, c_0, true, flip);
  441.     // check boundary status of c_inds[0]
  442.     mark_maybe_empty(c_0, p_1, c_1, bdry);
  443.   }
  444.   
  445.   // last row: all valids are part of boundary
  446.   mark_all_empty(c_1, bdry);
  447. }
  453. void
  454. SDfile::fill_index_array(int step, 
  455.  int row,
  456.  SubSampMode subSampMode,
  457.  vector<int> &p,
  458.  vector<Pnt3>  &pnts,
  459.  vector<int>   &ssmap,
  460.  vector<uchar> &intensity,
  461.  vector<uchar> &confidence)
  462. {
  463.   int half_step = step/2;
  464.   int i,j,k;
  465.   int zAccum;
  466.  
  467.   i = row * half_step;
  469.   set_xf(i, false);
  471.   unsigned short *z = &z_data[row_start[i]];
  473.   int firstrow = max(0, i - half_step / 2);
  474.   int lastrow  = min(int(n_frames), i + (half_step+1) / 2);
  475.   int end = first_bad[i] - first_good[i];
  476.   if (subSampMode == holesGrow) {
  477.     for (j=0; j<end;) {
  478.       if (z[j]) {
  479. k = first_good[i]+j;
  480. if (k % step == 0) {
  481.   // now need to search other rows to see how many
  482.   // samples are ok
  483.   int mincol = k - half_step;
  484.   int maxcol = k + half_step;
  485.   for (int irow = firstrow; irow < lastrow; irow++) {
  486.     if (mincol <  first_good[irow] ||
  487. maxcol >= first_bad[irow]) {
  488.       goto loop_exit;
  489.     }
  490.     int off = row_start[irow] - first_good[irow];
  491.     unsigned short *zz   = &z_data[mincol + off];
  492.     unsigned short *zend = &z_data[maxcol + off];
  493.     for (; zz != zend; zz++) {
  494.       if (*zz == 0) goto loop_exit;
  495.     }
  496.   }
  497.     
  498.   // keep the point
  499.   p[k/step] = pnts.size();
  500.   pnts.push_back(xf.apply_xform(k, z[j]));
  501.   ssmap.push_back(row_start[i]+j);
  502.   if (!g_bNoIntensity)
  503.     intensity.push_back (intensity_data[row_start[i]+j]);
  504. }
  505.       }
  506.     loop_exit: j++;
  507.     }
  508.   } else { // don't conserve holes
  509.     for (j=0; j<end; j++) {
  510.       if (z[j]) {
  511. k = first_good[i]+j;
  512. if (k % step == 0) {
  513.   if (subSampMode == conf || subSampMode == filterAvg) {
  514.     // if filtering, keep totals to average:
  515.     int iAccum = 0, kAccum = 0;
  516.     zAccum = 0;
  518.     // now need to search other rows to see how many
  519.     // samples are ok
  520.     int mincol = k - half_step;
  521.     int maxcol = k + half_step;
  522.     int nPossibleContrib = 
  523.       (maxcol - mincol) * (lastrow - firstrow);
  524.     int nContrib = 0;
  525.     for (int irow = firstrow; irow < lastrow; irow++) {
  526.       int fg = first_good[irow];
  527.       int rs = row_start[irow];
  528.       int firstcol = max(0, mincol - fg) + rs;
  529.       int lastcol = min(maxcol, int(first_bad[irow]))+rs-fg;
  530.       unsigned short *zbeg = &z_data[firstcol],
  531. *zend = &z_data[lastcol];
  532.       int _k = mincol;
  533.       for (unsigned short* zz = zbeg; zz < zend; zz++) {
  534. if (*zz) {
  535.   nContrib++;
  536.   if (subSampMode == filterAvg) {
  537.     iAccum += irow;
  538.     kAccum += _k++;
  539.     zAccum += *zz;
  540.   }
  541. }
  542.       }
  543.     }
  545.     // exaggerate distance from full confidence
  546.     float conf = float(nContrib) / float(nPossibleContrib);
  547.     conf = 1.0 - 4.0*(1.0 - conf);
  548.     confidence.push_back (255.0*max(0.0f, conf));
  550.     if (subSampMode == filterAvg) {
  551.       set_xf (iAccum / nContrib, true);
  552.       k = kAccum / nContrib;
  553.       zAccum /= nContrib;
  554.     }
  555.   }
  556.     
  557.   // keep the point
  558.   p[k/step] = pnts.size();
  559.   if (subSampMode == filterAvg)
  560.     pnts.push_back(xf.apply_xform(k, zAccum));
  561.   else
  562.     pnts.push_back(xf.apply_xform(k, z[j]));
  563.   ssmap.push_back(row_start[i]+j);
  564.   if (!g_bNoIntensity)
  565.     intensity.push_back (intensity_data[row_start[i]+j]);
  566. }
  567.       }
  568.     }
  569.   }
  570. }
  573. // return false if fully out of frustum
  574. bool
  575. SDfile::find_row_range(float screw, float radius, const Pnt3 &ctr,
  576.        int rowrange[2])
  577. {
  578.   float rowf = (screw - scan_screw) / frame_pitch + .5*n_frames-.25;
  580.   if (rowf < 0) {
  581.     // check whether the first row intersects sphere
  582.     if (1) {
  583.       // intersect
  584.       rowrange[0] = rowrange[1] = 0;
  585.       return true;
  586.     } else {
  587.       return false;
  588.     }
  589.   }
  591.   if (rowf >= n_frames) {
  592.     // check whether the last row intersects sphere
  593.     if (1) {
  594.       // intersect
  595.       rowrange[0] = rowrange[1] = 0;
  596.       return true;
  597.     } else {
  598.       return false;
  599.     }
  600.   }
  601.   return true;
  602. }
  606. SDfile::SDfile(void)
  607.   : row_start(NULL), first_good(NULL), first_bad(NULL),
  608.     z_data(NULL), intensity_data(NULL), n_valid_pts(0),
  609.     axis_proj_done(false)
  610. {
  612. }
  615. SDfile::~SDfile(void)
  616. { 
  617.   if (row_start)
  618.     delete[] row_start;
  619.   if (first_good)
  620.     delete[] first_good;
  621.   if (first_bad)
  622.     delete[] first_bad;
  623.   if (z_data)
  624.     delete[] z_data;
  625.   if (intensity_data)
  626.     delete[] intensity_data;
  627. }
  630. bool
  631. SDfile::read(const crope &fname)
  632. {
  633.   if(g_verbose) cout << "SDfile::read(" << fname << ")... ";
  634.   //
  635.   // Open the .sd file for reading
  636.   //
  638. #ifdef WIN32
  639.   FILE* sdfile;
  640.   if (!(sdfile = fopen (fname.c_str(), "rb"))) {
  641. #else
  642.   gzFile sdfile;
  643.   if (!(sdfile = gzopen(fname.c_str(), "rb"))) {
  644. #endif
  645.     cerr << "Can't open file " << fname << endl;
  646.     return false;
  647.   }
  649.   //
  650.   // Read and check the magic number
  651.   //
  653.   unsigned int magic_num;
  654.   if (READ_UINT(magic_num)) {
  655.     cerr << "Can't read magic number" << endl;
  656.     return false;
  657.   }
  658.   if (magic_num == 0x444d5001)
  659.     version = 1;
  660.   else if (magic_num == 0x444d5002)
  661.     version = 2;
  662.   else if (magic_num == 0x444d5003)
  663.     version = 3;
  664.   else if (magic_num == 0x444d5004)
  665.     version = 4;
  666.   else if (magic_num == 0x444d5005)
  667.     version = 5;
  668.   else if (magic_num == 0x444d5006)
  669.     version = 6;
  670.   else if (magic_num == 0x444d5007)
  671.     version = 7;
  672.   else  {
  673.     cerr << "Invalid magic number" << endl;
  674.     return false;
  675.   }
  677.   if (version != 7 && version != 6 && version != 5) {
  678.     cerr << "Only versions 5, 6, and 7 of sd supported..." << endl;
  679.     gzclose(sdfile);
  680.     return false;
  681.   }
  682.   
  683.   if(g_verbose) cout << "v" << version << "... " << flush;
  685.   scanner_vert = 0;
  687.   if (version == 7 || version == 6) {
  688.     //
  689.     // Read the rest of the .sd header information
  690.     //
  692.     if (READ_UINT(header_size) ||
  693. READ_UINT(n_frames) ||
  694. READ_UINT(pts_per_frame) ||
  695. READ_UINT(n_pts) ||
  696. READ_FLOAT(frame_pitch) ||
  697. READ_FLOAT(scan_screw) ||
  698. READ_FLOAT(other_screw) ||
  699. READ_FLOAT(scanner_trans)) {
  700.       cerr << "Can't read sd file header" << endl;
  701.       return false;
  702.     }
  704.     if (version == 7)  {
  705.       if (READ_FLOAT(scanner_vert))  {
  706.         cerr << "Can't read sd file header" << endl;
  707.         return false;
  708.       }
  709.     }
  711.     if (READ_UINT(scanner_config) ||
  712. READ_FLOAT(laser_intensity) ||
  713. READ_FLOAT(camera_sensitivity)) {
  714.       cerr << "Can't read sd file header" << endl;
  715.       return false;
  716.     }
  718.     first_good = new unsigned short[n_frames];
  719.     first_bad  = new unsigned short[n_frames];
  720.     z_data     = new unsigned short[n_pts];
  721.     if (!g_bNoIntensity)
  722.       intensity_data = new unsigned char[n_pts];
  724.     //
  725.     // Read the block of start/stop indices
  726.     //
  728.     for (int i=0; i<n_frames; ++i) {
  729.       if (READ_USHORT(first_good[i]) || 
  730.   READ_USHORT(first_bad[i]) ) {
  731. cerr << "Can't read sd file row indices" << endl;
  732. return false;
  733.       }
  734.     }
  736.     //
  737.     // Read the block of range data values
  738.     //
  740.     if (gzread(sdfile, z_data, sizeof(unsigned short) * n_pts) !=
  741. sizeof(unsigned short) * n_pts) { 
  742.       cerr << "Can't read sd file range data values" << endl;
  743.       gzclose(sdfile);
  744.       return false;
  745.     }
  747.     //
  748.     // Read the block of intensity data values
  749.     //
  750.     if (!g_bNoIntensity) {
  751.       if (gzread(sdfile, intensity_data, sizeof(unsigned char) * n_pts) !=
  752.        sizeof(unsigned char) * n_pts) {
  753. cerr << "Can't read sd file intensity data values" << endl;
  754. gzclose(sdfile);
  755. return false;
  756.       }
  757.     }
  758.   } else { // version 5
  759.     //
  760.     // Read the rest of the .sd header information
  761.     //
  763.     if (READ_UINT(n_frames) ||
  764. READ_UINT(pts_per_frame) ||
  765. READ_UINT(n_pts) ||
  766. READ_FLOAT(frame_pitch) ||
  767. READ_FLOAT(scan_screw) ||
  768. READ_FLOAT(other_screw) ||
  769. READ_FLOAT(scanner_trans) ||
  770. READ_UINT(scanner_config)) {
  771.       cerr << "Can't read sd file header" << endl;
  772.       return false;
  773.     }
  775.     //
  776.     // Read data a frame at a time
  777.     //
  779.     first_good = new unsigned short[n_frames];
  780.     first_bad  = new unsigned short[n_frames];
  781.     z_data     = new unsigned short[n_pts];
  783.     int cnt = 0;
  784.     for (int i=0; i<n_frames; ++i) {
  786.       if (READ_USHORT(first_good[i]) || 
  787.   READ_USHORT(first_bad[i]) ) {
  788. cerr << "Can't read sd file row indices" << endl;
  789. return false;
  790.       }
  792.       int toread = first_bad[i] - first_good[i];
  794.       if (gzread(sdfile, &z_data[cnt], sizeof(unsigned short) * toread) !=
  795.        sizeof(unsigned short) * toread)  {
  796. cerr << "Can't read sd file data values" << endl;
  797. gzclose(sdfile);
  798. return false;
  799.       }
  800.       cnt += toread;
  801.     }
  802.   }
  803.   gzclose(sdfile);
  805. #ifdef LITTLE_ENDIAN
  806.     for (int z = 0; z < n_pts; z++)
  807.      FIX_SHORT(z_data[z]);
  808. #endif
  810.   // initialize the CyberXform member variable
  811.   // we would REALLY like to do the following:
  812.   //xf.setup(scanner_config, scanner_trans, other_screw, scanner_vert);
  813.   // but that hoses things that were already registered before
  814.   // scanner_vert was taken into account, so we won't yet.
  815.   xf.setup(scanner_config, scanner_trans, other_screw);
  816.   
  817.   // initialize the row_start array
  818.   row_start  = new unsigned int[n_frames];
  819.   row_start[0] = 0;
  820.   for (int i=1; i<n_frames; ++i) {
  821.     int j = i-1;
  822.     row_start[i] = row_start[j] + first_bad[j] - first_good[j];
  823.   }
  825.   return true;
  826. }
  829. bool
  830. SDfile::write(const crope &fname)
  831. {
  832.   FILE *sdfile;
  833.   unsigned int magic_num =  0x444d5007;
  834.   unsigned int header_size = 52;
  835.   
  836.   if (!(sdfile = fopen(fname.c_str(), "wb")))
  837.     return false;
  839.   //
  840.   // Write the header block
  841.   //
  843.   if (WRITE_UINT(magic_num) ||
  844.       WRITE_UINT(header_size) ||
  845.       WRITE_UINT(n_frames) ||
  846.       WRITE_UINT(pts_per_frame) ||
  847.       WRITE_UINT(n_pts) ||
  848.       WRITE_FLOAT(frame_pitch) ||
  849.       WRITE_FLOAT(scan_screw) ||
  850.       WRITE_FLOAT(other_screw) ||
  851.       WRITE_FLOAT(scanner_trans) ||
  852.       WRITE_FLOAT(scanner_vert) ||
  853.       WRITE_UINT(scanner_config) ||
  854.       WRITE_FLOAT(laser_intensity) ||
  855.       WRITE_FLOAT(camera_sensitivity)) {
  856.     cerr << "Can't write sd file header" << endl;
  857.     fclose(sdfile);
  858.     return false;
  859.   }
  861.   //
  862.   // Write the block of start/stop indices
  863.   //
  864.   
  865.   for (int i=0; i<n_frames; ++i) {
  866.     if (WRITE_USHORT(first_good[i]) ||
  867. WRITE_USHORT(first_bad[i])) {
  868.       cerr << "Can't write sd file row indices" << endl;
  869.       fclose(sdfile);
  870.       return false;
  871.     }
  872.   }
  874.   //
  875.   // Write the block of range data values
  876.   //
  878. #ifdef LITTLE_ENDIAN
  879.   unsigned short *fixed_z = new unsigned short[n_pts];
  880.   for (int z = 0; z < n_pts; z++)
  881.     fixed_z[z] = swap_short(z_data[z]);
  883.   if (fwrite(fixed_z, sizeof(unsigned short), n_pts, sdfile) != n_pts)  {
  884.     delete [] fixed_z;
  885.     cerr << "Can't write sd file range data values" << endl;
  886.     fclose(sdfile);
  887.     return false;
  888.   }
  889.   delete [] fixed_z;
  890. #else
  891.   if (fwrite(z_data, sizeof(unsigned short), n_pts, sdfile) != n_pts)  {
  892.     cerr << "Can't write sd file range data values" << endl;
  893.     fclose(sdfile);
  894.     return false;
  895.   }
  897. #endif
  899.   //
  900.   // Write the block of intensity data values
  901.   //
  902.   
  903.   if (g_bNoIntensity) {
  904.     cerr << "No intensity data in memory, writing zeros..." << endl;
  905.     char c = 0;
  906.     for (i=0; i<n_pts; i++) {
  907.       fwrite(&c, sizeof(unsigned char), 1, sdfile);
  908.     }
  909.   } else {
  910.     if (fwrite(intensity_data, sizeof(unsigned char), n_pts, sdfile) !=
  911. n_pts) {
  912.       cerr << "Can't write sd file intensity data values" << endl;
  913.       fclose(sdfile);
  914.       return false;
  915.     }
  916.   }
  918.   fclose(sdfile);
  919.   return true;
  920. }
  922. // Fill single-sample holes in .sd files
  923. void
  924. SDfile::fill_holes(int max_missing, int thresh)
  925. {
  926.   //Median<unsigned short> m;
  927.   Median<float> m;
  928.   for (int row = 0; row < n_frames; row++) {
  929.     int numcols = first_bad[row]-first_good[row];
  930.     if (!numcols)
  931.       continue;
  932.     for (int col = max_missing; col < numcols-max_missing; col++) {
  933.       int index = row_start[row]+col;
  934.       unsigned short me = z_data[index];
  935.       unsigned short med;
  936.       if (max_missing == 1) {
  937. med = __median(z_data[index-1],
  938.        me,
  939.        z_data[index+1]);
  940. #if 1
  941.       } else if (max_missing == 2) {
  942. // Fast approx. to median of 5
  943. med = __median(z_data[index-1],
  944.        me,
  945.        z_data[index+1]);
  946. med = __median(z_data[index-2],
  947.        med,
  948.        z_data[index+2]);
  949. #endif
  950.       } else {
  951. // Slow general-case code
  952. for (int i = index-max_missing; i <= index+max_missing; i++)
  953.   m += z_data[i];
  954. med = m.find();
  955. m.zap();
  956.       }
  957.       if (med && (abs(med - me) > thresh)) {
  958. int tot = 0;
  959. int num = 0;
  960. for (int i = index-max_missing; i <= index+max_missing; i++)
  961.   if (abs(z_data[i] - med) < thresh) {
  962.     tot += z_data[i];
  963.     num++;
  964.   }
  965. if (num)
  966.   z_data[index] = tot / num;
  967.       }
  968.     }
  969.   }
  970. }
  973. void
  974. SDfile::make_tstrip(vector<int>  &tstrips,
  975.     vector<char> &bdry)
  976. {
  977.   if (fabs(frame_pitch) > .07) 
  978.     make_tstrip_horizontal(tstrips, bdry);
  979.   else 
  980.     make_tstrip_vertical(tstrips, bdry);
  981. }
  984. void
  985. SDfile::subsampled_tstrip(int            step,
  986.   char          *behavior,
  987.   vector<int>   &tstrips,
  988.   vector<char>  &bdry,
  989.   vector<Pnt3>  &pnts,
  990.   vector<uchar> &intensity,
  991.   vector<uchar> &confidence,
  992.   vector<int>   &ssmap)
  993. {
  994.   int n_s = pts_per_frame / step;
  995.   int end = 2*n_frames/step;
  997.   SubSampMode subSampMode = !strcmp (behavior, "holes") 
  998.     ? holesGrow
  999.     : !strcmp (behavior, "conf") ? conf
  1000.     : !strcmp (behavior, "filter") ? filterAvg
  1001.     : fast;
  1003.   if (n_s > 1 && end > 1) {
  1004.     // do horizontal stripping
  1005.     
  1006.     vector<int> p_inds[2], old_inds(n_s);
  1007.     p_inds[0].reserve(n_s); 
  1008.     p_inds[0].insert(p_inds[0].begin(), n_s, -1);
  1009.     p_inds[1].reserve(n_s); 
  1010.     p_inds[1].insert(p_inds[1].begin(), n_s, -1);
  1011.     
  1012.     vector<char> tmp_bdry(end*n_s); // a temporary bdry vector
  1013.     // first row
  1014.     fill_index_array(step, 0, subSampMode, p_inds[0],
  1015.      pnts, ssmap, intensity, confidence);
  1016.     mark_all_empty(p_inds[0], tmp_bdry);
  1017.     bool flip = (frame_pitch > 0.0);
  1018.     bool vscan = scanner_config & 0x00000001;
  1019.     if (!vscan) flip = !flip;
  1020.     // second row and first strip
  1021.     fill_index_array(step, 1, subSampMode, p_inds[1],
  1022.      pnts, ssmap, intensity, confidence);
  1023.     create_strip(tstrips, p_inds[0], p_inds[1],
  1024.  true, flip);
  1025.     // rest of the rows
  1026.     int i,j;
  1027.     for (i=2,j=0; i<end; i++, j^=1) {
  1028.       copy(p_inds[j].begin(), p_inds[j].end(),old_inds.begin());
  1029.       fill(p_inds[j].begin(), p_inds[j].end(), -1);
  1030.       fill_index_array(step, i, subSampMode, p_inds[j],
  1031.        pnts, ssmap, intensity, confidence);
  1032.       mark_maybe_empty(p_inds[!j], old_inds, p_inds[j], tmp_bdry);
  1033.       create_strip(tstrips, p_inds[!j], p_inds[j],
  1034.    true, flip);
  1035.     }
  1036.     mark_all_empty(p_inds[!j], tmp_bdry);
  1037.     // copy the boundary info
  1038.     copy(tmp_bdry.begin(), &tmp_bdry[pnts.size()],
  1039.  back_insert_iterator<vector<char> > (bdry));
  1040.   }
  1041. }
  1044. void 
  1045. SDfile::set_xf(int row, bool both)
  1046. {
  1047.   if (both) {
  1048.     // get the average screw length for the given row
  1049.     float scr = scan_screw + (row - .5*n_frames+.5) * frame_pitch;
  1050.     // the even field is a quarter pitch down, odd field up
  1051.     xf.set_screw(scr - 0.25*frame_pitch, scr + 0.25*frame_pitch);
  1052.   } else {
  1053.     // only even screw
  1054.     float scr = scan_screw + (row -.5*n_frames+.25) * frame_pitch;
  1055.     xf.set_screw(scr);
  1056.   }
  1057. }
  1060. void
  1061. SDfile::get_pnts_and_intensities(vector<Pnt3>  &pnts,
  1062.  vector<uchar> &intensity)
  1063. {
  1064.   int cnt = 0;
  1065.   for (int i = 0; i < n_frames; ++i) {
  1066.     set_xf(i);
  1067.     for (int j=first_good[i]; j<first_bad[i]; j++, cnt++)  {
  1068.       if (z_data[cnt] == 0)
  1069. continue; // missing data marked by zero
  1070.       pnts.push_back(xf.apply_xform(j, z_data[cnt]));
  1071.       if (!g_bNoIntensity) 
  1072. intensity.push_back (intensity_data[cnt]);
  1073.     }
  1074.   }
  1075. }
  1079. // access a data point, transformed or in laser coords
  1080. // row is the frame, col is a point on scanline,
  1081. // col even: frame 0; col odd: frame 1
  1082. // returns false if there is no such point
  1083. bool
  1084. SDfile::get_point(int row, int col, Pnt3 &p, 
  1085.   bool transformed)
  1086. {
  1087.   // row within range?
  1088.   if (row < 0 || row >= n_frames) 
  1089.     return false;
  1090.   // column within range?
  1091.   if (first_good[row] > col || first_bad[row] <= col)
  1092.     return false;
  1093.   // valid point?
  1094.   short z = z_data[row_start[row] + col - first_good[row]];
  1095.   if (z == 0) return false;
  1096.   if (transformed) {
  1097.     set_xf(row);  // should check whether this already is valid
  1098.     p = xf.apply_xform(col, z);
  1099.   } else {
  1100.     p = xf.apply_raw_to_laser(col, z);
  1101.   }
  1102.   return true;
  1103. }
  1106. // Calculate the laser direction at the middle frame,
  1107. // find also the rotation axis, and create a frame such
  1108. // that the laser direction is aligned with negative z
  1109. // and the scanning axis is aligned with x (pos or neg irrelevant)
  1110. Xform<float>
  1111. SDfile::vrip_reorientation_frame(void)
  1112. {
  1113.   set_xf(n_frames/2);
  1114.   
  1115.   // get the laser direction
  1116.   short mid_column = 480/2;
  1117.   short front = 60;
  1118.   short back  = 2940;
  1119.   Pnt3 p1 = xf.apply_xform(mid_column, front);
  1120.   Pnt3 p2 = xf.apply_xform(mid_column, back);
  1121.   Pnt3 laser_dir = (p2 - p1).normalize();
  1123.   // get the center point
  1124.   short mid_z = (front + back) / 2;
  1125.   Pnt3 ctr = xf.apply_xform(mid_column, mid_z);
  1127.   // assume that a constant z-row is aligned in 3D with
  1128.   // scan axis
  1129.   p1 = xf.apply_xform(  0, mid_z);
  1130.   p2 = xf.apply_xform(480, mid_z);
  1131.   Pnt3 new_x = (p2-p1).normalize();
  1133.   // create the frame
  1134.   Pnt3 new_z = laser_dir;
  1135.   Pnt3 new_y = cross(new_z, new_x);
  1136.   new_x = cross(new_y, new_z);
  1138.   Xform<float> a,b;
  1139.   a.translate(-ctr);
  1140.   b.fromFrame(new_x, new_y, new_z, Pnt3());
  1141.   return b * a;
  1142. }
  1145. // given a screw length and a position on the scanline
  1146. // find the closest data point in the sweep
  1147. // return false if that data point is invalid
  1148. // 
  1149. // a preliminary version, should be optimized and 
  1150. // the logic should be improved...
  1151. bool
  1152. SDfile::find_data(float screw, float y_in, 
  1153.   int            &row,
  1154.   unsigned short &y, 
  1155.   unsigned short &z)
  1156. {
  1157.   if (y_in < 0.0) { 
  1158.     //SHOW(y_in);
  1159.     return false;
  1160.   }
  1161.   // first, calculate row
  1162.   // invert the equation in set_xf()
  1163.   // treat the screw input as the "average screw of the frame"
  1164.   // change it to the even field screw...
  1165.   float rowf = (screw - scan_screw) / frame_pitch + .5*n_frames-.25;
  1167.   //SHOW(screw / frame_pitch);
  1168.   //SHOW(25*frame_pitch);
  1169.   //rowf -= 25;
  1171.   float rowfloor = floor(row);
  1172.   float frac = row - rowfloor;
  1173.   // if frac: [0.0,.25] -> even field
  1174.   //          (.25,.75] -> odd field
  1175.   //          (.75,1.0) -> next row even field
  1176.   bool field_even;
  1177.   if (frac > .75) {
  1178.     row = int(rowf) + 1;
  1179.     field_even = true;
  1180.   } else {
  1181.     row = int(rowf);
  1182.     field_even = (frac <= .25);
  1183.   }
  1184.   if (row < 0 || row >= n_frames) 
  1185.     return false;
  1186.   // if even field, snap to closest even integer,
  1187.   // similarly for odd
  1188.   if (field_even) {
  1189.     y = int(floor(y_in*.5+.5)) * 2;
  1190.   } else {
  1191.     y = int(floor(y_in*.5)) * 2 + 1;
  1192.   }
  1193.   if (y < first_good[row] || y >= first_bad[row]) {
  1194.     //SHOW(row);
  1195.     //SHOW(y);
  1196.     //SHOW(first_good[row]);
  1197.     //SHOW(first_bad[row]);
  1198.     return false;
  1199.   }
  1200.   z = z_data[row_start[row]+y-first_good[row]];
  1201.   return (z != 0);  
  1202. }
  1205. int
  1206. SDfile::count_valid_pnts(void)
  1207. {
  1208.   if (n_valid_pts == 0) {
  1209.     unsigned short *end = z_data+n_pts;
  1210.     for (unsigned short *sp = z_data; sp != end; sp++) {
  1211.       if (*sp) n_valid_pts++;
  1212.     }
  1213.   }
  1214.   return n_valid_pts;
  1215. }
  1218. void
  1219. SDfile::filtered_copy(const VertexFilter &filter,
  1220.       SDfile &newsd)
  1221. {
  1222.   // copy the data, first shallow copy
  1223.   newsd = *this;
  1224.   // then do a deeper, filtered copy
  1225.   newsd.row_start  = new unsigned int[n_frames];
  1226.   newsd.first_good = new unsigned short[n_frames];
  1227.   newsd.first_bad  = new unsigned short[n_frames];
  1228.   
  1229.   // initialize these to null
  1230.   newsd.intensity_data = NULL;
  1231.   newsd.z_data = NULL;
  1233.   // for z_data and intensity data, we need temporary
  1234.   // buffers, we don't know how big they'll be
  1235.   unsigned short *_z_data = new unsigned short[n_pts];
  1236.   unsigned char  *_bw_data = new unsigned char[n_pts];
  1237.   int cnt = 0;
  1239.   // march over all the points, apply transformations,
  1240.   // test for filtering, update the arrays above
  1241.   for (int i=0; i<n_frames; i++) {
  1243.     // initialize
  1244.     newsd.row_start[i] = cnt;
  1245.     int  FirstGood, LastGood;
  1246.     bool empty_row = true;
  1247.     newsd.set_xf(i);
  1249.     // check all valid ones of a row...
  1250.     for (int j=first_good[i]; j<first_bad[i]; j++) {
  1252.       int k = row_start[i] + j - first_good[i];
  1254.       // valid data, filter it
  1255.       if (z_data[k] != 0 &&
  1256.   filter.accept(newsd.xf.apply_xform(j, z_data[k]))) {
  1257. // accepted
  1258. if (empty_row) { FirstGood = j; empty_row = false; }
  1259. LastGood = j;
  1260. _z_data[cnt]    = z_data[k];
  1261. if (!g_bNoIntensity) 
  1262.   _bw_data[cnt] = intensity_data[k];
  1263. cnt++;
  1264.       } else if (!empty_row) {
  1265. // rejected
  1266. _z_data[cnt]    = 0;
  1267. _bw_data[cnt++] = 0;
  1268.       }
  1269.     }
  1271.     if (empty_row) {
  1272.       // row has no data
  1273.       newsd.first_good[i] = 0;
  1274.       newsd.first_bad[i]  = 0;
  1275.     } else {
  1276.       // row has filtered data
  1277.       newsd.first_good[i] = FirstGood;
  1278.       newsd.first_bad[i]  = LastGood+1;
  1279.       // remove the last bad entries from _z_data and _bw_data
  1280.       cnt -= (first_bad[i] - newsd.first_bad[i]);
  1281.     }
  1282.   }
  1284.   // copy the z and intensity data to the new copy
  1285.   newsd.n_pts  = cnt;
  1286.   if (cnt) {
  1287.     newsd.z_data = new unsigned short[cnt];
  1288.     memcpy(newsd.z_data, _z_data, sizeof(unsigned short) * cnt);
  1289.     if (!g_bNoIntensity) {
  1290.       newsd.intensity_data = new unsigned char[cnt];
  1291.       memcpy(newsd.intensity_data, _bw_data, cnt);
  1292.     }
  1293.   }
  1294.   delete[] _z_data;
  1295.   delete[] _bw_data;
  1296. }
  1299. void
  1300. SDfile::get_piece(int firstFrame, int lastFrame,
  1301.   SDfile &newsd)
  1302. {
  1303.   // copy the data, first shallow copy
  1304.   newsd = *this;
  1306.   newsd.n_frames = lastFrame - firstFrame + 1;
  1307.   // then do a deeper, filtered copy
  1308.   newsd.row_start  = new unsigned int[newsd.n_frames];
  1309.   newsd.first_good = new unsigned short[newsd.n_frames];
  1310.   newsd.first_bad  = new unsigned short[newsd.n_frames];
  1312.   newsd.scan_screw = scan_screw + 
  1313.     (firstFrame + (int(newsd.n_frames) - int(n_frames))*0.5)*frame_pitch;
  1315.   int offset = row_start[firstFrame];
  1316.   for (int i = firstFrame; i <= lastFrame; i++) {
  1317.     newsd.row_start[i-firstFrame]  = row_start[i] - offset;
  1318.     newsd.first_good[i-firstFrame] = first_good[i];
  1319.     newsd.first_bad[i-firstFrame]  = first_bad[i];
  1320.   }
  1322.   newsd.n_pts = newsd.row_start[newsd.n_frames-1] + 
  1323.     newsd.first_bad[newsd.n_frames-1] - 
  1324.     newsd.first_good[newsd.n_frames-1];
  1326.   if (newsd.n_pts) {
  1327.     // copy the z and intensity data to the new copy
  1328.     newsd.z_data = new unsigned short[newsd.n_pts];
  1329.     memcpy(newsd.z_data, z_data + row_start[firstFrame], 
  1330.    sizeof(unsigned short) * newsd.n_pts);
  1331.     if (!g_bNoIntensity) {
  1332.       newsd.intensity_data = new unsigned char[newsd.n_pts];
  1333.       memcpy(newsd.intensity_data,
  1334.      intensity_data+row_start[firstFrame], 
  1335.      newsd.n_pts);
  1336.     }
  1337.   }
  1338. }
  1341. Bbox
  1342. SDfile::get_bbox(void)
  1343. {
  1344.   int cnt = 0;
  1345.   Bbox bb;
  1346.   for (int i = 0; i < n_frames; ++i) {
  1347.     set_xf(i);
  1348.     for (int j=first_good[i]; j<first_bad[i]; j++, cnt++)  {
  1349.       if (z_data[cnt] == 0)
  1350. continue; // missing data marked by zero
  1351.       bb.add(xf.apply_xform(j, z_data[cnt]));
  1352.     }
  1353.   }
  1354.   return bb;
  1355. }
  1358. static int inside = 0;
  1359. static int outside = 0;
  1360. static int status_cnt = 0;
  1361. int
  1362. SDfile::sphere_status(const Pnt3 &ctr, float r)
  1363. {
  1364.   float screw, swin[2];
  1365.   short ywin[2], zwin[2];
  1366.   prepare_axis_proj();
  1367.   int ans = xf.sphere_status(ctr, r, screw, 
  1368.      axis_proj_min, axis_proj_max,
  1369.      ywin, zwin, swin);
  1370.   
  1371.   status_cnt++;
  1373.   if (ans == 0) {
  1374.     return 4; // NOT_IN_FRUSTUM
  1375.   }
  1377.   if (ans == 1) {
  1378.     return 1; // BOUNDARY;
  1379.   }
  1381.   cout << ywin[0] << " " 
  1382.        << ywin[1] << " " 
  1383.        << zwin[0] << " " 
  1384.        << zwin[1] << " " 
  1385.        << swin[0] << " " 
  1386.        << swin[1] << " " << r << endl;
  1387.   
  1388.   int start_row = (swin[0] - scan_screw) / frame_pitch + .5*n_frames-.25;
  1389.   int end_row   = (swin[1] - scan_screw) / frame_pitch + .5*n_frames-.25;
  1391.   if (end_row < 0 || start_row >= n_frames) {
  1392.     return 4; // NOT_IN_FRUSTUM
  1393.   }
  1394.   if (start_row < 0 || end_row >= n_frames) {
  1395.     // partially within frustum
  1396.     return 1; // BOUNDARY (force subdivision)
  1397.   }
  1399.   // really check the status
  1401.   bool all_behind = true;
  1402.   bool all_front  = true;
  1403.   bool all_within = true;
  1405.   SHOW(start_row);
  1406.   SHOW(end_row);
  1408.   int span = ywin[1] - ywin[0] + 1;
  1409.   for (int i=start_row; i<=end_row; i++) {
  1410.     if (first_good[i] >  ywin[0]) return 1; // invalid, force subdiv
  1411.     if (first_bad[i]  <= ywin[1]) return 1; // invalid, force subdiv
  1412.     unsigned short *z = &z_data[row_start[i] + ywin[0] - first_good[i]];
  1413.     for (int j=0; j<span; j++) {
  1414.       if      (z[j] < zwin[0]) { all_front  = all_within = false; }
  1415.       else if (z[j] > zwin[1]) { all_behind = all_within = false; }
  1416.       else                     { all_front  = all_behind = false; }
  1417.     }
  1418.     if (!all_behind && !all_front && !all_within)
  1419.       break;
  1420.   }
  1422.   if (all_behind) { cout << "OUTSIDE" << ++outside << endl; }
  1423.   if (all_front)  { cout << "INSIDE" << ++inside << endl; }
  1425.   if (all_within) return 1; // BOUNDARY
  1426.   if (all_behind) return 0; // OUTSIDE
  1427.   if (all_front)  return 3; // INSIDE
  1428.   return 2; // SILHOETTE
  1429. }
  1432. int
  1433. SDfile::dump_pts_laser_subsampled(ofstream &out, int nPnts)
  1434. {
  1435.   // go through all the valid points, do a uniform
  1436.   // subsampling
  1437.   int valids_left = n_valid_pts;
  1438.   int still_need  = nPnts;
  1439.   Random rnd;
  1440.   int cnt = 0;
  1442.   for (int i=0; i<n_frames; i++) {
  1444.     // see set_xf()
  1445.     float scr = scan_screw + (i - .5*n_frames+.5) * frame_pitch;
  1446.     float evenscr = scr - 0.25*frame_pitch;
  1447.     float oddscr  = scr + 0.25*frame_pitch;
  1448.     set_xf(i);
  1450.     // check all valid ones of a row...
  1451.     for (int j=first_good[i]; j<first_bad[i]; j++) {
  1453.       int k = row_start[i] + j - first_good[i];
  1455.       // valid data, filter it
  1456.       if (z_data[k]) {
  1458. if (rnd() < float(still_need)/float(valids_left)) {
  1459.   cnt++;
  1460.   still_need--;
  1461.   // transform to laser coords
  1462.   Pnt3 p(0, j, z_data[k]);
  1463.   xf.raw_to_laser(p);
  1464.   assert(fabs(p[0]) < 1e-5);
  1465.   out.write((char*)&p[1], sizeof(float));
  1466.   out.write((char*)&p[2], sizeof(float));
  1467.   if (j%2) out.write((char*)&oddscr,  sizeof(float));
  1468.   else     out.write((char*)&evenscr, sizeof(float));
  1469.   out.write((char*)&other_screw, sizeof(float));
  1470.   out.write((char*)&scanner_trans, sizeof(float));
  1471.   /*
  1472.   // DELETE FROM HERE
  1473.   if (j%2) SHOW(oddscr);
  1474.   else     SHOW(evenscr);
  1475.   SHOW(other_screw);
  1476.   Pnt3 pp(0, j, z_data[k]);
  1477.   SHOW(pp);
  1478.   xf.raw_to_laser(pp);
  1479.   SHOW(pp);
  1480.   xf.laser_to_scanaxis(pp);
  1481.   SHOW(pp);
  1482.   SHOW(xf.apply_xform(j, z_data[k]));
  1483.   return cnt;
  1484.   // DELETE UP TO HERE
  1485.   */
  1486. }
  1487. valids_left--;
  1488.       }
  1489.     }
  1490.   }
  1491.   return cnt;
  1492. }
  1495. Pnt3
  1496. SDfile::raw_for_ith(int   j, sd_raw_pnt &data)
  1497. {
  1498.   assert(j < n_pts);
  1500.   // find the row
  1501.   int bot = 0;
  1502.   int top = n_frames;
  1503.   while (top - bot > 1) {
  1504.     int i = (bot + top) / 2;
  1505.     if (row_start[i] > j) top = i;
  1506.     else                  bot = i;
  1507.   }
  1508.   int row = bot;
  1510.   // find the column
  1511.   int col = j - row_start[row] + first_good[row];
  1512.   
  1513.   data.y = col;
  1514.   data.z = z_data[j];
  1515.   data.config = scanner_config;
  1516.   float scr = scan_screw + (row - .5*n_frames+.5) * frame_pitch;
  1517.   if (j%2) scr += .25*frame_pitch;
  1518.   else     scr -= .25*frame_pitch;
  1519.   if (data.config & 0x00000001) {
  1520.     // vertical
  1521.     data.nod  = scr;
  1522.     data.turn = other_screw;
  1523.   } else {
  1524.     // horizontal
  1525.     data.turn = scr;
  1526.     data.nod  = other_screw;
  1527.   }
  1528.   data.tr_h = scanner_trans;
  1529.   // now, convert the raw to 3D for verification
  1530.   set_xf(row);
  1531.   return xf.apply_xform(data.y,data.z);
  1532. }
  1535. Pnt3
  1536. SDfile::raw_for_ith_valid(int   i, sd_raw_pnt &data)
  1537. {
  1538.   assert(i < n_valid_pts);
  1540.   // first, find the index for the ith valid point
  1541.   unsigned short *z = z_data;
  1542.   i++;
  1543.   while (i) { if (*(z++)) i--; }
  1544.   z--;
  1546.   return raw_for_ith(int(z-z_data), data);
  1547. }