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

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

3D图形编程

开发平台：

Visual C++

GlobalReg.cc：源码内容
  1. //############################################################
  2. // GlobalReg.cc
  3. // Kari Pulli
  4. // Tue Jun 23 11:54:00 PDT 1998
  5. // 
  6. // Global registration of several scans simultaneously
  7. //############################################################
  9. #include <iostream.h>
  10. #include <fstream.h>
  11. #include <stdlib.h>
  12. #include <multimap.h>
  13. #include <deque.h>
  14. #include <strstream.h>
  15. #include <sys/stat.h>
  16. #include "GlobalReg.h"
  17. #include "absorient.h"
  18. #include "TbObj.h"
  19. #include "DisplayMesh.h"
  20. #include "plvScene.h"
  21. #include "ConnComp.h"
  22. #include "Progress.h"
  23. #include "BailDetector.h"
  24. #include "DirEntries.h"
  25. #include "FileNameUtils.h"
  26. #include "ScanFactory.h"
  27. #include "CyberScan.h"
  28. #include "GroupScan.h"
  29. #include "TclCmdUtils.h"
  30. #include <math.h>
  32. // TODO: make these sliders in globalreg window
  33. #define VB_SIZE (100.0)
  34. #define NOT_FOUND (-1)
  35. #define THRESHOLD (700) // THIS SHOULD BE MUCH LARGER probably
  37. #if defined(WIN32) || defined(i386)
  38. #define LITTLE_ENDIAN
  39. #endif
  41. // setup file loading routines for a little endian machine
  42. #ifdef LITTLE_ENDIAN
  44. inline short swap_short(short x)
  45. {
  46.   return ((x & 0xff) << 8) | ((unsigned short)x >> 8);
  47. }
  49. /* unsigned is essential, otherwise bitshift will 
  50.  * sign extend and mess up the conversion.
  51.  */
  52. inline int swap_int(unsigned int x)
  53. {
  54.   return (x << 24) |
  55.          ((x << 8) & 0x00ff0000) | 
  56.  ((x >> 8) & 0x0000ff00) | 
  57.  (x >> 24);
  58. }
  60. inline float swap_float(float x)
  61. {
  62.   unsigned int y = (*((unsigned int *)&x));
  63.   y= (y << 24) |
  64.      ((y << 8) & 0x00ff0000) | 
  65.      ((y >> 8) & 0x0000ff00) | 
  66.       (y >> 24);
  67.   return (*((float *)&y));
  68. }
  70. inline short FixShort(short x)
  71. {
  72.   return (swap_short(x));
  73. }
  75. inline int FixInt(int x)
  76. {
  77.   return (swap_int(x));
  78. }
  80. inline float FixFloat(float x)
  81. {
  82.   return (swap_float(x));
  83. }
  84.  
  85. // otherwise set up file loading routines for a big endian machine
  86. #else
  88. inline short FixShort(short x)
  89. {
  90.   return (x);
  91. }
  93. inline int FixInt(int x)
  94. {
  95.   return (x);
  96. }
  98. inline float FixFloat(float x)
  99. {
  100.   return (x);
  101. }
  103. #endif
  105. // File writing routines
  106. inline void WriteInt(int x, ofstream &out)
  107. {
  108.   int temp = FixInt(x);
  109.   out.write((char*)&temp, 4);
  110. }
  112. inline void WriteFloat(float x, ofstream &out)
  113. {
  114.   float temp = FixFloat(x);
  115.   out.write((char*)&temp, 4);
  116. }
  118. inline void WriteShort(short x, ofstream &out)
  119. {
  120.   short temp = FixShort(x);
  121.   out.write((char*)&temp, 2);
  122. }
  124. /* There is no ReadChar since no special byte ordering is required for characters */
  126. inline int ReadInt(ifstream &in)
  127. {
  128.   int temp;
  129.   in.read((char*)&temp, 4);
  130.   return FixInt(temp);
  131. }
  133. inline short ReadShort(ifstream &in)
  134. {
  135.   short temp;
  136.   in.read((char*)&temp, 2);
  137.   return FixShort(temp);
  138. }
  140. inline float ReadFloat(ifstream &in)
  141. {
  142.   float temp;
  143.   in.read((char*)&temp, 4);
  144.   return FixFloat(temp);
  145. }
  147. inline void WriteSDRawPoints(vector<sd_raw_pnt> &rp, int n, ofstream &out)
  148. {
  149.   for (int i=0; i<n; i++) {
  150.     WriteInt(rp[i].config, out);
  151.     
  152.     WriteShort(rp[i].y, out); 
  153.     WriteShort(rp[i].z, out);
  154.     
  155.     WriteFloat(rp[i].nod, out);
  156.     WriteFloat(rp[i].turn, out);
  157.     WriteFloat(rp[i].tr_h, out);
  158.   }
  159. }
  161. inline void WritePnt3(vector<Pnt3> &pnt, int n, ofstream &out)
  162. {
  163.   for (int i=0; i<n; i++) {
  164.     Pnt3 p = pnt[i];
  165.     
  166.     WriteFloat(*(float *)p, out);
  167.     WriteFloat(*((float *)p + 1), out);
  168.     WriteFloat(*((float *)p + 2), out);
  169.   }
  170. }
  172. inline void ReadPnt3(vector<Pnt3> &pnt, int n, ifstream &in)
  173. {
  174.   Pnt3 tempPnt;
  176.   for (int i=0; i<n; i++) {
  177.     float f1 = ReadFloat(in);
  178.     float f2 = ReadFloat(in);
  179.     float f3 = ReadFloat(in);
  181.     tempPnt.set(f1, f2, f3);
  183.     pnt[i] = tempPnt;
  184.   }
  185. }
  187. #define GR_MESHNAMES_SEP "^^^"
  189. #define ECHO(x) cout << #x << endl; x
  191. #define FOR_MAP_ENTRIES(key, data) 
  192.   for (ITT __m = hmm.begin(); __m != hmm.end(); __m++) {
  193.     TbObj *key     = (*__m).first;
  194.     mapEntry *data = (*__m).second;
  195. #define END_FOR_MAP }
  197. #define FOR_MATCHING_KEYS(key, data) 
  198.   { pair<ITT, ITT> __p = hmm.equal_range(key);
  199.     for (ITT __i = __p.first; __i!=__p.second; __i++) {
  200.       mapEntry *data = (*__i).second;
  201. #define END_FOR_KEYS }}
  203. #define CONTAINS(vec, item) 
  204.  (find(vec.begin(),vec.end(),item) != vec.end())
  206. // this is ugly: it's linear time, which makes
  207. // processing scans n^2 (though we have small n...)
  208. inline int
  209. scan_idx(const TbObj* a, const vector<TbObj*> &scan)
  210. {
  211.   for (int i=scan.size()-1; i>=0; i--) 
  212.     if (scan[i] == a) return i;
  213.   assert(0);
  214.   return 0;
  215. }
  218. inline float
  219. dist2(const vector<Pnt3> &P, const vector<Pnt3> &Q)
  220. {
  221.   float d = 0.0;
  222.   vector<Pnt3>::const_iterator ip = P.begin();
  223.   vector<Pnt3>::const_iterator iq = Q.begin();
  224.   while (ip != P.end()) {
  225.     d += dist2(*ip, *iq);
  226.     ip++; iq++;
  227.   }
  228.   assert(iq == Q.end());
  229.   return d;
  230. }
  233. static RigidScan*
  234. createProxyScanNamed (const crope& name)
  235. {
  236.   if (strcmp (Tcl_GetVar (g_tclInterp, "allowProxiesWithGR",
  237.   TCL_GLOBAL_ONLY), "0") == 0) {
  238.     return NULL;
  239.   }
  241.   // make sure we can find the scan, else it's pointless to try to
  242.   // globalreg it because we won't be able to use the results.
  243.   Tcl_VarEval (g_tclInterp, "file_locateScan ", name.c_str(), NULL);
  244.   char* filename = g_tclInterp->result;
  245.   if (!filename[0]) {
  246.     cerr << "Unable to locate scan for " << name << endl;
  247.     return NULL;
  248.   }
  249.   
  250.   // create invisible scan with 0 bbox
  251.   RigidScan* rs = CreateScanFromBbox (filename, Pnt3(), Pnt3());
  252.   DisplayableMesh* dm = theScene->addMeshSet (rs);
  254.   //cerr << "creating proxy named " << name << " as "
  255.   //   << dm->getName() << endl;
  256.   
  257.   Tcl_VarEval (g_tclInterp, "addMeshToWindow ",
  258.        dm->getName(), " 0", NULL);
  259.   Tcl_VarEval (g_tclInterp, "changeVis ", dm->getName(), " 0", NULL);
  261.   return rs;
  262. }
  265. static void
  266. calcPairwiseRmsErr(const vector<Pnt3> &ptsa,
  267.    const vector<Pnt3> &nrma,
  268.    const vector<Pnt3> &ptsb,
  269.    const vector<Pnt3> &nrmb,
  270.    const Xform<float> &rel_xf,
  271.    float &pw_point_rmsErr,
  272.    float &pw_plane_rmsErr)
  273. {
  274.   assert(ptsa.size());
  276.   // calculate the rms for point pair distances
  277.   pw_point_rmsErr = pw_plane_rmsErr = 0.0;
  278.   Pnt3  p,nrm;
  279.   int   n = ptsa.size();
  280.   bool  do_tangents = (nrma.size() != 0);
  281.   float d;
  282.   for (int i=0; i<n; i++) {
  283.     rel_xf.apply(ptsa[i], p);
  284.     pw_point_rmsErr += dist2(p, ptsb[i]);
  286.     if (do_tangents) {
  287.       rel_xf.apply_nrm(nrma[i], nrm);
  288.       p -= ptsb[i];
  289.       d = dot(nrm, p);
  290.       pw_plane_rmsErr += d*d;
  291.       d = dot(nrmb[i], p);
  292.       pw_plane_rmsErr += d*d;
  293.     }
  294.   }
  295.   pw_point_rmsErr = sqrtf(pw_point_rmsErr / n);
  296.   pw_plane_rmsErr = sqrtf(pw_plane_rmsErr / (2*n));
  297. }
  300. void 
  301. GlobalReg::mapEntry::stats(void)
  302. {
  303.   Xform<float> xf_a = xfa->getXform();
  304.   Xform<float> xf_b = xfb->getXform();
  306.   int n = ptsa.size();
  307.   Pnt3 first, second, nrm;
  308.   maxErr = avgErr = rmsErr = 0.0;
  309.   
  310.   Xform<float> actual_rel_xf = xf_b;
  311.   actual_rel_xf.fast_invert();
  312.   actual_rel_xf = actual_rel_xf * xf_a;
  313.   
  314.   for (int j=0; j<n; j++) {
  315.     // calculate the point wise distances
  316.     first  = ptsa[j];
  317.     second = ptsa[j];
  318.     actual_rel_xf (first);
  319.     rel_xf (second);
  320.     
  321.     float d = dist2(first, second);
  322.     if (d > maxErr) maxErr = d;
  323.     rmsErr += d;
  324.     avgErr += sqrtf(d);
  325.   }
  326.   for (j=0; j<n; j++) {
  327.     // calculate the point wise distances
  328.     first  = ptsb[j];
  329.     second = ptsb[j];
  330.     actual_rel_xf.apply_inv(first,first);
  331.     rel_xf.apply_inv(second,second);
  332.     
  333.     float d = dist2(first, second);
  334.     if (d > maxErr) maxErr = d;
  335.     rmsErr += d;
  336.     avgErr += sqrtf(d);
  337.   }
  338.   n *= 2;
  340.   avgErr /= n;
  341.   rmsErr = sqrtf(rmsErr/n);
  342. }
  345. static bool 
  346. write_points(ofstream &out, vector<Pnt3> &pts, TbObj *scan)
  347. {
  348.   CyberScan* cs = dynamic_cast<CyberScan*> (scan);
  350.   if (cs && GetTclGlobalBool ("writeGRsRaw")) {
  352.     //SHOW(cs);
  353.     // magic number (CyberScan point data 1)
  354.     out.write("cpd1", 4);
  355.     // point count
  356.     int n = pts.size();
  357.     WriteInt(n, out);
  358.     
  359.     // the points
  360.     vector<sd_raw_pnt> cw(n);
  361.     for (int i=0; i<n; i++) {
  362.       if (cs->get_raw_data(pts[i], cw[i]) == false) {
  363. cerr << "Problem in GlobalReg::write_points()"
  364.      << endl;
  365. return false;
  366.       }
  367.     }
  368.     
  369.     WriteSDRawPoints(cw, n, out);    
  370.   } else {
  372.     // magic number (general point data 1)
  373.     out.write("gpd1", 4);  
  374.     // point count
  375.     int n = pts.size();
  376.     WriteInt(n, out);
  377.    
  378.     // the points
  379.     WritePnt3(pts, n, out);   
  380.   }
  382.   return !out.fail();
  383. }
  386. static bool 
  387. read_points (ifstream &in,  vector<Pnt3> &pts)
  388. {
  389.   // magic number (general point data 1)
  390.   char magic[4];
  391.   in.read(magic, 4);
  393.   if ( strncmp(magic, "cpd1", 4)==0 ) {
  395.     cout << "CyberScan raw data" << endl;
  396.     // CyberScan raw data
  397.     int n;
  398.     n = ReadInt(in);
  399.     
  400.     pts.resize(n);
  401.     unsigned int config;
  402.     short yz[2];
  403.     float nt[2]; // nod, turn
  404.     float tr_h;
  405.     CyberXform xf;
  407.     for (int i=0; i<n; i++) {
  408.       config = ReadInt(in);
  409.       yz[0] = ReadShort(in);
  410.       yz[1] = ReadShort(in);
  411.       nt[0] = ReadFloat(in);
  412.       nt[1] = ReadFloat(in);
  413.       tr_h = ReadFloat(in);
  414.        
  415.       int os = config&1; // other_screw
  416.       
  417.       xf.setup(config, tr_h, nt[os]);
  418.       xf.set_screw(nt[!os], nt[!os]);
  419.       pts[i] = xf.apply_xform(yz[0],yz[1]);
  420.       if (! pts[i].isfinite()) {
  421. SHOW(pts[i]);
  422. SHOW(i);
  423. SHOW(config);
  424. SHOW(yz[0]);
  425. SHOW(yz[1]);
  426. SHOW(nt[0]);
  427. SHOW(nt[1]);
  428. SHOW(tr_h);
  429. SHOW(os);
  430.       }
  431.     }
  433.   } else if (!strncmp (magic, "gpd1", 4)) {
  435.     // good old regular 3d points
  436.     // point count
  437.     int n;
  438.     
  439.     n = ReadInt(in);
  440.     
  441.     pts.resize(n);
  442.     
  443.     // the points
  444.     ReadPnt3(pts, n, in);
  445.   } else {
  447.     cerr << "Bad magic field in .gr file!" << endl;
  448.     return false;
  450.   }
  452.   return !in.fail();
  453. }
  456. static struct {
  457.   char* magic; int ver;
  458. } GRVersionTable[] = {
  459.   { "GR01", 1 },
  460.   { "GR02", 2 },
  461.   { "GR03", 3 },
  462.   { "GR04", 4 }
  463. };
  464. #define cGRVersionTableSize (sizeof(GRVersionTable)/sizeof(GRVersionTable[0]))
  465. #define GRVersionCurrent (GRVersionTable[cGRVersionTableSize-1].magic)
  467. int GetGRVersion (char* magic)
  468. {
  469.   for (int i = 0; i < cGRVersionTableSize; i++) {
  470.     if (!strncmp (GRVersionTable[i].magic, magic, 4))
  471.       return GRVersionTable[i].ver;
  472.   }
  474.   return 0;
  475. }
  478. // try writing the mapEntry data into a single file
  479. void
  480. GlobalReg::mapEntry::export_to_file(const std::string &gr_dir)
  481. {
  482.   // figure out the filename
  483.   const char *name1 = GetTbObjName(xfa);
  484.   const char *name2 = GetTbObjName(xfb);
  485.   if (!name1 || !name2) {
  486.     cerr << "Couldn't get names, not saving global"
  487.  << " data" << endl;
  488.     return;
  489.   }
  491.   bool flip = false;
  492.   if (strcmp(name1, name2) == 1) {
  493.     flip = true;
  494.     const char *tmp = name2;
  495.     name2 = name1; name1 = tmp;
  496.   }
  497.   
  498.   std::string path = gr_dir;
  499.   if (!manually_aligned) path += "/auto";
  500.   path += std::string("/") + name1 + GR_MESHNAMES_SEP + name2 + ".gr";
  502.   // just overwrite the file if it exists
  503. #ifdef WIN32
  504.   ofstream out(path.c_str(), ios::binary);
  505. #else
  506.   ofstream out(path.c_str());
  507. #endif
  508.   if (!out) {
  509.     cerr << "Couldn't create file " << path.c_str() << endl;
  510.     return;
  511.   } else {
  512.     SHOW(path.c_str());
  513.   }
  514.   
  515.   // fill the file with useful stuff
  517.   cout << "Writing correspondences between " << name1
  518.        << " and " << name2 << " ... ";
  519.   
  520.   out.write(GRVersionCurrent, 4); // magic number
  522.   //SHOW(name1);
  523.   //SHOW(name2);
  524.   //SHOW(dynamic_cast<CyberScan*> (xfa));
  525.   //SHOW(dynamic_cast<GroupScan*> (xfa));
  526.   if (flip) {
  527.     write_points(out, ptsb, xfb);
  528.     write_points(out, ptsa, xfa);
  529.     Xform<float> tmp_xf = rel_xf;
  530.     tmp_xf.fast_invert();
  531.     for (int i=0; i<16; i++)
  532.         WriteFloat(*((float*)tmp_xf + i), out);
  533.   } else {    
  534.     write_points(out, ptsa, xfa);
  535.     write_points(out, ptsb, xfb);
  536.     for (int i=0; i<16; i++)
  537.       WriteFloat(*((float*)rel_xf + i), out);
  538.   } 
  540.   WriteFloat(pw_point_rmsErr, out);
  541.   WriteFloat(pw_plane_rmsErr, out);
  542.   WriteInt(quality_grade, out);
  543.   out.write((char *)&manually_aligned, 1 /*sizeof(bool) */);
  544.   
  545.   if (out.fail()) return;
  546.   cout << "success." << endl;
  548.   // currently saving:
  549.   // pairwiserms errors
  550.   // point count
  551.   // points
  552.   // registration xform
  553.   // whether manually aligned
  555.   // should also write:
  556.   // rmserror to tangent plane
  557.   // sampling density? 
  558.   // or weight that would normalize based on area?
  559. }
  562. void
  563. GlobalReg::mapEntry::export_cyber_raw(const char *fname)
  564. {
  565.   // only work if both scans are CyberScans
  566.   CyberScan *csa = dynamic_cast<CyberScan*> (xfa);
  567.   CyberScan *csb = dynamic_cast<CyberScan*> (xfb);
  569.   if (!csa || !csb) return;
  571.   ofstream out(fname);
  572.   if (!out) {
  573.     cerr << "Couldn't create file " << fname << endl;
  574.     return;
  575.   } else {
  576.     SHOW(fname);
  577.   }
  578.   
  579.   int cPnt = ptsa.size();
  580.   out.write ((char*)&cPnt, sizeof(cPnt));
  581.   //bool vert;
  582.   sd_raw_pnt cw;
  583.   for (int i=0; i<cPnt; i++) {
  584.     if (csa->get_raw_data(ptsa[i], cw) == false) {
  585.       cerr << "Problem in GlobalReg::mapEntry::export_cyber_raw()"
  586.    << endl;
  587.       return;
  588.     }
  589.     int tmp = (cw.config & 0x00000001); // vert;
  590.     out.write((char*) &tmp,    sizeof(int));
  591.     out.write((char*) &cw.y,   2*sizeof(short));
  592.     out.write((char*) &cw.nod, 2*sizeof(float));
  593.   }
  594.   for (i=0; i<cPnt; i++) {
  595.     if (csb->get_raw_data(ptsb[i], cw) == false) {
  596.       cerr << "Problem in GlobalReg::mapEntry::export_cyber_raw()"
  597.    << endl;
  598.       return;
  599.     }
  600.     int tmp = (cw.config & 0x00000001); // vert;
  601.     out.write((char*) &tmp,    sizeof(int));
  602.     out.write((char*) &cw.y,   2*sizeof(short));
  603.     out.write((char*) &cw.nod, 2*sizeof(float));
  604.   }
  605.   out.write ((char*)(float*)rel_xf, 16 * sizeof(float));
  606.   
  607.   if (g_verbose) {
  608.     if (out.fail()) cout << "failure." << endl;
  609.     else            cout << "success." << endl;
  610.   }
  611. }
  613. // if partner == NULL, this fn will return the points from ALL
  614. // the scans P is partnered with
  615. void 
  616. GlobalReg::get_pts(TbObj *x, vector<Pnt3> &P, vector<Pnt3> &Q,
  617.    TbObj* partner)
  618. {
  619.   P.clear(); Q.clear();
  621.   // find entries with key x
  622.   FOR_MATCHING_KEYS(x, data) {
  624.     TbObj *datap, *dataq;
  625.     XF_F  dataxf = data->rel_xf;
  626.     vector<Pnt3> *pp;//, *qq;
  627.     if (x == data->xfa) {
  628.       datap = data->xfa; dataq = data->xfb;
  629.       pp = &data->ptsa;//  qq = &data->ptsb;
  630.     } else {
  631.       datap = data->xfb; dataq = data->xfa;
  632.       pp = &data->ptsb;//  qq = &data->ptsa;
  633.       dataxf.fast_invert();
  634.     }
  636.     if (partner != NULL && dataq != partner)
  637.       continue;
  639.     int ind = P.size();
  640.     P.insert(P.end(), pp->begin(), pp->end());
  641. #if 1
  642.     // Q becomes "where P's points should be in Q's coords"
  643.     for (int i=0; i<pp->size(); i++) {
  644.       //Pnt3 p = data->ptsa[i];
  645.       Pnt3 p = (*pp)[i];
  646.       dataxf(p);
  647.       Q.push_back(p);
  648.     }
  649. #else
  650.     Q.insert(Q.end(), qq->begin(), qq->end());
  651. #endif
  652.     // then convert to world coordinates
  653.     for_each (P.begin() + ind, P.end(), datap->getXform());
  654.     for_each (Q.begin() + ind, Q.end(), dataq->getXform());
  655.   } END_FOR_KEYS
  656. }
  659. // used for global registration, and only Horn's version
  660. void 
  661. GlobalReg::get_pts_within_group(TbObj *x,
  662. vector<Pnt3> &P, vector<Pnt3> &Q,
  663. vector<TbObj*> &group)
  664. {
  665.   P.clear(); Q.clear();
  667.   // find entries with key x
  668.   FOR_MATCHING_KEYS(x, data) {
  670.     TbObj *datap, *dataq;
  671.     XF_F  dataxf = data->rel_xf;
  672.     vector<Pnt3> *pp;//, *qq;
  673.     if (x == data->xfa) {
  674.       datap = data->xfa; dataq = data->xfb;
  675.       pp = &data->ptsa;//  qq = &data->ptsb;
  676.     } else {
  677.       datap = data->xfb; dataq = data->xfa;
  678.       pp = &data->ptsb;//  qq = &data->ptsa;
  679.       dataxf.fast_invert();
  680.     }
  681.     
  682.     if (CONTAINS(group, dataq)) {
  683.       // copy the points (in local coordinates)
  684.       int ind = P.size();
  685.       P.insert(P.end(), pp->begin(), pp->end());
  687. #if 1
  688.       // Q becomes "where P's points should be in Q's coords"
  689.       for (int i=0; i<pp->size(); i++) {
  690. Pnt3 p = (*pp)[i];
  691. dataxf(p);
  692. Q.push_back(p);
  693.       }
  694. #else
  695.       Q.insert(Q.end(), qq->begin(), qq->end());
  696. #endif
  697.       // then convert to world coordinates
  698.       for_each (P.begin() + ind, P.end(), datap->getXform());
  699.       for_each (Q.begin() + ind, Q.end(), dataq->getXform());
  700.     }
  702.   } END_FOR_KEYS;
  703.       
  704. }
  707. void
  708. GlobalReg::evaluate(const vector<TbObj*> &group)
  709. {
  710.   for (int i=0; i<group.size(); i++) {
  711.     FOR_MATCHING_KEYS(group[i], data) {
  712.       // TbObjs in a pair must be in the same group!
  713.       assert(data->xfa == group[i] ||
  714.      CONTAINS(group, data->xfa));
  715.       assert(data->xfb == group[i] ||
  716.      CONTAINS(group, data->xfb));
  718.       if (data->xfb == group[i]) continue;
  719.       
  720.       data->stats();
  721.     } END_FOR_KEYS;
  722.   }
  723. }
  726. float
  727. GlobalReg::evaluate(TbObj *one)
  728. {
  729.   // calculate the mean distance between every point pair
  730.   int   cnt   = 0;
  731.   float sumSq = 0.0;
  732.   
  733.   FOR_MATCHING_KEYS(one, data) {
  734.      data->stats();
  735.      int n     = data->ptsa.size();
  736.      float rms = data->rmsErr;
  737.      cnt   += n;
  738.      sumSq += n*rms*rms;
  739.   } END_FOR_KEYS;
  740.   if (cnt == 0) {
  741.     cerr << "GlobalReg::evaluate() didn't have entries to"
  742.  << " evaluate" << endl;
  743.     return 0.0;
  744.   }
  745.   return sqrtf(sumSq/cnt);
  746. }
  749. static crope
  750. nameFromTbObj (TbObj* tbo)
  751. {
  752.   if (tbo) {
  753.     RigidScan* rs = dynamic_cast<RigidScan*> (tbo);
  754.     if (rs) {
  755.       DisplayableMesh* dm = FindMeshDisplayInfo (rs);
  756.       if (dm) return dm->getName();
  757.       else    return rs->get_name();
  758.     }
  759.     return crope("(unknown)");
  760.   }
  761.   return crope("(NULL)");
  762. }
  765. void 
  766. GlobalReg::dump(void)
  767. {
  768.   cout << endl;
  769.   FOR_MAP_ENTRIES(key, data) {
  770.     SHOW(key);
  771.     SHOW(data);
  772.     SHOW(data->xfa);
  773.     SHOW(data->xfb);
  774.     vector<Pnt3> &ptsa = data->ptsa;
  775.     cout << "-" << endl;
  776.     copy(ptsa.begin(), ptsa.end(), 
  777.  ostream_iterator<Pnt3>(cout, "n"));
  778.     vector<Pnt3> &ptsb = data->ptsb;
  779.     cout << "-" << endl;
  780.     copy(ptsb.begin(), ptsb.end(), 
  781.  ostream_iterator<Pnt3>(cout, "n"));
  782.     cout << "-" << endl;
  783.     copy(all_scans.begin(), all_scans.end(), 
  784.  ostream_iterator<TbObj*>(cout, "n"));
  785.   } END_FOR_MAP
  786.   cout << endl;
  787. }
  790. void
  791. GlobalReg::move_back(const vector<TbObj*> &group,
  792.      const vector<XF_F> &old_xf)
  793. {
  794.   cout << "Moving the scans back..." << flush;
  795.   // first, create point pairs
  796.   vector<Pnt3> P,Q;
  797.   XF_F xfp, xfq;
  798.   int i,j;
  799.   for (i=0; i<group.size(); i++) {
  800.     FOR_MATCHING_KEYS(group[i], data) {
  801.       // each entry twice in the map
  802.       if (group[i] == data->xfb) continue;
  803.       // points in data ptsa
  804.       int end = P.size();
  805.       for (j=0; j<data->ptsa.size(); j+=20) {
  806. P.push_back(data->ptsa[j]);
  807. Q.push_back(data->ptsa[j]);
  808.       }
  809.       xfp = data->xfa->getXform();               // current xf
  810.       xfq = old_xf[scan_idx(data->xfa, group)];  // old xf
  811.       for_each(P.begin() + end, P.end(), xfp);
  812.       for_each(Q.begin() + end, Q.end(), xfq);
  813.       // points in data ptsb
  814.       end = P.size();
  815.       for (j=0; j<data->ptsb.size(); j+=20) {
  816. P.push_back(data->ptsb[j]);
  817. Q.push_back(data->ptsb[j]);
  818.       }
  819.       xfp = data->xfb->getXform();               // current xf
  820.       xfq = old_xf[scan_idx(data->xfb, group)];  // old xf
  821.       for_each(P.begin() + end, P.end(), xfp);
  822.       for_each(Q.begin() + end, Q.end(), xfq);
  823.     } END_FOR_KEYS;
  824.   }
  826.   // find the motion that brings the group back close to
  827.   // where it was
  828.   double q[7];
  829.   horn_align(&P[0], &Q[0], P.size(), q);
  830.   xfp.fromQuaternion(q, q[4], q[5], q[6]);
  832.   // apply it
  833.   for (i=0; i<group.size(); i++) {
  834.     group[i]->setXform((xfp * 
  835. group[i]->getXform()).enforce_rigidity());
  836.   }
  837.   cout << "done." << endl;
  838. }
  841. Xform<float>
  842. GlobalReg::compute_xform(vector<Pnt3> &P, vector<Pnt3> &Q)
  843. {
  844.   Xform<float> xf;
  845.   double q[7];
  846.   horn_align (&P[0], &Q[0], P.size(), q);
  847.   xf.fromQuaternion (q, q[4], q[5], q[6]);
  848.   return xf;
  849. }
  852. void
  853. GlobalReg::align_one_to_others (TbObj* one, TbObj* two)
  854. {
  855.   vector<Pnt3> P,Q;
  857.   float sum = .5*FLT_MAX, oldsum;
  859.   one->save_for_undo();
  860.   do {
  861.     oldsum = sum;
  863.     get_pts (one, P, Q, two);  // two==NULL means use all partners
  864.     if (P.size() == 0) continue;
  866.     one->setXform(compute_xform(P, Q) * one->getXform(),
  867.   false);
  869.     sum = evaluate(one);
  870.     cout << "The average point-to-point distance is "
  871.  << sum << endl;
  872.     
  873.     if (BailDetector::bail()) {
  874.       cerr << "global alignment cancelled." << endl;
  875.       break;
  876.     }
  877.   } while (2.0*(oldsum-sum) > ftol*(sum+oldsum+1.e-10));
  878. }
  881. #define FOR_ACTIVE_NBORS(scan, nbor) 
  882.   FOR_MATCHING_KEYS(scan, data) {
  883.     TbObj *nbor = (data->xfa == scan ? data->xfb : data->xfa);
  884.     if (active.end() != find(active.begin(),active.end(),nbor)){
  885. #define END_FOR_NBORS }}}}
  887. /* // get emacs identation working after above macro definition... 
  888. { 
  889. */
  891. // part covered by psuedo-code in the paper
  892. void
  893. GlobalReg::align_group(const vector<TbObj*> &group)
  894. {
  895.   vector<Pnt3> P,Q;
  896.   int          i;
  898.   assert (group.size() > 1);
  900.   vector<XF_F> old_xf(group.size());
  901.   for (i=0; i<group.size(); i++) {
  902.     old_xf[i] = group[i]->getXform();
  903.     group[i]->save_for_undo();
  904.   }
  906.   // new algorithm
  907.   //
  908.   // for each member of the group, calculate the number
  909.   // of links, choose the one with the most links as the seed
  910.   //
  911.   // push the seed to a queue and to the active set
  912.   // 
  913.   // process the queue until empty
  914.   //     take the scan at the front
  915.   //     align it with the scans in the active set
  916.   //     if it improves more than the threshold
  917.   //         push all the neighbors in the active set to queue
  918.   // 
  919.   // add to the queue and the active set the scan with the
  920.   // most links to scans in the active set
  922.   vector<TbObj*> active, dormant, nbors;
  923.   deque<TbObj*>  que;
  924.   TbObj *seed;
  925.   int    seed_links = 0;
  927.   // set the dormant and active sets...
  928.   if (dirty_scans.size()) {
  929.     for (i=0; i<group.size(); i++) {
  930.       if (dirty_scans.find(group[i]) != dirty_scans.end()) {
  931. dormant.push_back(group[i]);
  932.       } else {
  933. active.push_back(group[i]);
  934.       }
  935.     }
  936.   }
  938.   if (active.size() == 0) {
  939.     // active set is empty!
  940.     // initialize, find the seed (one with most links)
  941.     for (i=0; i<group.size(); i++) {
  942.       int links = 0;
  943.       FOR_MATCHING_KEYS(group[i], data) {
  944. links++;
  945.       } END_FOR_KEYS;
  946.       if (links > seed_links) {
  947. seed_links = links;
  948. seed = group[i];
  949.       }
  950.     }
  951.     assert(seed_links);
  953.     // initialize the active and dormant sets
  954.     active.push_back(seed);
  955.     dormant.clear();
  956.     dormant.reserve(group.size()-1);
  957.     for (i=0; i<group.size(); i++) {
  958.       if (group[i] != seed) dormant.push_back(group[i]);
  959.     }
  960.   }
  962.   Progress progress (dormant.size(), "Global alignment");
  964.   // process all scans
  965.   while (!dormant.empty()) {
  967.     // find the dormant scan that has the most links to scans
  968.     // in active set
  969.     seed_links = 0;
  970.     vector<TbObj*>::iterator it, pos;
  971.     for (it = dormant.begin(); it != dormant.end(); it++) {
  972.       int links = 0;
  973.       FOR_ACTIVE_NBORS(*it, nbor) {
  974. links++;
  975.       } END_FOR_NBORS;
  976.       if (links > seed_links) {
  977. seed_links = links;
  978. pos        = it;
  979.       }
  980.     }
  981.     assert(seed_links);
  983.     // remove from dormant set
  984.     seed = *pos;
  985.     int dormant_size = dormant.size();
  986.     dormant.erase(pos);
  987.     assert(dormant_size - 1 == dormant.size());
  989.     // push to active and queue
  990.     active.push_back(seed);
  991.     assert(que.empty());
  992.     que.push_back(seed);
  993.     
  994.     cout << "n";
  995.     SHOW(active.size());
  996.     SHOW(dormant.size());
  998.     // process the queue
  999.     int cnt = 0;
  1000.     while (!que.empty()) {
  1002.       seed = que.front();
  1003.       que.pop_front();
  1005.       // align front of the queue with the active set
  1006.       nbors.clear();
  1007.       FOR_ACTIVE_NBORS(seed, nbor) {
  1008. nbors.push_back(nbor);
  1009.       } END_FOR_NBORS;
  1011.       SHOW(nbors.size());
  1012.       
  1013.       // here we already need to have determined the needed pts
  1014.       get_pts_within_group(seed, P, Q, nbors);
  1015.       assert(P.size());
  1016.       assert(P.size() == Q.size());
  1018.       float start_dist = dist2(P,Q);
  1019.       SHOW(start_dist);
  1021.       //Xform<float> xf;
  1022.       //double q[7];
  1023.       //horn_align (&P[0], &Q[0], P.size(), q);
  1024.       //xf.fromQuaternion (q, q[4], q[5], q[6]);
  1025.       
  1026.       // ... so when we get to here we can compute a less 
  1027.       // biased xform
  1028.       Xform<float> xf = compute_xform(P, Q);
  1030.       for_each(P.begin(), P.end(), xf);
  1031.       float end_dist = dist2(P,Q);
  1032.       SHOW(end_dist);
  1034.       // if changed enough, push the neighbors into queue
  1036.       float diff = start_dist - end_dist;
  1038.       if (diff > 0.0) 
  1039. seed->setXform(xf * seed->getXform(), false);
  1041.       if (diff > ftol * end_dist && 
  1042.   diff > 1.0e-6) {
  1043. if (cnt++ < 200) {
  1044.   for (it = nbors.begin(); it != nbors.end(); it++) {
  1045.     if (!CONTAINS(que, *it)) {
  1046.       // nbor not in queue already, push
  1047.       que.push_back(*it);
  1048.     }
  1049.   }
  1050. }
  1051.       }
  1053.       //SHOW(start_dist);
  1054.       //SHOW(end_dist);
  1055.       cout << que.size() << " left; last ";
  1056.       SHOW(diff/P.size());
  1057.       //SHOW(que.size());
  1058.       //SHOW(active.size());
  1059.       //SHOW(ftol);
  1061.     }
  1063.     if (!progress.updateInc())
  1064.       break;
  1065.     if (BailDetector::bail()) {
  1066.       cerr << "global alignment cancelled." << endl;
  1067.       break;
  1068.     }
  1069.   }
  1071.   // align the new points with the old points in order
  1072.   // to minimize the motion of the whole group
  1074.   move_back(group, old_xf);
  1075.   evaluate(group);
  1076. }
  1079. bool
  1080. GlobalReg::getPairError(TbObj *a, TbObj *b, 
  1081.                         float &pointError,
  1082.                         float &planeError,
  1083.                         float &globalError,
  1084.                         bool  &manual)
  1085. {
  1086.   FOR_MATCHING_KEYS(a, data) {
  1087.     if ((data->xfa == a && data->xfb == b) ||
  1088. (data->xfb == a && data->xfa == b)) {
  1089.       pointError  = data->pw_point_rmsErr;
  1090.       planeError  = data->pw_plane_rmsErr;
  1091.       globalError = data->rmsErr;
  1092.       manual      = data->manually_aligned;
  1093.       return true;
  1094.     }
  1095.   } END_FOR_KEYS;
  1097.   return false;
  1098. }
  1101. // later overwrites the first
  1102. // also if detect that both auto and manual versions
  1103. // exist, unlink the auto version
  1104. // or if two versions of the same link exist, delete
  1105. // the older one
  1106. GlobalReg::mapEntry*
  1107. GlobalReg::import(const std::string &fname, bool manual)
  1108. {
  1109.   cout << "import " << fname.c_str() << endl;
  1111.   // TODO: the below (I guess just the rfind) expects / as path separator,
  1112.   // and can't deal with , so we need to either teach it or replace  for /
  1113.   // to make this work on Windows.
  1115.   // get the mesh names
  1116.   std::string name[2];
  1117.   int tmp1 = fname.rfind("/");
  1118.   if (tmp1) tmp1++; // skip /
  1119.   int tmp2 = fname.find(GR_MESHNAMES_SEP);
  1120.   name[0].assign(fname, tmp1, tmp2-tmp1);
  1121.   tmp2 += strlen(GR_MESHNAMES_SEP); // skip ::: or whatever the separator is
  1122.   name[1].assign(fname, tmp2, fname.size()-tmp2-3);
  1124.   // get the TbObj pointers
  1125.   TbObj* tbobj[2];
  1126.   for (int is = 0; is < 2; is++) {
  1127.     DisplayableMesh *dm = FindMeshDisplayInfo(name[is].c_str());
  1128.     if (dm) {
  1129.       // the mesh is in memory
  1130.       tbobj[is] = dm->getMeshData();
  1131.     } else {
  1132.       // the mesh is not in memory; need to proxy
  1133.       tbobj[is] = createProxyScanNamed (name[is].c_str());
  1134.       if (!tbobj[is]) {
  1135. //cerr << "  ignoring correspondence " 
  1136. //     << fname.c_str() << endl;
  1137. return NULL;
  1138.       }
  1139.     }
  1140.   }
  1142.   FOR_MATCHING_KEYS(tbobj[0], data) {
  1143.     if ((data->xfa == tbobj[0] && data->xfb == tbobj[1]) ||
  1144. (data->xfb == tbobj[0] && data->xfa == tbobj[1])) {
  1145.       // this mapentry exists!
  1146.       if (manual && data->manually_aligned == false) {
  1147. // the previous entry was an autoICP entry, delete
  1148. cerr << "Warning: there seem to be several entries " 
  1149.      << endl << "involving meshes " << name[0].c_str() 
  1150.      << " and " << name[1].c_str() << endl;
  1151. cerr << "Deleting autoICP *.gr entry" << endl;
  1152. unlink_gr_files(data->xfa, data->xfb, true);
  1153. // continue to read over this entry
  1154. break;
  1155.       } else {
  1156. // don't read this entry
  1157. return NULL;
  1158.       }
  1159.     }      
  1160.   } END_FOR_KEYS;
  1162. #ifdef WIN32
  1163.   ifstream in(fname.c_str(), ios::binary);
  1164. #else
  1165.   ifstream in(fname.c_str());
  1166. #endif
  1167.   if (!in) {
  1168.     cerr << "Couldn't open file " << fname.c_str()
  1169.  << " for reading" << endl;
  1170.     return NULL;
  1171.   }
  1172.   
  1173.   vector<Pnt3> ap, bp;
  1174.   float rel_xf[16];
  1175.   float pw_point_rmsErr, pw_plane_rmsErr;
  1176.   char version[4];
  1177.   in.read(version, 4);
  1178.   int cPnt;
  1179.   
  1180.   int iVersion = GetGRVersion (version);
  1181.   switch (iVersion) {
  1182.   case 1:
  1183.     // Oops, in version one saved this twice.
  1184.     // Ignore this first one.
  1185.     pw_point_rmsErr = ReadFloat(in);
  1186.     cPnt = ReadInt(in);
  1187.     ap.resize(cPnt); bp.resize(cPnt);
  1188.     ReadPnt3(ap, cPnt, in);
  1189.     ReadPnt3(bp, cPnt, in);
  1191.     /* haven't check this case, should work, but this was the code
  1192.      * that was here before the endian fix.
  1193.     in.read((char*)&pw_point_rmsErr, sizeof(float));
  1194.     in.read((char*)&cPnt, sizeof(int));
  1195.     ap.resize(cPnt); bp.resize(cPnt);
  1196.     in.read((char*)&ap[0], cPnt*sizeof(Pnt3));
  1197.     in.read((char*)&bp[0], cPnt*sizeof(Pnt3)); 
  1198.     */
  1200.     break;
  1202.   case 2:
  1203.     
  1204.     cPnt = ReadInt(in);
  1205.     ap.resize(cPnt); bp.resize(cPnt);
  1206.     ReadPnt3(ap, cPnt, in);
  1207.     ReadPnt3(bp, cPnt, in);
  1209.     /* haven't check this case, should work, but this was the code
  1210.      * that was here before the endian fix.
  1211.     in.read((char*)&cPnt, sizeof(int));
  1212.     ap.resize(cPnt); bp.resize(cPnt);
  1213.     in.read((char*)&ap[0], cPnt*sizeof(Pnt3));
  1214.     in.read((char*)&bp[0], cPnt*sizeof(Pnt3));
  1215.     */
  1217.     break;
  1219.   case 3:
  1220.   case 4:
  1221.     read_points(in, ap);
  1222.     read_points(in, bp);
  1223.     break;
  1225.   case 0:
  1226.   default:
  1227.     cerr << fname.c_str() 
  1228.  << " does not have the correct signature" << endl;
  1229.     cerr << "It may have been created with an out-of-date "
  1230.  << "version of scanalyze" << endl;
  1231.     return NULL;
  1232.   }
  1234.   for (int i=0; i<16; i++) 
  1235.     (*((float *)rel_xf + i)) = ReadFloat(in);
  1236.   pw_point_rmsErr = ReadFloat(in);
  1237.   pw_plane_rmsErr = ReadFloat(in);
  1239.   int quality = mapEntry::qual_Unknown;
  1240.   if (iVersion == 4) {
  1241.     // version 4 fields: quality, manually_aligned
  1242.     // yeah, manual is passed in to this function, but that seems a
  1243.     // broken idea to me, so let this override it
  1244.     
  1245.     quality = ReadInt(in);
  1246.     in.read((char*)&manual, 1 /*sizeof(bool)*/);
  1247.     
  1248.   }
  1250.   // create and insert mapEntry
  1251.   mapEntry* entry = addPair(tbobj[0], tbobj[1], ap, bp,
  1252.     pw_point_rmsErr, pw_plane_rmsErr,
  1253.     rel_xf, manual, 1000000, false, quality);
  1255.   // and grab its modification time from file
  1256.   struct stat fileinfo;
  1257.   stat (fname.c_str(), &fileinfo);
  1258.   entry->modifyDate = fileinfo.st_mtime;
  1259.   return entry;
  1260. }
  1263. inline void
  1264. unlink_gr_file(std::string path,
  1265.        std::string n1,
  1266.        std::string n2)
  1267. {
  1268.   path += "/";
  1269.   path += n1;
  1270.   path += GR_MESHNAMES_SEP;
  1271.   path += n2;
  1272.   path += ".gr";
  1273.   unlink_if_exists(path.c_str());  
  1274. }
  1277. void
  1278. GlobalReg::unlink_gr_files(TbObj *a, TbObj *b, bool only_auto)
  1279. {
  1280.   std::string name1, name2;
  1281.   name1 = GetTbObjName(a);
  1282.   name2 = GetTbObjName(b);
  1284.   unlink_gr_file(gr_auto_dir, name1, name2);
  1285.   unlink_gr_file(gr_auto_dir, name2, name1);
  1286.   if (only_auto == false) {
  1287.     unlink_gr_file(gr_dir, name1, name2);
  1288.     unlink_gr_file(gr_dir, name2, name1);
  1289.   }
  1290. }
  1293. GlobalReg::GlobalReg(void)
  1294.   : initial_import_done(false)
  1295. {
  1296.   
  1297.   //
  1298.   // set up gr_dir and gr_auto_dir
  1299.   //
  1300.   char *tmp = getenv("SC_GR_DIR");
  1301.   if (tmp == NULL) {
  1302.     // use the current working directory
  1303.     char buf[256];
  1304.     getcwd(buf, 256);
  1305.     gr_dir.assign(buf);
  1306.     gr_dir.append("/globalregs");
  1307.   } else {
  1308.     gr_dir.assign(tmp);
  1309.     // remove trailing slash if needed
  1310.     std::string::iterator back = gr_dir.end()-1;
  1311.     if (*back == '/') gr_dir.erase(back);
  1312.   }
  1314.   if (!check_file_access(gr_dir.c_str(), 1,1,1,1,1)) {
  1315.     // need to have something; revert to current dir
  1316.     char current[PATH_MAX];
  1317.     getcwd (current, PATH_MAX);
  1318.     gr_dir.assign (current);
  1319.     cerr << "Reverting to current dir (" << gr_dir.c_str()
  1320.  << ") for globalregs" << endl;
  1321.   }
  1322.   
  1323.   SHOW(gr_dir.c_str());
  1325.   cyber_raw_name = getenv("SC_RAW_DUMP");
  1327. }
  1330. GlobalReg::~GlobalReg(void)
  1331. {
  1332.   // delete the mapEntries
  1333.   // recall they are there twice, delete only once!
  1334.   FOR_MAP_ENTRIES(key, data) {
  1335.     if (key == data->xfa) delete data;
  1336.   } END_FOR_MAP
  1337. }
  1340. void
  1341. GlobalReg::initial_import(void)
  1342. {
  1343.   if (!initial_import_done) {
  1345.     gr_auto_dir.assign(gr_dir);
  1346.     gr_auto_dir.append("/auto");
  1348.     if (!check_file_access(gr_auto_dir.c_str(), 1,1,1,1,1)) {
  1349.       cerr << "Creating autoreg directory " 
  1350.    << gr_auto_dir.c_str() << endl;
  1351.       portable_mkdir (gr_auto_dir.c_str(), 00775);
  1352.     }
  1354.     perform_import();
  1356.     initial_import_done = true;
  1357.   }
  1358. }
  1361. void
  1362. GlobalReg::perform_import(void)
  1363. {
  1364.   cerr << "Importing *.gr... " << flush;
  1366.   int nTotal = 0, nYes = 0;
  1367.   int nTotalAuto = 0, nYesAuto = 0;
  1368.   for (DirEntries de_auto(gr_auto_dir, ".gr");
  1369.        !de_auto.done(); de_auto.next()) {
  1370.     ++nTotalAuto;
  1371.     if (import(de_auto.path()))
  1372.       ++nYesAuto;
  1373.   }
  1375.   int nQuality = mapEntry::qual_Good + 1;
  1376.   assert (nQuality == 4);
  1377.   int quality_bucket[4] = {0};
  1378.   mapEntry* entry;
  1379.   for (DirEntries de(gr_dir, ".gr"); !de.done(); de.next()) {
  1380.     ++nTotal;
  1381.     if (entry = import(de.path(),true)) {
  1382.       ++nYes;
  1383.       ++quality_bucket[entry->getQuality()];
  1384.     }
  1385.   }
  1387.   cerr << "imported " << nYesAuto << " / " << nTotalAuto << " auto; "
  1388.        << nYes << " / " << nTotal << " manual."
  1389.        << endl;
  1390.   cerr << "Quality distribution: ";
  1391.   for (int i = 0; i < nQuality; i++) {
  1392.     cerr << quality_bucket[i];
  1393.     if (i < nQuality - 1) cerr << " / ";
  1394.   }
  1395.   cerr << endl << "done." << endl;
  1396. }
  1399. GlobalReg::mapEntry*
  1400. GlobalReg::addPair(TbObj *a, TbObj *b, 
  1401.    const vector<Pnt3> &ap, const vector<Pnt3> &nrma,
  1402.    const vector<Pnt3> &bp, const vector<Pnt3> &nrmb,
  1403.    Xform<float> rel_xf,
  1404.    bool manually_aligned,
  1405.    int  max_pairs,
  1406.    bool save, int saveQual)
  1407. {
  1408.   assert(ap.size() == bp.size());
  1409.   // delete previous instance
  1410.   deletePair(a,b, save);
  1411.   // create a mapEntry
  1412.   mapEntry *me = new mapEntry(a,b,ap,bp,rel_xf,max_pairs);
  1413.   me->manually_aligned = manually_aligned;
  1414.   me->quality_grade = saveQual;
  1415.   calcPairwiseRmsErr(ap,nrma,bp,nrmb,rel_xf,
  1416.      me->pw_point_rmsErr,
  1417.      me->pw_plane_rmsErr);
  1418.   // add it twice
  1419.   hmm.insert(HMM::value_type(a,me));
  1420.   hmm.insert(HMM::value_type(b,me));
  1421.   // add to set
  1422.   all_scans.insert(a);
  1423.   all_scans.insert(b);
  1424.   dirty_scans.insert(a);
  1425.   dirty_scans.insert(b);
  1427.   // Compute errors, results stored 
  1428.   me->stats();
  1430.   if (save) me->export_to_file(gr_dir);
  1431.   if (cyber_raw_name) me->export_cyber_raw(cyber_raw_name);
  1433.   return me;
  1434. }
  1437. GlobalReg::mapEntry*
  1438. GlobalReg::addPair(TbObj *a, TbObj *b, 
  1439.    const vector<Pnt3> &ap,
  1440.    const vector<Pnt3> &bp,
  1441.    float pw_point_rmsErr,
  1442.    float pw_plane_rmsErr,
  1443.    Xform<float> rel_xf,
  1444.    bool manually_aligned,
  1445.    int  max_pairs,
  1446.    bool save, int saveQual)
  1447. {
  1448.   assert(ap.size() == bp.size());
  1449.   // delete previous instance
  1450.   deletePair(a,b, save);
  1451.   // create a mapEntry
  1452.   mapEntry *me = new mapEntry(a,b,ap,bp,rel_xf,max_pairs);
  1453.   me->manually_aligned = manually_aligned;
  1454.   me->quality_grade = saveQual;
  1455.   me->pw_point_rmsErr = pw_point_rmsErr;
  1456.   me->pw_plane_rmsErr = pw_plane_rmsErr;
  1457.   // add it twice
  1458.   hmm.insert(HMM::value_type(a,me));
  1459.   hmm.insert(HMM::value_type(b,me));
  1460.   // add to set
  1461.   all_scans.insert(a);
  1462.   all_scans.insert(b);
  1463.   dirty_scans.insert(a);
  1464.   dirty_scans.insert(b);
  1466.   // Compute errors, results stored 
  1467.   me->stats();
  1469.   if (save) me->export_to_file(gr_dir);
  1470.   if (cyber_raw_name) me->export_cyber_raw(cyber_raw_name);
  1472.   return me;
  1473. }
  1476. bool
  1477. GlobalReg::markPairQuality (TbObj *a, TbObj* b, int saveQual)
  1478. {
  1479.   FOR_MATCHING_KEYS(a, data) {
  1480.     if (data->xfa == b || data->xfb == b) {
  1481.       data->quality_grade = saveQual;
  1482.       data->export_to_file(gr_dir);
  1483.       return true;
  1484.     }
  1485.   } END_FOR_KEYS;
  1487.   return false;
  1488. }
  1492. void
  1493. GlobalReg::deletePair(TbObj *a, TbObj *b, bool deleteFile)
  1494. {
  1495.   // erase twice, delete once
  1496.   FOR_MATCHING_KEYS(a, data) {
  1497.     if (data->xfa == b || data->xfb == b) {
  1498.       hmm.erase(__i);
  1499.       break;
  1500.     }
  1501.   } END_FOR_KEYS;
  1502.   FOR_MATCHING_KEYS(b, data) {
  1503.     if (data->xfa == a || data->xfb == a) {
  1504.       hmm.erase(__i);
  1505.       delete data;
  1506.       break;
  1507.     }
  1508.   } END_FOR_KEYS;
  1509.   // if the transforms are not there, remove from set
  1510.   if (hmm.end() == hmm.find(a)) {
  1511.     all_scans.erase(a);
  1512.     dirty_scans.erase(a);
  1513.   } else {
  1514.     dirty_scans.insert(a);
  1515.   }
  1516.   if (hmm.end() == hmm.find(b)) {
  1517.     all_scans.erase(b);
  1518.     dirty_scans.erase(b);
  1519.   } else {
  1520.     dirty_scans.insert(b);
  1521.   }
  1522.   // delete the corresponding files
  1523.   if (deleteFile) unlink_gr_files(a,b);
  1524. }
  1527. void
  1528. GlobalReg::deleteAllPairs (TbObj *a, bool deleteFile)
  1529. {
  1530.   bool again;
  1531.   do {
  1532.     again = false;
  1534.     // deletion invalidates existing iterators, so don't go 
  1535.     // through the
  1536.     // FOR_MATCHING_KEYS loop more than once; exit it and restart.
  1537.     FOR_MATCHING_KEYS (a, data) {
  1538.       // erase once
  1539.       hmm.erase (__i);
  1540.       
  1541.       // possible delete the *.gr file
  1542.       if (deleteFile) unlink_gr_files(data->xfa, data->xfb);
  1544.       // erase twice
  1545.       TbObj* other = data->xfa;
  1546.       if (other == a)
  1547. other = data->xfb;
  1548.       bool foundit = false;
  1549.       FOR_MATCHING_KEYS (other, data2) {
  1550. if (data2->xfa == a || data2->xfb == a) {
  1551.   assert (data2 == data);
  1552.   hmm.erase (__i);
  1553.   foundit = true;
  1554.   break;
  1555. }
  1556.       } END_FOR_KEYS;
  1557.       assert (foundit);
  1558.       
  1559.       // delete once
  1560.       delete data;
  1562.       // then reset the iterator
  1563.       again = true;
  1564.       break;
  1565.     } END_FOR_KEYS;
  1566.   } while (again);
  1568.   assert (hmm.end() == hmm.find(a));
  1569.   all_scans.erase (a);
  1571.   cerr << "Nuked all pairs for " << nameFromTbObj (a) << endl;
  1572. }
  1575. void
  1576. GlobalReg::deleteAutoPairs (float thr, TbObj *a)
  1577. {
  1578.   bool again;
  1579.   int  cnt = 0;
  1580.   do {
  1581.     again = false;
  1583.     // deletion invalidates existing iterators, so don't go 
  1584.     // through the
  1585.     // FOR_MAP_ENTRIES loop more than once; exit it and restart.
  1586.     FOR_MAP_ENTRIES (key, data) {
  1588.       if (a != NULL && data->xfa != a && data->xfb != a) continue;
  1590.       if (!data->manually_aligned &&
  1591.   data->pw_plane_rmsErr > thr) {
  1593. cnt++;
  1594. deletePair(data->xfa, data->xfb, true);
  1596. // then reset the iterator
  1597. again = true;
  1598. break;
  1599.       }
  1600.     } END_FOR_MAP;
  1601.   } while (again);
  1603.   cerr << "Nuked " << cnt 
  1604.        << " auto pairs greater than " << thr << endl;
  1605. }
  1608. void 
  1609. GlobalReg::showPointpairCount(TbObj *a, TbObj *b)
  1610. {
  1611.   assert(a);
  1612.   if (b == NULL) {
  1613.     cout << endl << "From " 
  1614.  << nameFromTbObj(a) << " to" << endl;
  1615.     cout << "counttrmsErrtglobalErrtscan" << endl;
  1616.   }
  1617.   bool found = false;
  1618.   FOR_MATCHING_KEYS(a, data) {
  1619.     if (b == NULL) {
  1620.       cout << data->ptsa.size() << "t";
  1621.       cout << data->rmsErr << "t";
  1622.       cout << data->maxErr << "t";
  1623.       if (data->xfa == a) cout << nameFromTbObj(data->xfb);
  1624.       else                cout << nameFromTbObj(data->xfa);
  1625.       cout << endl;
  1626.       found = true;
  1627.     } else if (data->xfa == b || data->xfb == b) {
  1628.       cout << endl << data->ptsa.size() << "t";
  1629.       cout << data->rmsErr << "t";
  1630.       cout << data->maxErr << "t";
  1631.       cout << nameFromTbObj(a) << "t";
  1632.       cout << nameFromTbObj(b) << endl;
  1633.       found = true;
  1634.       break;
  1635.     }
  1636.   } END_FOR_KEYS;
  1637.   if (!found) {
  1638.     cout << "Couldn't find such pair!" << endl;
  1639.   }
  1640. }
  1643. bool
  1644. GlobalReg::pairRegistered (TbObj* a, TbObj* b,
  1645.    bool &manual,
  1646.    bool transitiveAllowed)
  1647. {
  1648.   FOR_MATCHING_KEYS(a, data) {
  1649.     if (data->xfa == a && data->xfb == b ||
  1650. data->xfb == a && data->xfa == b) {
  1651.       manual = data->manually_aligned;
  1652.       return true;
  1653.     }
  1654.   } END_FOR_KEYS;
  1656.   manual = false;
  1658.   if (transitiveAllowed) {
  1659.     // create a vector of the TbObj pointers
  1660.     vector<TbObj*> scan;
  1661.     int inda = -1, indb = -1;
  1662.     for (ITS is = all_scans.begin(); is!=all_scans.end(); is++) {
  1663.       if (*is == a)
  1664. inda = scan.size();
  1665.       if (*is == b)
  1666. indb = scan.size();
  1667.       scan.push_back(*is);
  1668.     }
  1669.     if (inda == -1 || indb == -1) {
  1670.       // one of the scans has no partners at all
  1671.       return false;
  1672.     }
  1674.     // find the connected components
  1675.     ConnComp cc(scan.size());
  1676.     FOR_MAP_ENTRIES(key, data) {
  1677.       if (key == data->xfb) continue;
  1678.       cc.connect(scan_idx(data->xfa, scan),
  1679.  scan_idx(data->xfb, scan));
  1680.     } END_FOR_MAP;
  1682.     // find connected component containing a
  1683.     vector<int> g;
  1684.     while (cc.get_next_group(g)) {
  1685.       // is a here?  if not, go to next group
  1686.       if (CONTAINS(g, inda))
  1687. continue;
  1689.       // this is a's group... is b here?
  1690.       if (CONTAINS(g, indb))
  1691. return true;  // b found in same group as a
  1692.       else
  1693. return false; // a's here but b's not
  1694.     }
  1695.     // only get here if we never found a
  1696.     cerr << "Internal error in "
  1697.  << "GlobalReg::pairRegistered(transitive)" << endl;
  1698.   }
  1700.   return false;
  1701. }
  1704. crope
  1705. GlobalReg::list_partners (TbObj* mesh, bool transitiveAllowed)
  1706. {
  1707.   crope response;
  1709.   if (!transitiveAllowed) {
  1710.     FOR_MATCHING_KEYS (mesh, data) {
  1711.       TbObj* other = data->xfa;
  1712.       if (other == mesh)
  1713. other = data->xfb;
  1714.       if (!response.empty())
  1715. response += crope (" ");
  1716.       response += nameFromTbObj(other);
  1717.     } END_FOR_KEYS;
  1719.   } else {
  1721.     // create a vector of the TbObj pointers
  1722.     vector<TbObj*> scan;
  1723.     int ind = -1;
  1724.     for (ITS is = all_scans.begin(); is!=all_scans.end(); is++) {
  1725.       if (*is == mesh)
  1726. ind = scan.size();
  1727.       scan.push_back(*is);
  1728.     }
  1729.     if (ind != -1) {
  1730.       // find the connected components
  1731.       ConnComp cc(scan.size());
  1732.       FOR_MAP_ENTRIES(key, data) {
  1733. if (key == data->xfb) continue;
  1734. cc.connect(scan_idx(data->xfa, scan),
  1735.    scan_idx(data->xfb, scan));
  1736.       } END_FOR_MAP;
  1737.       
  1738.       // find connected component containing a
  1739.       vector<int> g;
  1740.       while (cc.get_next_group(g)) {
  1741. // is a here?  if not, go to next group
  1742. if (CONTAINS(g, ind))
  1743.   continue;
  1745. // this is a's group... dump it
  1746. for (int i = 0; i < g.size(); i++) {
  1747.   if (!response.empty())
  1748.     response += crope (" ");
  1749.   response += nameFromTbObj(scan[g[i]]);
  1750. }
  1751. break;
  1752.       }
  1753.     }
  1754.   }
  1756.   return response;
  1757. }
  1760. int
  1761. GlobalReg::count_partners (TbObj* mesh)
  1762. {
  1763.   int partners = 0;
  1764.   FOR_MATCHING_KEYS (mesh, data) {
  1765.     ++partners;
  1766.   } END_FOR_KEYS;
  1768.   return partners;
  1769. }
  1772. void
  1773. GlobalReg::dump_connected_groups (void)
  1774. {
  1775.   // create a vector of the TbObj pointers
  1776.   vector<TbObj*> scan;
  1777.   for (ITS is = all_scans.begin(); is!=all_scans.end(); is++) {
  1778.     scan.push_back(*is);
  1779.   }
  1780.   
  1781.   // find the connected components
  1782.   ConnComp cc(scan.size());
  1783.   FOR_MAP_ENTRIES(key, data) {
  1784.     if (key == data->xfb) continue;
  1785.     cc.connect(scan_idx(data->xfa, scan),
  1786.        scan_idx(data->xfb, scan));
  1787.   } END_FOR_MAP;
  1789.   // dump contents of each group
  1790.   vector<int> g;
  1791.   int nGroups = 0;
  1792.   while (cc.get_next_group(g)) {
  1793.     if (g.size()) {
  1794.       cout << "group (" << g.size() << "): ";
  1795.       for (int* p = g.begin(); p != g.end(); p++)
  1796. cout << nameFromTbObj (scan[*p]) << " ";
  1797.       cout << endl;
  1798.       ++nGroups;
  1799.     }
  1800.   }
  1802.   cout << "(" << nGroups << ") total" << endl;
  1803. }
  1805. // the old version of align (no area-normalization)
  1806. void
  1807. GlobalReg::align(float _ftol,
  1808.  TbObj* scanToMove, TbObj* scanToMoveTo)
  1809. {
  1810.   ftol   = _ftol;
  1812.   if (scanToMove) {
  1813.     // just move one scan
  1814.     align_one_to_others (scanToMove, scanToMoveTo);
  1816.   } else {
  1817.     // align scans in connected groups
  1819.     // create a vector of the TbObj pointers
  1820.     vector<TbObj*> scan;
  1821.     for (ITS is = all_scans.begin(); is!=all_scans.end(); is++) {
  1822.       scan.push_back(*is);
  1823.     }
  1825.     // find the connected components
  1826.     ConnComp cc(scan.size());
  1827.     FOR_MAP_ENTRIES(key, data) {
  1828.       if (key == data->xfb) continue;
  1829.       cc.connect(scan_idx(data->xfa, scan),
  1830.  scan_idx(data->xfb, scan));
  1831.     } END_FOR_MAP;
  1832.     // for each connected component, align
  1833.     vector<int> g;
  1834.     vector<TbObj*> group;
  1835.     while (cc.get_next_group(g)) {
  1836.       if (g.size() > 1) {
  1837. group.clear();
  1838. for (int i=0; i<g.size(); i++)
  1839.   group.push_back(scan[g[i]]);
  1840. align_group(group);
  1841.       }
  1842.       if (BailDetector::bail()) {
  1843. cerr << "global alignment cancelled." << endl;
  1844. break;
  1845.       }
  1846.     }
  1847.     dirty_scans.erase(dirty_scans.begin(), dirty_scans.end());
  1848.   }
  1849. }
  1851. // NEW VERSION of align
  1852. // this align function will area-normalize (whereas
  1853. // the above fn that does not take a worldBbox won't)
  1855. // TODO: use worldBbox instead of re-computing
  1856. void
  1857. GlobalReg::align(float _ftol, Bbox worldBbox, 
  1858.  TbObj* scanToMove, TbObj* scanToMoveTo)
  1859. {
  1860.   ftol   = _ftol;
  1862.   normalize_samples(worldBbox, scanToMoveTo);
  1864.   if (scanToMove) {
  1865.     // just move one scan
  1866.     align_one_to_others (scanToMove, scanToMoveTo);
  1868.   } else {
  1869.     // align scans in connected groups
  1871.     // create a vector of the TbObj pointers
  1872.     vector<TbObj*> scan;
  1873.     for (ITS is = all_scans.begin(); is!=all_scans.end(); is++) {
  1874.       scan.push_back(*is);
  1875.     }
  1877.     // find the connected components
  1878.     ConnComp cc(scan.size());
  1879.     FOR_MAP_ENTRIES(key, data) {
  1880.       if (key == data->xfb) continue;
  1881.       cc.connect(scan_idx(data->xfa, scan),
  1882.  scan_idx(data->xfb, scan));
  1883.     } END_FOR_MAP;
  1884.     // for each connected component, align
  1885.     vector<int> g;
  1886.     vector<TbObj*> group;
  1887.     while (cc.get_next_group(g)) {
  1888.       if (g.size() > 1) {
  1889. group.clear();
  1890. for (int i=0; i<g.size(); i++)
  1891.   group.push_back(scan[g[i]]);
  1892. align_group(group);
  1893.       }
  1894.       if (BailDetector::bail()) {
  1895. cerr << "global alignment cancelled." << endl;
  1896. break;
  1897.       }
  1898.     }
  1899.     dirty_scans.erase(dirty_scans.begin(), dirty_scans.end());
  1900.   }
  1902. }
  1904. // normalize_samples goes through the map entries and throws
  1905. // out points based on a bucket structure that keeps track of
  1906. // how many points are already in the unit volume to which
  1907. // the new points belong.  only the set stored in scanToMoveTo
  1908. // in align(...) should be normalized in this way.
  1909. void GlobalReg::normalize_samples(Bbox worldBbox, TbObj *scanToMoveTo)
  1910. {
  1911.   VolumeBucket *vb;
  1913.   cerr << "initializing buckets...";
  1914.   int numBuckets = init_vol_buckets(&vb, worldBbox, scanToMoveTo,
  1915.     VB_SIZE);
  1916.   cerr << "done" << endl;
  1917.   vector<TbObj*> scans; // vectorized version of scanToMoveTo
  1919.   // check to see if we're aligning with one scan or all scans
  1920.   if (scanToMoveTo) {
  1921.     // we only have one scan - throw it in our scan vector
  1922.     scans.push_back(scanToMoveTo);
  1923.   } else { // sTMT == NULL means use all
  1924.     for (ITS is = all_scans.begin(); is!=all_scans.end(); is++) {
  1925.       scans.push_back(*is);
  1926.     }
  1927.   }
  1929.   // iterating thru our scan vector, interate thru the map to
  1930.   // find matching keys to the current scan we're considering
  1931.   // for each point in ptsa (don't check ptsb because corresponds),
  1932.   // - find which bucket it belongs to
  1933.   // - check that bucket.  
  1934.   // - if too full, delete points from a and b
  1935.   // - otherwise, throw the new points in the bucket
  1937.   int bucketInd;
  1939.   cerr << "iterating through points..." << scans.size() << " scans"
  1940.        << endl;
  1941.   for (int i = 0; i < scans.size(); i++) {
  1942.     // data is a mapEntry
  1943.     FOR_MATCHING_KEYS(scans[i], data) {
  1944.       vector<Pnt3>::iterator ip, ip2;
  1945.       ip = data->ptsa.begin();
  1946.       ip2 = data->ptsb.begin();
  1947.       for (int j = 0; j < data->ptsa.size(); j++) {
  1948. assert(ip != data->ptsa.end());
  1949. assert(ip2 != data->ptsb.end());
  1950. // changed
  1951. Pnt3 temp(data->ptsa[j]);
  1953. scans[i]->xformPnt(temp);
  1954. cerr << "pt: " << temp[0] << " " << temp[1] << " "
  1955.      << temp[2];
  1956. bucketInd = find_bucket(temp, vb, VB_SIZE, numBuckets);
  1957. if (bucketInd == NOT_FOUND) {
  1958.   temp.set((data->ptsb[j])[0],
  1959.    (data->ptsb[j])[1], (data->ptsb[j])[2]);
  1960.   scans[i]->xformPnt(temp);
  1961.   bucketInd = find_bucket(temp, vb, VB_SIZE, numBuckets);
  1962.   assert(bucketInd != NOT_FOUND);
  1963. }
  1964. //if (should_delete_point(data->ptsa[j], vb, bucketInd)) {
  1965. if (vb[bucketInd].points.size() > THRESHOLD) {
  1966.   
  1967.   data->ptsa.erase(ip);
  1968.   data->ptsb.erase(ip2);
  1969.   cerr << "...throwing out" << endl;
  1971. } else {
  1973.   vb[bucketInd].points.push_back(temp);
  1974.   //vb[bucketInd].points.push_back(data->ptsa[j]);
  1975.   //vb[bucketInd].points.push_back(data->ptsb[j]);
  1976.   cerr << "...keeping" << endl;
  1977. }
  1979. ip++; ip2++;
  1980.       } // for j
  1981.     } END_FOR_KEYS;
  1982.   } // for i
  1984.   cerr << "done" << endl;
  1985.   free(vb);
  1987. }
  1989. // linear time vector delete
  1990. // int k = 0;
  1991. //    for (vector<Pnt3>::iterator ip = data->ptsa.begin(); 
  1992. //         ip != data->ptsa.end(); ip++) {
  1993. //      cerr << "looping a..., k = " << k << ", j = "
  1994. //   << j << endl;
  1995. //      if (k == j) {
  1996. //        data->ptsa.erase(ip); break;
  1997. //      }
  1998. //      k++;
  1999. //    }
  2000. //    k = 0;
  2001. //    for (vector<Pnt3>::iterator ip2 = data->ptsb.begin(); 
  2002. //         ip2 != data->ptsb.end(); ip2++) {
  2003. //      cerr << "looping b..., k = " << k << ", j = "
  2004. //   << j << endl;
  2005. //      if (k == j) {
  2006. //        data->ptsb.erase(ip2); break;
  2007. //      }
  2008. //      k++;
  2009. //    }
  2011. bool GlobalReg::should_delete_point(Pnt3 pt, VolumeBucket *vb, 
  2012.     int bucketInd)
  2013. {
  2014.   if (vb[bucketInd].points.size() > THRESHOLD) return (true);
  2016.   // put poisson disk jitter here - efficient way to do this?
  2017.   // if we go thru all the points in the bucket, it will be
  2018.   // very slow...
  2020.   return (false);
  2021. } 
  2023. // figure out how large the space is
  2024. // divide to make some number of buckets
  2025. // store all the min and max vars for those buckets
  2026. int GlobalReg::init_vol_buckets(VolumeBucket **vb, 
  2027. Bbox worldBbox, 
  2028. TbObj *scanToMoveTo, float bucket_side)
  2029. {
  2030.   Pnt3 min;
  2031.   Bbox bbox; // bbox around all the scans in scanToMoveTo
  2033.   if (scanToMoveTo) {
  2034.     bbox.add(scanToMoveTo->worldBbox());
  2035.   } else { // sTMT == NULL means use all
  2036.     for (ITS is = all_scans.begin(); is != all_scans.end(); is++) {
  2037.       TbObj *key = *is;
  2038.       bbox.add(key->worldBbox());
  2039.     }
  2040.   }
  2042.   min.set(bbox.min()[0], bbox.min()[1], bbox.min()[2]);
  2043.   Pnt3 dim = bbox.max() - min;
  2044.   dim /= bucket_side;
  2045.   Pnt3 xincr(bucket_side, 0, 0);
  2046.   Pnt3 yincr(0, bucket_side, 0);
  2047.   Pnt3 zincr(0, 0, bucket_side);
  2048.   
  2049.   int numBuckets = ceil(dim[0]) * ceil(dim[1]) * ceil(dim[2]);
  2050.   cerr << "Initializing " << numBuckets << " buckets...";
  2051.   *vb = new VolumeBucket[numBuckets];
  2052.   assert(*vb);
  2053.   Pnt3 vol_size(bucket_side, bucket_side, bucket_side);
  2054.   Pnt3 max;
  2055.   int cnt = 0;
  2057.   for (int i = 0; i < ceil(dim[0]); i++) {
  2059.     for (int j = 0; j < ceil(dim[1]); j++) {
  2060.       
  2061.       for (int k = 0; k < ceil(dim[2]); k++) {
  2063. max = min + vol_size;
  2065. ((*vb)[cnt]).volume = Bbox(min, max);
  2066. min += zincr;
  2067. cnt++;
  2068.       }
  2069.       min.set(min[0], min[1], bbox.min()[2]);
  2070.       min += yincr;
  2071.     }
  2072.     min.set(min[0], bbox.min()[1], min[2]);
  2073.     min += xincr;
  2074.   }
  2075.   assert(cnt == numBuckets);
  2076.   return (numBuckets);
  2077. }
  2079. // TODO: optimize so runs in constant time 
  2080. // (currently linear time)
  2081. int GlobalReg::find_bucket(Pnt3 pt, VolumeBucket *vb, 
  2082.    float bucket_side, int numBuckets)
  2083. {
  2084.   for (int i = 0; i < numBuckets; i++) {
  2085.     if (!vb[i].volume.outside(pt)) {
  2086.       return (i);
  2087.     }
  2088.   }
  2090.   return (NOT_FOUND);
  2091.   //return (1);
  2092. }
  2094. std::string
  2095. GlobalReg::dump_meshpairs (int   choice,
  2096.    float listThresh)
  2097. {
  2098.   vector<crope> names;
  2099.   vector<TbObj*> objs;
  2101.   for (DisplayableMesh** dm = theScene->meshSets.begin();
  2102.        dm < theScene->meshSets.end(); dm++) {
  2103.     if ((*dm)->getVisible()) {
  2104.       names.push_back ((*dm)->getName());
  2105.       objs.push_back ((*dm)->getMeshData());
  2106.     } else {
  2107.       cout << "Excluding invisible mesh " 
  2108.    << (*dm)->getName() << endl;
  2109.     }
  2110.   }
  2112.   int i, j;
  2113.   // write 2 digit numbers, horizontal row
  2114.   printf ("n%35s", "");
  2115.   for (i = 0; i < names.size(); i++) {
  2116.     printf (" %d ", i / 10);
  2117.   }
  2118.   printf ("n");
  2119.   printf ("%35s", "");
  2120.   for (i = 0; i < names.size(); i++) {
  2121.     printf (" %d ", i % 10);
  2122.   }
  2123.   printf ("nn");
  2125.   multimap<const float, int, greater<float> > sortedList;
  2126.   float pointError, planeError, globalError;
  2127.   bool  manual;
  2128.   for (i = 0; i < names.size(); i++) {
  2129.     printf ("%28s (%02d): ", names[i].c_str(), i);
  2130.     int nPartners = 0;
  2131.     for (j = 0; j < names.size(); j++) {
  2132.       char code[3];
  2133.       sprintf(code," -");
  2134.       bool paired = getPairError (objs[i], objs[j], 
  2135.   pointError,
  2136.   planeError,
  2137.   globalError,
  2138.   manual);
  2139.       float error;
  2140.       if      (choice == 0) error = pointError;
  2141.       else if (choice == 1) error = planeError;
  2142.       else                  error = globalError;
  2143.       if (paired) {
  2144. ++nPartners;
  2145. int i_err = int(error*10 + 0.5);
  2146. if (i_err > 99) i_err = 99;
  2147. sprintf(code,"%02d",i_err);
  2148. if (i > j)
  2149.   sortedList.insert(pair<const float, int>(error,
  2150.    i*65000+j));
  2151.       }
  2153.       // if we're at/past i, it's diagonal or upper right --
  2154.       // duplicates, don't print
  2155.       if (j == i)
  2156. sprintf(code," O");
  2157.       else if (j > i)
  2158. sprintf(code,"  ");
  2159.       if (paired && manual) printf ("%s.", code);
  2160.       else                  printf ("%s ", code);
  2161.     }
  2163.     printf ("(%d partners)n", nPartners);
  2164.   }
  2166.   printf ("n");
  2167.   printf ("Meshes and their unsorted pairwise errors:n");
  2168.   printf ("n");
  2170.   for (i = 0; i < names.size(); i++) {
  2171.     printf ("%28s:", names[i].c_str());
  2172.     for (j = 0; j < names.size(); j++) {
  2173.       bool paired = getPairError (objs[i], objs[j], 
  2174.   pointError,
  2175.   planeError,
  2176.   globalError,
  2177.   manual);
  2178.       float error;
  2179.       if      (choice == 0) error = pointError;
  2180.       else if (choice == 1) error = planeError;
  2181.       else                  error = globalError;
  2182.       if (paired) printf(" %1.2f", error);
  2183.     }
  2184.     printf ("n");
  2185.   }
  2187.   std::string ans;
  2189.   if (listThresh < FLT_MAX) {
  2190.      cout << endl << "List of worst offenders:" << endl;
  2191.      multimap<const float, int, greater<float> >::iterator it;
  2192.      for (it = sortedList.begin(); it != sortedList.end(); it++) {
  2193. float err = (*it).first;
  2194. if (err > listThresh) {
  2195.    unsigned int tricky_code = (*it).second;
  2196.    unsigned int i = tricky_code / 65000;
  2197.    unsigned int j = tricky_code % 65000;
  2198.    
  2199.    bool paired = getPairError (objs[i], objs[j], 
  2200.        pointError,
  2201.        planeError,
  2202.        globalError,
  2203.        manual);
  2204.    ans += names[i].c_str(); ans += " ";
  2205.    ans += names[j].c_str();
  2206.    if (manual) {
  2207.      printf("%s to %s: %g t(manual %f)n",
  2208.     names[i].c_str(), names[j].c_str(), 
  2209.     err, planeError);
  2210.      ans += " manual ";
  2211.    } else {
  2212.      printf("%s to %s: %g t(auto %f)n",
  2213.     names[i].c_str(), names[j].c_str(),
  2214.     err, planeError);
  2215.      ans += " auto ";
  2216.    }
  2217.    char str[256];
  2218.    sprintf(str, "%f ", err);
  2219.    ans += str;
  2220. } else {
  2221.    break;
  2222. }
  2223.      }
  2224.   }
  2225.   cout << endl;
  2226.   return ans;
  2227. }
  2230. static void
  2231. update_getPairingSummary_fields (GlobalReg::mapEntry* data,
  2232.  GlobalReg::ERRMETRIC metric,
  2233.  int count[3], float err[3], int qual[4])
  2234. {
  2235.   ++count[0];
  2236.   if (data->manually_aligned)
  2237.     ++count[1];
  2238.   else
  2239.     ++count[2];
  2241.   // choice of error metrics:
  2242.   // maxErr, avgErr, rmsErr, pw_point_rmsErr, pw_plane_rmsErr
  2243.   float errSrc;
  2244.   switch (metric) {
  2245.   case GlobalReg::errmetric_max: errSrc = data->maxErr;          break;
  2246.   case GlobalReg::errmetric_avg: errSrc = data->avgErr;          break;
  2247.   case GlobalReg::errmetric_rms: errSrc = data->rmsErr;          break;
  2248.   case GlobalReg::errmetric_pln: errSrc = data->pw_plane_rmsErr; break;
  2249.   default:  // this is the only one populated by a single icp
  2250.   case GlobalReg::errmetric_pnt: errSrc = data->pw_point_rmsErr; break;
  2251.   }
  2253.   err[0] = min (err[0], errSrc);
  2254.   err[1] += errSrc;
  2255.   err[2] = max (err[2], errSrc);
  2256.   
  2257.   ++qual[data->getQuality()];
  2258. }
  2261. bool
  2262. GlobalReg::getPairingSummary (TbObj* mesh, ERRMETRIC metric,
  2263.       int count[3], float err[3], int qual[4])
  2264. {
  2265.   count[0] = count[1] = count[2] = 0;
  2266.   qual[0] = qual[1] = qual[2] = qual[3] = 0;
  2267.   err[0] = 1e6; err[1] = err[2] = 0;
  2269.   if (mesh) {
  2271.     FOR_MATCHING_KEYS(mesh, data) {
  2272.       update_getPairingSummary_fields (data, metric, count, err, qual);
  2273.     } END_FOR_KEYS;
  2274.     
  2275.   } else {
  2277.     FOR_MAP_ENTRIES (mesh, data) {
  2278.       update_getPairingSummary_fields (data, metric, count, err, qual);
  2279.     } END_FOR_MAP;
  2281.   }
  2283.   if (count[0])
  2284.     err[1] /= count[0];
  2285.   else
  2286.     err[0] = 0;
  2288.   if (!mesh) { // don't double-count
  2289.     for (int i = 0; i < 3; i++) count[i] /= 2;
  2290.     for (i = 0; i < 4; i++) qual[i] /= 2;
  2291.   }
  2292.   
  2293.   return true;
  2294. }
  2297. bool
  2298. GlobalReg::getPairingStats (TbObj* from, TbObj* to,
  2299.     bool& bManual, int& iQuality,
  2300.     int& nPoints, time_t& date,
  2301.     float err[5])
  2302. {
  2303.   FOR_MATCHING_KEYS(from, data) {
  2304.     if (data->xfa == to || data->xfb == to) {
  2305.       bManual = data->manually_aligned;
  2306.       iQuality = data->getQuality();
  2307.       nPoints = data->ptsa.size();
  2308.       date = data->modifyDate;
  2309.       err[0] = data->maxErr;
  2310.       err[1] = data->avgErr;
  2311.       err[2] = data->rmsErr;
  2312.       err[3] = data->pw_point_rmsErr;
  2313.       err[4] = data->pw_plane_rmsErr;
  2314.       return true;
  2315.     }
  2316.   } END_FOR_KEYS;
  2318.   return false;
  2319. }
  2322. #ifdef MAIN_GR
  2324. #include "Random.h"
  2327. void
  2328. main(void)
  2329. {
  2330.   GlobalReg gr;
  2332.   int nviews = 10;
  2333.   vector<Xform<float> > xf(nviews);
  2334.   Random rnd;
  2336.   int i,j,k;
  2338.   // random initial transforms
  2339.   for (i=0; i<nviews; i++) {
  2340.     float fact = .5;
  2341.     xf[i].rotX((rnd()-.5)*M_PI*fact);
  2342.     xf[i].rotY((rnd()-.5)*M_PI*fact);
  2343.     xf[i].rotZ((rnd()-.5)*M_PI*fact);
  2344.     xf[i].translate((rnd()-.5)*fact, 
  2345.      (rnd()-.5)*fact, 
  2346.      (rnd()-.5)*fact);
  2347.   }
  2349.   vector<int> views;
  2350.   int ti;
  2351.   for (i=0; i<nviews; i++) {
  2352.     // pair each view randomly with 3 other views
  2353.     views.clear();
  2354.     views.push_back(i);
  2355.     for (j=0; j<3; j++) {
  2356.       do {
  2357. ti = nviews*rnd(); ti %= nviews;
  2358.       } while (CONTAINS(views,ti));
  2359.       views.push_back(ti);
  2360.     }
  2361.     // create 3000 random point pairs with each view pair
  2362.     vector<Pnt3> P(3000),Q(3000);
  2363.     for (j=1; j<4; j++) {
  2364.       for (k=0; k<3000; k++) {
  2365. // get a random point
  2366. Pnt3 p(rnd(), rnd(), rnd());
  2367. // store it in world coordinates according the
  2368. // current transformations
  2369. xf[views[0]].apply(p,P[k]);
  2370. xf[views[j]].apply(p,Q[k]);
  2371. // add some noise to points
  2372. P[k] += Pnt3(rnd()*.001-.0005,
  2373.      rnd()*.001-.0005,
  2374.      rnd()*.001-.0005);
  2375. Q[k] += Pnt3(rnd()*.001-.0005,
  2376.      rnd()*.001-.0005,
  2377.        rnd()*.001-.0005);
  2379.       }
  2380.       gr.addPair(&xf[i], &xf[views[j]], P, Q);
  2381.     }    
  2382.   }
  2384.   float ftol = 1.e-3;
  2386.   copy(xf.begin(), xf.end(), 
  2387.        ostream_iterator<Xform<float> >(cout, "n"));
  2389.   gr.align(ftol, alignHorn);
  2391.   copy(xf.begin(), xf.end(), 
  2392.        ostream_iterator<Xform<float> >(cout, "n"));
  2393. }
  2394. #endif // MAIN_GR