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

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

3D图形编程

开发平台：

Visual C++

sczRegCmds.cc：源码内容
  1. //////////////////////////////////////////////////////////////////////
  2. //
  3. // sczRegCmds.cc
  4. // Matt Ginzton, Kari Pulli
  5. // Fri Jun 12 15:19:16 PDT 1998
  6. // 
  7. // Interface between C and Tcl code for various registration UIs
  8. //   1.  Correspondence registration
  9. //   2.  Drag registration
  10. //   3.  Global registration
  11. //   4.  ICP
  12. //
  13. //////////////////////////////////////////////////////////////////////
  16. #include "plvGlobals.h"
  17. #include "ToglHash.h"
  18. #include "togl.h"
  19. #include "Trackball.h"
  20. #include "RigidScan.h"
  21. #include "plvMeshCmds.h"
  22. #include "plvDraw.h"
  23. #include "sczRegCmds.h"
  24. #include "plvAnalyze.h"
  25. #include "ColorUtils.h"
  26. #include "GlobalReg.h"
  27. #include "plvScene.h"
  28. #include "plvDrawCmds.h"
  29. #include "DisplayMesh.h"
  30. #include "absorient.h"
  31. #include "Random.h"
  32. #include "ToglCache.h"
  33. #include "ICP.h"
  34. #include "Progress.h"
  38. //////////////////////////////////////////////////////////////////////
  39. //
  40. // 1.  Correspondence-registration UI
  41. //
  42. //////////////////////////////////////////////////////////////////////
  44. static struct Togl* g_overviewTogl = NULL;
  45. static uchar colorNew[3] = { 0, 0, 0 };            // black
  46. static uchar colorOld[3] = { 255, 0, 0 };          // red
  47. static uchar colorSel[3] = { 0, 255, 0 };          // green
  48. static uchar colorHilite[3] = { 255, 255, 255 };   // white
  51. #define A_LITTLE_OUT_OF_THE_SCREEN  1e-6
  52. #define GLOBALREG_TOLERANCE         1.e-4
  55. class AlignmentInfo
  56. {
  57. public:
  58.   AlignmentInfo();
  59.   virtual ~AlignmentInfo();
  61.   Trackball* tb;
  62. };
  65. class AlignmentToglInfo: public AlignmentInfo
  66. {
  67. public:
  68.   void SetTargetMesh (DisplayableMesh* _md);
  70.   DisplayableMesh* meshDisplay;
  71.   RigidScan* meshData;
  72. };
  75. class CorrespondencePt
  76. {
  77. public:
  78.   DisplayableMesh* mesh;
  79.   Pnt3 pt;
  80. };
  83. class CorrespondenceList
  84. {
  85. public:
  86.   int AddPoint (DisplayableMesh* mesh, const Pnt3& pt);
  87.   CorrespondencePt* FindPoint (DisplayableMesh* mesh);
  88.   void DeletePoint (CorrespondencePt* cp);
  89.   void ToString (char* text);
  91.   int id;
  92.   uchar color[3];
  93.   vector <CorrespondencePt> points;
  94. };
  97. class AlignmentOverviewInfo: public AlignmentInfo
  98. {
  99. public:
  100.   AlignmentOverviewInfo (struct Togl* overviewTogl);
  101.   ~AlignmentOverviewInfo();
  103.   CorrespondenceList* AddCorrespondence (DisplayableMesh* mesh,
  104.  const Pnt3& pt);
  105.   CorrespondenceList* CompleteCorrespondence (bool bKeep, char* text);
  107.   vector<CorrespondenceList*> correspondences;
  108.   int GetSelectedCorrespondenceId();
  109.   CorrespondenceList* GetSelectedCorrespondence();
  110.   CorrespondenceList* GetCorrespondenceById (int id);
  111.   CorrespondenceList* GetCorrespondenceInProgress();
  113.   bool                 bColorPoints;
  115. private:
  116.   int                  nNextId;
  117.   CorrespondenceList*  correspInProgress;
  118.   ColorSet             correspColors;
  119.   struct Togl*         togl;
  120. };
  123. int 
  124. CorrespondenceList::AddPoint (DisplayableMesh* mesh, const Pnt3& pt)
  125. {
  126.   CorrespondencePt* cp = FindPoint (mesh);
  127.   if (cp != NULL) {
  128.     // already have one for this mesh -- adjust it
  129.     cp->pt = pt;
  130.   } else {
  131.     // create new correspondence for this mesh
  132.     CorrespondencePt cp;
  133.     cp.mesh = mesh;
  134.     cp.pt = pt;
  135.     points.push_back (cp);
  136.   }
  138.   return points.size();
  139. }
  142. CorrespondencePt* 
  143. CorrespondenceList::FindPoint (DisplayableMesh* mesh)
  144. {
  145.   for (CorrespondencePt* cp = points.begin(); cp < points.end(); cp++) {
  146.     if (cp->mesh == mesh)
  147.       return cp;
  148.   }
  150.   return NULL;
  151. }
  154. void 
  155. CorrespondenceList::DeletePoint (CorrespondencePt* cp)
  156. {
  157.   points.erase (cp);
  158. }
  161. void 
  162. CorrespondenceList::ToString (char* text)
  163. {
  164.   sprintf (text, "[%d]", id);
  165.   for (CorrespondencePt* cp = points.begin();
  166.        cp < points.end();
  167.        cp++) {
  168.     sprintf (text + strlen(text), " %s:(%.4g,%.4g,%.4g)",
  169.      cp->mesh->getName(),
  170.      cp->pt[0], cp->pt[1], cp->pt[2]);
  171.       
  172.     if (cp + 1 < points.end())
  173.       strcat (text, " <=>");
  174.   }
  175. }
  178. AlignmentInfo::AlignmentInfo()
  179. {
  180.   tb = NULL;
  181. }
  184. AlignmentInfo::~AlignmentInfo()
  185. {
  186.   if (tb != NULL)
  187.     delete tb;
  188. }
  191. AlignmentOverviewInfo::AlignmentOverviewInfo (struct Togl* overviewTogl)
  192. {
  193.   nNextId = 0;
  194.   correspInProgress = NULL;
  195.   togl = overviewTogl;
  196.   bColorPoints = true;
  197. }
  200. AlignmentOverviewInfo::~AlignmentOverviewInfo ()
  201. {
  202.   for (int i = 0; i < correspondences.size(); i++) {
  203.     delete correspondences[i];
  204.   }
  206.   g_overviewTogl = NULL;
  207. }
  210. CorrespondenceList* 
  211. AlignmentOverviewInfo::AddCorrespondence (DisplayableMesh* mesh, 
  212.   const Pnt3& pt)
  213. {
  214.   CorrespondenceList* clSelected = GetSelectedCorrespondence();
  215.   if (clSelected != NULL) {
  216.     clSelected->AddPoint (mesh, pt);
  217.     return clSelected;
  218.   }
  220.   if (correspInProgress == NULL) {
  221.     correspInProgress = new CorrespondenceList;
  222.     correspInProgress->id = 0;
  223.   }
  225.   correspInProgress->AddPoint (mesh, pt);
  226.   return correspInProgress;
  227. }
  230. CorrespondenceList* 
  231. AlignmentOverviewInfo::CompleteCorrespondence (bool bKeep, 
  232.        char* text)
  233. {
  234.   if (correspInProgress == NULL) {
  235.     strcpy (text, "[0]");
  236.     return NULL;     // nothing to do
  237.   }
  239.   if (correspInProgress->points.size() < 2)  // nothing to keep!
  240.     bKeep = false;
  242.   if (bKeep) {
  243.     correspInProgress->id = ++nNextId;
  244.     correspInProgress->ToString (text);
  245.     correspColors.chooseNewColor (correspInProgress->color);
  246.     correspondences.push_back (correspInProgress);
  247.     correspInProgress = NULL;
  248.     return correspondences.back();
  249.   } else {
  250.     correspInProgress->ToString (text);
  251.     delete correspInProgress;
  252.     correspInProgress = NULL;
  253.     return NULL;
  254.   }
  255. }
  258. CorrespondenceList* 
  259. AlignmentOverviewInfo::GetSelectedCorrespondence (void)
  260. {
  261.   return GetCorrespondenceById (GetSelectedCorrespondenceId());
  262. }
  265. CorrespondenceList*
  266. AlignmentOverviewInfo::GetCorrespondenceInProgress (void)
  267. {
  268.   return correspInProgress;
  269. }
  272. CorrespondenceList* 
  273. AlignmentOverviewInfo::GetCorrespondenceById (int id)
  274. {
  275.   if (id == 0)
  276.     return NULL;
  278.   CorrespondenceList** cl;
  279.   for (cl = correspondences.begin(); cl < correspondences.end(); cl++) {
  280.     if ((*cl)->id == id)
  281.       return *cl;
  282.   }
  284.   printf ("Consistency error in correspondence data structuresn");
  285.   return NULL;
  286. }
  289. int 
  290. AlignmentOverviewInfo::GetSelectedCorrespondenceId (void)
  291. {
  292.   Tcl_Interp* interp = Togl_Interp (togl);
  293.   // figure out which correspondence is selected
  294.   Tcl_Eval (interp, "getListboxSelection .reg.lines.list");
  295.   int iCorresp = 0;
  296.   if (strlen(interp->result) > 3)
  297.     iCorresp = atoi (interp->result + 1);
  299.   return iCorresp;
  300. }
  303. void 
  304. AlignmentToglInfo::SetTargetMesh (DisplayableMesh* _md)
  305. {
  306.   meshDisplay = _md;
  307.   if (meshDisplay == NULL) {
  308.     meshData = NULL;
  309.     return;
  310.   }
  311.   meshData = meshDisplay->getMeshData();
  312.   assert (meshData != NULL);
  314.   // set up trackball, rotate by current view of object...
  315.   Pnt3 up (0, 1, 0);
  316.   Pnt3 obj = meshData->localCenter();
  317.   float radius = meshData->radius();
  318.   Pnt3 camera (0, 0, radius * 3);
  319.     
  320.   //position camera in front of object
  321.   camera += obj;
  322.   tb->setup (camera, obj, up, radius);
  324.   // transform by rotations in effect: local, then viewer
  325.   Xform<float> xfr = meshData->getXform();
  326.   xfr.removeTranslation();
  327.   float q[4], t[3];
  328.   tbView->getXform (q, t);
  329.   xfr.rotQ (q[0], q[1], q[2], q[3]);
  331.   // and apply to trackball
  332.   xfr.toQuaternion (q);
  333.   tb->rotateQuat (q);
  334. }
  337. void
  338. DrawAlignmentPoint (const Pnt3& pt, uchar* color, bool bSelected = false)
  339. {
  340.   glDisable (GL_LIGHTING);
  342.   for (int iPass = 1; iPass >= 0; iPass--) {
  343.     //draw twice, once partially transparent without depth test
  345.     if (iPass == 0) {
  346.       glEnable (GL_DEPTH_TEST);
  347.       glDepthFunc (GL_LEQUAL);
  348.       glDisable (GL_BLEND);
  349.     } else {
  350.       glDisable (GL_DEPTH_TEST);
  351.       glEnable (GL_BLEND);
  352.       glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  353.     }
  354.     
  355.     if (bSelected && iPass == 1) {
  356.       glColor4ub (colorHilite[0], colorHilite[1], colorHilite[2], 255);
  357.       glPointSize (7);
  358.     } else {
  359.       glColor4ub (color[0], color[1], color[2], 256*(1-iPass/1.3));
  360.       if (bSelected)
  361. glPointSize (5);
  362.       else
  363. glPointSize (4);
  364.     }
  365.         
  366.     glBegin (GL_POINTS);
  367.     glVertex3fv (pt);
  368.     glEnd();
  369.   }
  370. }
  373. void
  374. DrawAlignmentPoints (struct Togl* togl)
  375. {
  376.   //warning: code that does not scale well with a huge number of
  377.   //correspondences, but that probably won't matter.  If it does,
  378.   //do something with better than linear performance here.
  380.   AlignmentOverviewInfo* aoi = NULL;
  381.   if (g_overviewTogl != NULL)
  382.     aoi = (AlignmentOverviewInfo*)Togl_GetClientData (g_overviewTogl);
  383.   if (aoi != NULL) {
  384.     AlignmentToglInfo* ati = (AlignmentToglInfo*)Togl_GetClientData (togl);
  385.     assert (ati != NULL);
  387.     if (ati->meshData != NULL) {
  389.       int iCorresp = aoi->GetSelectedCorrespondenceId();
  391.       // now draw all correspondences in correct color
  392.       for (int i = 0; i < aoi->correspondences.size(); i++) {
  393. CorrespondenceList* cl = aoi->correspondences[i];
  395. bool bSelected = (cl->id == iCorresp);
  397. for (CorrespondencePt* c = cl->points.begin();
  398.      c < cl->points.end();
  399.      c++) {
  400.   if (c->mesh == ati->meshDisplay) {
  401.     uchar* color;
  402.     if (aoi->bColorPoints)
  403.       color = cl->color;
  404.     else
  405.       color = bSelected ? colorSel : colorOld;
  407.     DrawAlignmentPoint (c->pt, color, bSelected);
  408.   }
  409. }
  410.       }
  411.     }
  413.     // now draw correspondence in progress
  414.     CorrespondenceList* cl = aoi->GetCorrespondenceInProgress();
  415.     if (cl != NULL) {
  416.       for (CorrespondencePt* c = cl->points.begin();
  417.    c < cl->points.end();
  418.    c++) {
  419. if (c->mesh == ati->meshDisplay)
  420.   DrawAlignmentPoint (c->pt, colorNew, true);
  421.       }
  422.     }
  423.   }
  424. }
  427. void
  428. DrawAlignmentMesh (struct Togl* togl)
  429. {
  430.   DrawAlignmentMeshToBack (togl);
  431.   DrawAlignmentPoints (togl);
  433.   Togl_SwapBuffers (togl);
  434. }
  437. void
  438. DrawAlignmentMeshToBack (struct Togl* togl)
  439. {
  440.   AlignmentToglInfo* ati = (AlignmentToglInfo*)Togl_GetClientData (togl);
  441.   assert (ati != NULL);
  443.   if (ati->meshData == NULL)
  444.     return;
  446.   // initialize buffers and modes
  447.   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  448.   ati->tb->setSize (Togl_Width (togl), Togl_Height (togl));
  449.   glViewport(0, 0, Togl_Width (togl), Togl_Height (togl));
  450.   glEnable (GL_DEPTH_TEST);
  451.   glDepthFunc (GL_LESS);
  453.   // Prepare projection
  454.   ati->tb->applyProjection();
  456.   // set up lighting
  457.   glEnable(GL_LIGHTING);
  458.   glEnable(GL_LIGHT0);
  459.   glMatrixMode (GL_MODELVIEW);
  460.   glLoadIdentity();
  461.   float downZaxis[4] = { 0, 0, 1, 0 };
  462.   glLightfv(GL_LIGHT0, GL_POSITION, downZaxis);
  464.   // prepare modelview
  465.   ati->tb->applyXform();
  467.   // now render
  468.   ati->meshDisplay->drawImmediate();
  469. }
  472. void
  473. SpinToglTrackball (struct Togl* togl)
  474. {
  475.   AlignmentInfo* ai = (AlignmentInfo*)Togl_GetClientData (togl);
  476.   assert (ai != NULL);
  478.   if (ai->tb->isSpinning())
  479.   {
  480.     Togl_PostRedisplay (togl);
  481.   }
  482. }
  485. void
  486. FreeAlignmentInfo (struct Togl* togl)
  487. {
  488.   AlignmentInfo* ai = (AlignmentInfo*)Togl_GetClientData (togl);
  490.   assert (ai != NULL);
  491.   delete ai;
  493.   Togl_SetClientData (togl, NULL);
  494. }
  497. int
  498. PlvBindToglToAlignmentOverviewCmd(ClientData clientData, Tcl_Interp *interp, 
  499.   int argc, char *argv[])
  500. {
  501.   if (argc < 2) {
  502.     interp->result = "Bad arguments to PlvBindToglToAlignmentOverviewCmd";
  503.     return TCL_ERROR;
  504.   }
  506.   struct Togl* togl = toglHash.FindTogl (argv[1]);
  507.   if (togl == NULL) {
  508.     interp->result = "Missing togl widget in PlvBindToglToAlignmentViewCmd";
  509.     return TCL_ERROR;
  510.   }
  511.   assert (0 == strcmp (argv[1], Togl_Ident (togl)));
  513.   AlignmentOverviewInfo* aoi = new AlignmentOverviewInfo (togl);
  514.   aoi->tb = new Trackball;
  515.   Togl_SetClientData (togl, aoi);
  516.   Togl_SetDestroyFunc (togl, FreeAlignmentInfo);
  517.   Togl_SetTimerFunc (togl, SpinToglTrackball);
  519.   assert (g_overviewTogl == NULL);
  520.   g_overviewTogl = togl;
  522.   return TCL_OK;
  523. }
  526. int
  527. PlvCorrespRegParmsCmd(ClientData clientData, Tcl_Interp *interp, 
  528.       int argc, char *argv[])
  529. {
  530.   if (argc < 3) {
  531.     interp->result = "Bad arguments to PlvCorrespRegParmsCmd";
  532.     return TCL_ERROR;
  533.   }
  535.   struct Togl* togl = toglHash.FindTogl (argv[1]);
  536.   if (togl == NULL) {
  537.     interp->result = "Missing togl widget in PlvCorrespRegParmsCmd";
  538.     return TCL_ERROR;
  539.   }
  540.   AlignmentOverviewInfo* aoi =
  541.     (AlignmentOverviewInfo*)Togl_GetClientData (togl);
  543.   if (!strcmp (argv[2], "colorpoints")) {
  544.     if (argc > 2) {
  545.       aoi->bColorPoints = atoi (argv[3]);
  546.     } else {
  547.       interp->result = aoi->bColorPoints ? "1" : "0";
  548.     }
  549.   }
  551.   return TCL_OK;
  552. }
  555. int
  556. PlvBindToglToAlignmentViewCmd(ClientData clientData, Tcl_Interp *interp, 
  557.       int argc, char *argv[])
  558. {
  559.   if (argc < 3) {
  560.     interp->result = "Bad arguments to PlvBindToglToAlignmentViewCmd";
  561.     return TCL_ERROR;
  562.   }
  564.   struct Togl* togl = toglHash.FindTogl (argv[1]);
  565.   if (togl == NULL) {
  566.     interp->result = "Missing togl widget in PlvBindToglToAlignmentViewCmd";
  567.     return TCL_ERROR;
  568.   }
  569.   assert (0 == strcmp (argv[1], Togl_Ident (togl)));
  571.   DisplayableMesh* mesh = FindMeshDisplayInfo (argv[2]);
  572.   if (mesh == NULL && strcmp (argv[2], "")) {
  573.     interp->result = "Missing mesh in PlvBindToglToAlignmentViewCmd";
  574.     return TCL_ERROR;
  575.   }
  577.   //set that togl widget up to display the given mesh
  578.   AlignmentToglInfo* ati = (AlignmentToglInfo*)Togl_GetClientData (togl);
  579.   if (ati == NULL) {
  580.     ati = new AlignmentToglInfo;
  581.     ati->tb = new Trackball;
  582.     Togl_SetClientData (togl, ati);
  583.     Togl_SetDisplayFunc (togl, DrawAlignmentMesh);
  584.     Togl_SetDestroyFunc (togl, FreeAlignmentInfo);
  585.     Togl_SetTimerFunc (togl, SpinToglTrackball);
  586.   }
  588.   ati->SetTargetMesh (mesh);
  590.   Togl_PostRedisplay (togl);
  592.   return TCL_OK;
  593. }
  596. int
  597. PlvRegUIMouseCmd (ClientData clientData, Tcl_Interp *interp, 
  598.   int argc, char *argv[])
  599. {
  600.   // argument string: togl button x y time [start|stop]
  601.   if (argc < 6) {
  602.     interp->result = "Bad arguments to PlvRegUIMouseCmd";
  603.     return TCL_ERROR;
  604.   }
  606.   struct Togl* togl = toglHash.FindTogl (argv[1]);
  607.   if (togl == NULL) {
  608.     interp->result = "Missing togl widget in PlvRegUIMouseCmd";
  609.     return TCL_ERROR;
  610.   }
  612.   int button = atoi (argv[2]);
  613.   int mouseX = atoi (argv[3]);
  614.   int mouseY = atoi (argv[4]);
  615.   int eventT = atoi (argv[5]);
  616.   bool up = TRUE;
  617.   if (argc > 6 && !strcmp (argv[6], "start"))
  618.     up = FALSE;
  620.   AlignmentToglInfo* ati = (AlignmentToglInfo*)Togl_GetClientData (togl);
  621.   assert (ati != NULL);
  622.   
  623.   if (button == 0)
  624.     ati->tb->move (mouseX, mouseY, eventT);
  625.   else
  626.     ati->tb->pressButton (button, up, mouseX, mouseY, eventT);
  628.   Togl_PostRedisplay (togl);
  630.   return TCL_OK;
  631. }
  634. int
  635. PlvAddPartialRegCorrespondenceCmd (ClientData clientData, Tcl_Interp *interp, 
  636.    int argc, char *argv[])
  637. {
  638.   //argument string: togl overviewTogl x y
  639.   if (argc < 5) {
  640.     interp->result = "Bad arguments to PlvAddPartialRegCorrespondenceCmd";
  641.     return TCL_ERROR;
  642.   }
  644.   struct Togl* togl = toglHash.FindTogl (argv[1]);
  645.   struct Togl* toglOV = toglHash.FindTogl (argv[2]);
  647.   if (togl == NULL) {
  648.     interp->result = "Missing togl widget in PlvCreateRegCorrespondenceCmd";
  649.     return TCL_ERROR;
  650.   }
  652.   int x = atoi (argv[3]);
  653.   int y = Togl_Height (togl) - atoi (argv[4]);
  655.   AlignmentToglInfo* ati = (AlignmentToglInfo*)Togl_GetClientData (togl);
  656.   assert (ati != NULL);
  657.   AlignmentOverviewInfo* aoi =
  658.     (AlignmentOverviewInfo*)Togl_GetClientData (toglOV);
  660.   Pnt3 pt;
  662.   if (!findZBufferNeighborExt (x, y, pt, togl, ati->tb, 300)) {
  663.     // allow removal of one point from an existing correspondence,
  664.     // if it has more than two points
  665.     CorrespondenceList* clSelected = aoi->GetSelectedCorrespondence();
  666.     CorrespondencePt* cpOld = NULL;
  667.     if (clSelected != NULL) {
  668.       if (clSelected->points.size() > 2)
  669. cpOld = clSelected->FindPoint (ati->meshDisplay);
  670.       else
  671. printf ("Cannot delete last two points from correspondencen");
  672.     }
  674.     if (cpOld != NULL) { // prompt to delete point
  675.       Tcl_VarEval (interp, "tk_dialog .reg.confirm {Delete point?} "
  676.    "{Delete meshset ", 
  677.    ati->meshDisplay->getName(),
  678.    " from correspondence?} {} 0 No Yes", NULL);
  679.       if (atoi (interp->result) != 0) {
  680. clSelected->DeletePoint (cpOld);
  681. Tcl_Eval (interp, "rebuildSelectedCorrespondenceString");
  682. DrawAlignmentMesh (togl);
  683.       }
  684.     }
  686.     interp->result = "";
  687.     return TCL_OK;
  688.   }
  690.   // add to temporary correspondence set
  691.   CorrespondenceList* cl = aoi->AddCorrespondence (ati->meshDisplay, pt);
  692.   // if modifying existing corresp, tell tcl
  693.   if (cl->id != 0) {
  694.     Tcl_Eval (interp, "rebuildSelectedCorrespondenceString");
  695.   }
  697.   // draw new point hilited
  698.   DrawAlignmentMeshToBack (togl);
  699.   DrawAlignmentPoints (togl);
  700.   Togl_SwapBuffers (togl);
  702.   interp->result = (char*)ati->meshDisplay->getName();
  703.   return TCL_OK;
  704. }
  707. int
  708. PlvDeleteRegCorrespondenceCmd (ClientData clientData, Tcl_Interp *interp, 
  709.        int argc, char *argv[])
  710. {
  711.   if (argc != 3) {
  712.     interp->result = "Bad arguments to PlvDeleteRegCorrespondenceCmd";
  713.     return TCL_ERROR;
  714.   }
  716.   struct Togl* toglOV = toglHash.FindTogl (argv[1]);
  717.   if (!toglOV) {
  718.     interp->result = "Missing overview togl widget in DeleteRegCorrespondence";
  719.     return TCL_ERROR;
  720.   }
  721.   AlignmentOverviewInfo* aoi =
  722.     (AlignmentOverviewInfo*)Togl_GetClientData (toglOV);
  723.   assert (aoi != NULL);
  725.   int iCorresp = atoi (argv[2] + 1);  // in format [nnn] asdfasd
  727.   for (CorrespondenceList** cl = aoi->correspondences.begin();
  728.        cl < aoi->correspondences.end();
  729.        cl++) {
  730.     if ((*cl)->id == iCorresp) {
  731.       aoi->correspondences.erase (cl);
  732.       return TCL_OK;
  733.     }
  734.   }
  736.   interp->result = "Index not found in PlvDeleteRegCorrespondenceCmd";
  737.   return TCL_ERROR;
  738. }
  741. int
  742. PlvConfirmRegCorrespondenceCmd (ClientData clientData, Tcl_Interp *interp, 
  743. int argc, char *argv[])
  744. {
  745.   if (argc < 2) {
  746.     interp->result = "Bad arguments to PlvConfirmRegCorrespondenceCmd";
  747.     return TCL_ERROR;
  748.   }
  750.   struct Togl* toglOV = toglHash.FindTogl (argv[1]);
  751.   if (!toglOV) {
  752.     interp->result = "Missing overview togl widget in CreateRegCorrespondence";
  753.     return TCL_ERROR;
  754.   }
  755.   AlignmentOverviewInfo* aoi =
  756.     (AlignmentOverviewInfo*)Togl_GetClientData (toglOV);
  757.   assert (aoi != NULL);
  759.   bool bKeep = true;
  760.   if (argc > 2 && 0 == strcmp (argv[2], "delete"))
  761.     bKeep = false;
  763.   char text[2000];
  764.   CorrespondenceList* cl = aoi->CompleteCorrespondence (bKeep, text);
  766.   Tcl_SetResult (interp, text, TCL_VOLATILE);
  767.   return TCL_OK;
  768. }
  771. int
  772. PlvGetCorrespondenceInfoCmd (ClientData clientData, Tcl_Interp *interp, 
  773.      int argc, char *argv[])
  774. {
  775.   if (argc != 3) {
  776.     interp->result = "Bad arguments to PlvGetCorrespondenceInfoCmd";
  777.     return TCL_ERROR;
  778.   }
  780.   struct Togl* toglOV = toglHash.FindTogl (argv[1]);
  781.   if (toglOV == NULL) {
  782.     interp->result = "Missing overlay togl in PlvGetCorrespondenceInfoCmd";
  783.     return TCL_ERROR;
  784.   }
  786.   int idCorresp = atoi (argv[2]);
  787.   if (idCorresp == 0) {
  788.     interp->result = "Bad correspondence id in PlvGetCorrespondenceInfoCmd";
  789.     return TCL_ERROR;
  790.   }
  792.   AlignmentOverviewInfo* aoi =
  793.     (AlignmentOverviewInfo*)Togl_GetClientData (toglOV);
  794.   assert (aoi != NULL);
  795.   CorrespondenceList* cl = aoi->GetCorrespondenceById (idCorresp);
  796.   if (cl == NULL) {
  797.     interp->result = "Missing correspondence in PlvGetCorrespondenceInfoCmd";
  798.     return TCL_ERROR;
  799.   }
  801.   char text[2000];
  802.   cl->ToString (text);
  803.   Tcl_SetResult (interp, text, TCL_VOLATILE);
  805.   return TCL_OK;
  806. }
  810. static int CorrespRegOneMesh (AlignmentOverviewInfo* aoi,
  811.       DisplayableMesh* meshFrom,
  812.       DisplayableMesh* meshTo);
  813. static int CorrespRegAllToAll (AlignmentOverviewInfo* aoi);
  816. int
  817. PlvCorrespondenceRegistrationCmd (ClientData clientData, Tcl_Interp *interp, 
  818.   int argc, char *argv[])
  819. {
  820.   // arguments are: overviewTogl mode from to
  821.   // from and to are irrelevant for all2all mode, but must exist
  822.   if (argc != 5) {
  823.     interp->result = "Bad arguments to PlvCorrespondenceRegistrationCmd";
  824.     return TCL_ERROR;
  825.   }
  827.   // get correspondence list
  828.   struct Togl* toglOV = toglHash.FindTogl (argv[1]);
  829.   if (toglOV == NULL) {
  830.     interp->result = "Missing overview togl in PlvCorrespRegistrationCmd";
  831.     return TCL_ERROR;
  832.   }
  833.   AlignmentOverviewInfo* aoi = (AlignmentOverviewInfo*)
  834. Togl_GetClientData (toglOV);
  835.   assert (aoi != NULL);
  837.   // get mode
  838.   enum { from2to, from2all, all2all, error } mode = error;
  839.   if (!strcmp (argv[2], "from2to")) {
  840.     mode = from2to;
  841.   } else if (!strcmp (argv[2], "from2all")) {
  842.     mode = from2all;
  843.   } else if (!strcmp (argv[2], "all2all")) {
  844.     mode = all2all;
  845.   }
  847.   switch (mode) {
  848.   case all2all:
  849.     return CorrespRegAllToAll (aoi);
  851.   case from2to:
  852.   case from2all:
  853.     {
  854.       DisplayableMesh* meshFrom = FindMeshDisplayInfo (argv[3]);
  855.       if (!meshFrom) {
  856. interp->result = "Missing 'from' mesh in PlvCorrespRegistrationCmd";
  857. return TCL_ERROR;
  858.       }
  860.       if (mode == from2all)
  861. return CorrespRegOneMesh (aoi, meshFrom, NULL);
  862.       else {
  863. DisplayableMesh* meshTo = FindMeshDisplayInfo (argv[4]);
  864. if (!meshTo) {
  865.   interp->result = "Missing 'to' mesh in PlvCorrespRegistrationCmd";
  866.   return TCL_ERROR;
  867. }
  868. return CorrespRegOneMesh (aoi, meshFrom, meshTo);
  869.       }
  870.     }
  871.   }
  872.     
  873.   interp->result = "Bad mode passed to PlvCorrespRegistrationCmd";
  874.   return TCL_ERROR;
  875. }
  878. static int
  879. CorrespRegOneMesh (AlignmentOverviewInfo* aoi, DisplayableMesh* meshFrom,
  880.    DisplayableMesh* meshTo)
  881. {
  882.   // pass meshTo as NULL to use all correspondences involving meshFrom
  883.   // (from2all), or meshTo as a specific mesh to use only correspondences
  884.   // between meshFrom and meshTo (from2to).
  886.   RigidScan* msFrom = meshFrom->getMeshData();
  887.   RigidScan* msTo = meshTo->getMeshData();
  889.   // build lists of points from these meshes
  890.   // transform points to world coordinates
  891.   vector<Pnt3> srcPt, dstPt;
  892.   for (int i = 0; i < aoi->correspondences.size(); i++) {
  893.     CorrespondenceList* cl = aoi->correspondences[i];
  895.     CorrespondencePt* cpThis = cl->FindPoint (meshFrom);
  896.     if (cpThis != NULL) { // references this mesh
  897.       for (CorrespondencePt* cpOther = cl->points.begin();
  898.    cpOther < cl->points.end();
  899.    cpOther++) {
  900. // don't add us-us to list
  901. if (cpThis == cpOther)
  902.   continue;
  903. // and if from2to mode, only add corresp. involving To mesh
  904. if (meshTo != NULL && cpOther->mesh != meshTo)
  905.   continue;
  907. srcPt.push_back (cpThis->pt);
  908. cpThis->mesh->getMeshData()->xformPnt (srcPt.back());
  909. dstPt.push_back (cpOther->pt);
  910. cpOther->mesh->getMeshData()->xformPnt (dstPt.back());
  911.       }
  912.     }
  913.   }
  915.   double q[7];
  916.   horn_align (&srcPt[0], &dstPt[0], srcPt.size(), q);
  917.   Xform<float> xf = msFrom->getXform();
  918.   xf.addQuaternion (q, 7);
  919.   msFrom->setXform (xf);
  920.   theScene->computeBBox();
  922.   return TCL_OK;
  923. }
  926. static int
  927. CorrespRegAllToAll (AlignmentOverviewInfo* aoi)
  928. {
  929.   GlobalReg gr;
  930.   assert (aoi != NULL);
  932.   // for each pair of meshes, find all correspondences involving them
  933.   // and add pairs to globalreg gr
  935.   // so: iterate mesh pairs
  936.   int numSets = theScene->meshSets.size();
  937.   for (int iFrom = 0; iFrom < numSets; iFrom++) {
  938.     for (int iTo = iFrom + 1; iTo < numSets; iTo++) {
  939.       DisplayableMesh* mdFrom = theScene->meshSets[iFrom];
  940.       DisplayableMesh* mdTo = theScene->meshSets[iTo];
  941.       RigidScan* msFrom = mdFrom->getMeshData();
  942.       RigidScan* msTo = mdTo->getMeshData();
  943.       vector<Pnt3> pntFrom;
  944.       vector<Pnt3> pntTo;
  946.       // then iterate correspondences
  947.       for (int i = 0; i < aoi->correspondences.size(); i++) {
  948. CorrespondenceList* cl = aoi->correspondences[i];
  950. // if correspondence mentions both meshes currently under consideration
  951. CorrespondencePt* cpFrom = cl->FindPoint (mdFrom);
  952. CorrespondencePt* cpTo = cl->FindPoint (mdTo);
  954. if (cpFrom != NULL && cpTo != NULL) {
  955.   // then add points (in local coords) to pair data
  956.   pntFrom.push_back (cpFrom->pt);
  957.   pntTo.push_back (cpTo->pt);
  958. }
  959.       }
  961.       // now, if there were any correspondences between these meshes,
  962.       // add to globalreg
  963.       if (pntFrom.size() != 0) {
  964. gr.addPair(msFrom, msTo,
  965.    pntFrom, vector<Pnt3>(),
  966.    pntTo,   vector<Pnt3>());
  967. cout << pntFrom.size();
  968.       } else {
  969. cout << "no";
  970.       }
  972.       cout << " correspondences between "
  973.    << mdFrom->getName() << " and " << mdTo->getName()
  974.    << endl;
  975.     }
  976.   }
  977.   
  978.   gr.align (GLOBALREG_TOLERANCE, theScene->worldBbox());
  979.   theScene->computeBBox();
  981.   return TCL_OK;
  982. }
  987. //////////////////////////////////////////////////////////////////////
  988. //
  989. // 2nd method: drag-registration
  990. // UI and implementation follows
  991. //
  992. //////////////////////////////////////////////////////////////////////
  995. static void dragAlign (DisplayableMesh* mesh, AlignmentMethod method);
  997. int
  998. PlvDragRegisterCmd (ClientData clientData, Tcl_Interp *interp, 
  999.     int argc, char *argv[])
  1000. {
  1001.   // argument: name of mesh being registered (to all visible meshes)
  1002.   if (argc < 2) {
  1003.     interp->result = "Bad arguments to PlvDragRegisterCmd";
  1004.     return TCL_ERROR;
  1005.   }
  1007.   DisplayableMesh* ms = FindMeshDisplayInfo (argv[1]);
  1008.   if (ms == NULL) {
  1009.     interp->result = "Missing meshset in PlvDragRegisterCmd";
  1010.     return TCL_ERROR;
  1011.   }
  1013.   AlignmentMethod method = alignHorn;
  1014.   if (argc > 2) {
  1015.     if (       !strcmp (argv[2], "horn")) {
  1016.       method = alignHorn;
  1017.     } else if (!strcmp (argv[2], "chen")) {
  1018.       method = alignChenMedioni;
  1019.     }
  1020.   }
  1022.   dragAlign (ms, method);
  1023.   DisplayCache::InvalidateToglCache (toglCurrent);
  1025.   return TCL_OK;
  1026. }
  1029. #define ISBACKGROUND(x)  ((x)>=zBackground)
  1031. class SceneBuf
  1032. {
  1033. public:
  1034.   SceneBuf (int w, int h)
  1035.     {
  1036.       width = w;
  1037.       height = h;
  1039.       z = new unsigned int[width*height];
  1040.       zBackground = 0xffffffff;
  1042.       pt = new Pnt3*[w];
  1043.       for (int x = 0; x < w; x++) {
  1044. pt[x] = new Pnt3[h];
  1045.       }
  1046.     }
  1048.   ~SceneBuf()
  1049.     {
  1050.       delete[] z;
  1051.       for (int x = 0; x < width; x++) {
  1052. delete[] pt[x];
  1053.       }
  1054.       delete[] pt;
  1055.     }
  1057.   unsigned int* z;
  1058.   unsigned int zBackground;
  1059.   Pnt3** pt;
  1060.   int width, height;
  1061. };
  1064. // BUGBUG this next function is now very similar to ReadZBufferRect...
  1065. // should merge code
  1066. SceneBuf* 
  1067. GetScenePoints (DisplayableMesh* meshWithout = NULL)
  1068. {
  1069.   // allocate buffer storage
  1070.   Togl_MakeCurrent (toglCurrent);
  1071.   int width = Togl_Width (toglCurrent);
  1072.   int height = Togl_Height (toglCurrent);
  1073.   
  1074.   SceneBuf* sceneBuf = new SceneBuf (width, height);
  1076.   // clear z-buffer to get background z-value
  1077.   glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  1078.   unsigned int zBackground;
  1079.   glReadPixels (0, 0, 1, 1, GL_DEPTH_COMPONENT, GL_UNSIGNED_INT,
  1080. &zBackground);
  1081.   sceneBuf->zBackground = zBackground;
  1083.   // draw scene
  1084.   if (meshWithout)
  1085.     meshWithout->setVisible (false);
  1086.   drawInToglBuffer (toglCurrent, GL_BACK);
  1087.   if (meshWithout)
  1088.     meshWithout->setVisible (true);
  1090.   // and collect z-buffer
  1091.   glReadBuffer (GL_BACK);
  1092.   glReadPixels (0, 0, width, height, GL_DEPTH_COMPONENT,
  1093. GL_UNSIGNED_INT, sceneBuf->z);
  1095.   glMatrixMode (GL_MODELVIEW);
  1096.   glPushMatrix();
  1097.   glLoadIdentity ();
  1098.   tbView->applyXform();
  1099.   GLdouble model[16], proj[16];
  1100.   glGetDoublev(GL_MODELVIEW_MATRIX, model);    // applied from trackball
  1101.   glGetDoublev(GL_PROJECTION_MATRIX, proj);    // current GL state
  1102.   glPopMatrix();
  1104.   Xform<float> projMat = Xform<float>(proj) * Xform<float>(model);
  1105.   projMat.invert();
  1107.   for (int x = 0; x < width; x++) {
  1108.     for (int y = 0; y < height; y++) {
  1109.       int ofs = y*width + x;
  1110.       if (!ISBACKGROUND (sceneBuf->z[ofs])) {
  1111. float tx = (2. * x / width) - 1;
  1112. float ty = (2. * y / height) - 1;
  1113. float tz = (2. * sceneBuf->z[ofs] / (float)zBackground) - 1;
  1114. sceneBuf->pt[x][y] = projMat.unproject (tx, ty, tz);
  1115.       }
  1116.       else
  1117. sceneBuf->pt[x][y] = Pnt3(0.0);
  1118.     }
  1119.   }
  1121.   return sceneBuf;
  1122. }
  1125. void
  1126. GetMeshSceneOverlapPoints (DisplayableMesh* meshDisplay,
  1127.    const SceneBuf& sceneBuf,
  1128.    vector<Pnt3>& ptMesh,
  1129.    vector<Pnt3>& ptScene,
  1130.    vector<int>* ptSceneOfs)
  1131. {
  1132.   // already have rendering of scene without mesh
  1133.   // render mesh alone, to back buffer and collect z information
  1135.   // allocate buffer storage
  1136.   Togl_MakeCurrent (toglCurrent);
  1137.   int width = Togl_Width (toglCurrent);
  1138.   int height = Togl_Height (toglCurrent);
  1139.   
  1140.   unsigned int* bufMesh = new unsigned int[width*height];
  1142.   // now draw mesh alone
  1143.   glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  1144.   // but first, while buffer is clear, get background z-value
  1145.   unsigned int zBackground;
  1146.   glReadPixels (0, 0, 1, 1, GL_DEPTH_COMPONENT, GL_UNSIGNED_INT,
  1147. &zBackground);
  1149.   glMatrixMode (GL_MODELVIEW);
  1150.   glPushMatrix();
  1151.   meshDisplay->getMeshData()->gl_xform();
  1152.   meshDisplay->drawList();
  1153.   glFinish();
  1154.   glPopMatrix();
  1155.   // and collect z-buffer
  1156.   glReadPixels (0, 0, width, height, GL_DEPTH_COMPONENT,
  1157. GL_UNSIGNED_INT, bufMesh);
  1160.   glMatrixMode (GL_MODELVIEW);
  1161.   glPushMatrix();
  1162.   glLoadIdentity ();
  1163.   tbView->applyXform();
  1164.   GLdouble model[16], proj[16];
  1165.   glGetDoublev(GL_MODELVIEW_MATRIX, model);    // applied from trackball
  1166.   glGetDoublev(GL_PROJECTION_MATRIX, proj);    // current GL state
  1167.   glPopMatrix();
  1169.   Xform<float> projMat = Xform<float>(proj) * Xform<float>(model);
  1170.   projMat.invert();
  1172.   // iterate points that overlap, untransform and add to point lists
  1173.   for (int x = 0; x < width; x++) {
  1174.     for (int y = 0; y < height; y++) {
  1175.       int ofs = y*width + x;
  1176.       if (!ISBACKGROUND (sceneBuf.z[ofs]) &&
  1177.   !ISBACKGROUND (bufMesh[ofs])) {
  1178. // time to unproject
  1179. float tx = (2. * x / width) - 1;
  1180. float ty = (2. * y / height) - 1;
  1181. float tz = (2. * bufMesh[ofs] / (float)zBackground) - 1;
  1182. Pnt3 unproj = projMat.unproject (tx, ty, tz);
  1184. ptScene.push_back (sceneBuf.pt[x][y]);
  1185. if (ptSceneOfs != NULL)
  1186.   ptSceneOfs->push_back (ofs);
  1187. ptMesh.push_back (unproj);
  1189. #if 0
  1190. Pnt3 fast = projMat.unproject_fast (tx, ty, tz);
  1191. SHOW (fast - unproj);
  1192. #endif
  1193. #if 0
  1194. SHOW(ofs);
  1195. SHOW(bufScene[ofs]);
  1196. SHOW(bufMesh[ ofs]);
  1197. SHOW(Pnt3(scenePos));
  1198. SHOW(Pnt3( meshPos));
  1199. #endif
  1200.       }
  1201.     }
  1202.   }
  1204.   printf ("Have %ld overlapping pointsn", ptScene.size());
  1206.   delete[] bufMesh;
  1207. }
  1210. static void 
  1211. calculateNormals (vector<Pnt3>& normals, SceneBuf* sceneBuf,
  1212.   const vector<int>& sampPos)
  1213. {
  1214.   Pnt3 norm;
  1216.   for (int i = 0; i < normals.size(); i++) {
  1217.     // calculate and average cross product of all (possibly 8)
  1218.     // triangles sharing this point and its neighbors
  1219.     norm = 0.0;
  1221.     // first, calculate ray from this point to each of its
  1222.     // 8 neighbors
  1223.     vector<Pnt3> rays;
  1224.     rays.reserve (normals.size());
  1225.     int ofs = sampPos[i];
  1226.     int ptX = ofs % sceneBuf->width;
  1227.     int ptY = ofs / sceneBuf->width;
  1228.     for (int ix = -1; ix <= 1; ix++) {
  1229.       for (int iy = -1; iy <= 1; iy++) {
  1230. if (ix == 0 && iy == 0) continue;
  1231. int x = ptX + ix; int y = ptY + iy;
  1232. int iofs = y * sceneBuf->width + x;
  1233. if (sceneBuf->z[iofs] >= sceneBuf->zBackground)
  1234.   continue; // no data at this point
  1236. rays.push_back (sceneBuf->pt[x][y] - 
  1237. sceneBuf->pt[ptX][ptY]);
  1238.       }
  1239.     }
  1241.     // now we have up to 8 rays from chosen point to its existing
  1242.     // neighbors
  1243.     // calculate cross products and add to norm
  1244.     for (int ray = 0; ray < rays.size(); ray++) {
  1245.       Pnt3 ray1 = rays[ray];
  1246.       Pnt3 ray2 = rays[(ray+1)%rays.size()];
  1248.       Pnt3 tn = (cross (ray1, ray2)).normalize();
  1249.       cout << "SubNormal " << ray << ":";
  1250.       SHOW (tn);
  1252.       norm += (cross (ray1, ray2)).normalize();
  1253.     }
  1255.     normals[i] = norm.normalize();
  1256.     SHOW(normals[i]);
  1257.   }
  1258. }
  1261. static void 
  1262. dragAlign (DisplayableMesh* mesh, AlignmentMethod method)
  1263. {
  1264.   const int nIterations = 20;
  1265.   const int nPtsPerIter = 8;
  1267.   RigidScan* scan = mesh->getMeshData();
  1268.   Xform<float> orig = scan->getXform();
  1269.   Xform<float> best = orig;
  1270.   float bestCost = -1;
  1272.   // restrict to small drawing area to speed up buffer reading
  1273.   int oldResCap = theResCap;
  1274.   theResCap = 100;
  1275.   // calculate minimum allowed mesh overlap, in pixels
  1276.   int minOverlap = (theResCap * theResCap) * 0.01; // 1% overlap minimum
  1278.   printf ("Starting drag alignment (%d iterations)n",
  1279.   nIterations);
  1281.   SceneBuf* sceneBuf = GetScenePoints (mesh);
  1283.   // establish initial correspondences
  1284.   vector<Pnt3> ptOrigMesh, ptOrigScene;
  1285.   vector<int> ptSceneOfs;
  1286.   GetMeshSceneOverlapPoints (mesh, *sceneBuf, 
  1287.      ptOrigMesh, ptOrigScene,
  1288.      method == alignChenMedioni
  1289.      ? &ptSceneOfs : NULL);
  1291.   for (int iter = 0; iter < nIterations; iter++) {
  1292.     vector<Pnt3> ptMesh, ptScene;
  1294.     // randomly choose 8 points from overlap region
  1295.     // and align using them
  1296.     vector<Pnt3> subPtMesh(nPtsPerIter), subPtScene(nPtsPerIter);
  1297.     vector<int> subSampPos(nPtsPerIter);
  1298.     for (int iPt = 0; iPt < nPtsPerIter; iPt++) {
  1299.       int iPos = drand48() * ptOrigMesh.size();
  1300.       subPtMesh[iPt] =  ptOrigMesh [iPos];
  1301.       subPtScene[iPt] = ptOrigScene[iPos];
  1302.       if (ptSceneOfs.size() > iPt)
  1303. subSampPos[iPt] = ptSceneOfs[iPos];
  1304.     }
  1306.     double q[7];
  1307.     Xform<float> current (orig);
  1308.     switch (method) {
  1309.     case alignChenMedioni: {
  1310.       vector<Pnt3> nrmScene (nPtsPerIter);
  1311.       calculateNormals (nrmScene, sceneBuf, subSampPos);
  1312.       chen_medioni (&subPtMesh[0], &subPtScene[0], &nrmScene[0],
  1313.     nPtsPerIter, q);
  1314.       }
  1315.       break;
  1317.     case alignHorn:
  1318.     default:
  1319.       horn_align (&subPtMesh[0], &subPtScene[0], nPtsPerIter, q);
  1320.     }
  1321.     current.addQuaternion (q, 7);
  1323.     // rerender using new transformation, to evaluate it
  1324.     scan->setXform (current);
  1325.     // this is unnecessary for algorithm, but looks neat:
  1326.     drawInToglBuffer (toglCurrent, GL_FRONT);
  1327.     GetMeshSceneOverlapPoints (mesh, *sceneBuf, ptMesh, ptScene,
  1328.        NULL);
  1329.     scan->setXform (orig);
  1331.     // if overlap is too small, reject this guess outright
  1332.     if (ptMesh.size() < minOverlap) {
  1333.       printf ("tOverlap region too small (%ld points)n", 
  1334.       ptMesh.size());
  1335.       continue;
  1336.     }
  1338.     // calc cost: average distance between corresponding points
  1339.     float cost = 0;
  1340.     for (int i = 0; i < ptMesh.size(); i++) {
  1341.       cost += dist (ptMesh[i], ptScene[i]);
  1342.     }
  1343.     cost /= ptMesh.size();
  1344. printf ("tCost for %ld points is %gn", ptMesh.size(), cost);
  1346.     // update best if this is better
  1347.     if (cost < bestCost || bestCost < 0) {
  1348.       best = current;
  1349.       bestCost = cost;
  1350.     }
  1351.   }
  1353.   // apply best transformation
  1354.   printf ("Best cost is %gn", bestCost);
  1355.   scan->setXform (best);
  1357.   theResCap = oldResCap;
  1358.   delete sceneBuf;
  1359. }
  1364. //////////////////////////////////////////////////////////////////
  1365. //
  1366. // 3rd method: global registration
  1367. // code and UI follows
  1368. //
  1369. //////////////////////////////////////////////////////////////////
  1372. int
  1373. PlvGlobalRegistrationCmd (ClientData clientData, Tcl_Interp *interp, 
  1374.   int argc, char *argv[])
  1375. {
  1376.   if (argc < 2) {
  1377.     interp->result = "Bad argument to PlvGlobalRegistrationCmd";
  1378.     return TCL_ERROR;
  1379.   }
  1381.   GlobalReg* gr = theScene->globalReg;
  1382.   assert (gr != NULL);
  1384.   if (       !strcmp (argv[1], "reset")) {
  1385.     delete gr;
  1386.     theScene->globalReg = new GlobalReg;
  1387.   } else if (!strcmp (argv[1], "init_import")) {
  1388.     gr->initial_import();
  1389.   } else if (!strcmp (argv[1], "re_import")) {
  1390.     gr->perform_import();
  1391.   } else if (!strcmp (argv[1], "register")) {
  1392.     if (argc < 3) {
  1393.       interp->result = "Bad argument to PlvGlobalRegistrationCmd";
  1394.       return TCL_ERROR;
  1395.     }
  1396.     TbObj* scanToMove = NULL;   // default to registering all scans
  1397.     TbObj* scanToMoveTo = NULL; // default to registering to all scans
  1398.     if (argc > 3) {
  1399.       DisplayableMesh* dm = FindMeshDisplayInfo (argv[3]);
  1400.       if (dm != NULL) {
  1401. scanToMove = dm->getMeshData();
  1402.       }
  1403.       if (scanToMove == NULL) {
  1404. cerr << "Scan " << argv[3] << " does not exist." << endl;
  1405. interp->result =
  1406.   "Bad mesh passed to GlobalReg 1->all align";
  1407. return TCL_ERROR;
  1408.       }
  1409.     }
  1411.     if (argc > 4) {
  1412.       DisplayableMesh* dm = FindMeshDisplayInfo (argv[4]);
  1413.       if (dm != NULL) {
  1414. scanToMoveTo = dm->getMeshData();
  1415.       }
  1416.       if (scanToMoveTo == NULL) {
  1417. cerr << "Scan " << argv[4] << " does not exist." << endl;
  1418. interp->result =
  1419.   "Bad mesh passed to GlobalReg 1->2 align";
  1420. return TCL_ERROR;
  1421.       }
  1422.     }
  1424.     // to put in code for normalizing, comment out this align call
  1425.     // and call the one below it
  1426.     gr->align (atof(argv[2]),
  1427.        scanToMove, scanToMoveTo);
  1428.     //gr->align (atof(argv[2]), theScene->worldBbox(),
  1429.     //       scanToMove, scanToMoveTo);
  1430.     theScene->computeBBox();
  1432.   } else if (!strcmp (argv[1], "pairstatus")) {
  1433.     if (argc < 4) {
  1434.       interp->result = "Bad # args";
  1435.       return TCL_ERROR;
  1436.     }
  1437.     DisplayableMesh* dm1 = FindMeshDisplayInfo (argv[2]);
  1438.     DisplayableMesh* dm2 = FindMeshDisplayInfo (argv[3]);
  1439.     if (!dm1 || !dm2) {
  1440.       //interp->result = "Bad mesh specified - sczRegCmds.cc:PlvGlobalRegistrationCmd pairstatus";
  1441.       //      rihan 7/9/01
  1442.       interp->result = "0";
  1443.     } else {
  1444.       bool bTrans = false;
  1445.       if (argc > 4 && !strcmp (argv[4], "transitive"))
  1446. bTrans = true;
  1447.       bool manual;
  1448.       bool reg = gr->pairRegistered (dm1->getMeshData(),
  1449.      dm2->getMeshData(),
  1450.      manual,
  1451.      bTrans);
  1452.       interp->result = reg ? "1" : "0";
  1453.     }
  1454.   } else if (!strcmp (argv[1], "listpairsfor")) {
  1455.     if (argc < 3) {
  1456.       interp->result = "Bad # args";
  1457.       return TCL_ERROR;
  1458.     }
  1459.     DisplayableMesh* dm = FindMeshDisplayInfo (argv[2]);
  1460.     if (!dm) {
  1461.       interp->result = "Bad mesh specified - sczRegCmds.cc:PlvGlobalRegistrationCmd listpairsfor";
  1462.       return TCL_ERROR;
  1463.     }
  1464.     
  1465.     bool bTrans = false;
  1466.     if (argc > 3 && !strcmp (argv[3], "transitive"))
  1467.       bTrans = true;
  1468.     crope partners = gr->list_partners(dm->getMeshData(), bTrans);
  1469.     Tcl_SetResult (interp, (char*)partners.c_str(), TCL_VOLATILE);
  1471.   } else if (!strcmp (argv[1], "dumpallpairs")) {
  1472.     std::string tcl_cmd;
  1473.     if (argc > 3) {
  1474.       float showThresh = atof(argv[3]);
  1475.       tcl_cmd = gr->dump_meshpairs(atoi(argv[2]),
  1476.     showThresh);
  1477.     } else {
  1478.       tcl_cmd = gr->dump_meshpairs(atoi(argv[2]));
  1479.     }
  1480.     Tcl_SetResult (interp, (char*) tcl_cmd.c_str(), TCL_VOLATILE);
  1482.   } else if (!strcmp (argv[1], "getpaircount")) {
  1483.     if (argc < 3) {
  1484.       interp->result = "bad # args";
  1485.       return TCL_ERROR;
  1486.     }
  1487.     DisplayableMesh* dm = FindMeshDisplayInfo (argv[2]);
  1488.     if (!dm) {
  1489.       interp->result = "Bad mesh specified - sczRegCmds.cc:PlvGlobalRegistrationCmd getpaircount";
  1490.       return TCL_ERROR;
  1491.     }
  1493.     int nPartners = gr->count_partners (dm->getMeshData());
  1494.     char buf[10];
  1495.     sprintf (buf, "%d", nPartners);
  1496.     Tcl_SetResult (interp, buf, TCL_VOLATILE);
  1498.   } else if (!strcmp (argv[1], "groupstatus")) {
  1499.     gr->dump_connected_groups();
  1501.   } else if (!strcmp (argv[1], "killpair")) {
  1502.     if (argc < 4) {
  1503.       interp->result =
  1504. "Bad args to PlvGlobalRegistrationCmd killpair";
  1505.       return TCL_ERROR;
  1506.     }
  1507.     DisplayableMesh* dm1 = FindMeshDisplayInfo (argv[2]);
  1508.     DisplayableMesh* dm2 = FindMeshDisplayInfo (argv[3]);
  1509.     // "mesh *" is allowed to leave dm2 NULL and delete 
  1510.     // all involving mesh
  1511.     if (!dm1 || (!dm2 && argv[3][0] != '*')) {
  1512.       interp->result = 
  1513. "Bad mesh in PlvGlobalRegistrationCmd killpair";
  1514.       return TCL_ERROR;
  1515.     }
  1516.     if (dm2)
  1517.       gr->deletePair (dm1->getMeshData(),
  1518.       dm2->getMeshData(), true);
  1519.     else
  1520.       gr->deleteAllPairs (dm1->getMeshData(), true);
  1522.   } else if (!strcmp (argv[1], "point_pair_count")) {
  1523.     if (argc < 4) {
  1524.       interp->result =
  1525. "Bad args to PlvGlobalRegistrationCmd point_pair_count";
  1526.       return TCL_ERROR;
  1527.     }
  1528.     DisplayableMesh* dm1 = FindMeshDisplayInfo (argv[2]);
  1529.     DisplayableMesh* dm2 = FindMeshDisplayInfo (argv[3]);
  1530.     // "mesh *" is allowed to leave dm2 NULL and deals with 
  1531.     // all involving meshes
  1532.     if (!dm1 || (!dm2 && argv[3][0] != '*')) {
  1533.       interp->result = 
  1534. "Bad mesh in PlvGlobalRegistrationCmd point_pair_count";
  1535.       return TCL_ERROR;
  1536.     }
  1537.     if (dm2)  gr->showPointpairCount(dm1->getMeshData(),
  1538.      dm2->getMeshData());
  1539.     else      gr->showPointpairCount(dm1->getMeshData());
  1541.   } else if (!strcmp (argv[1], "deleteautopairs")) {
  1542.     if (argc < 3 || argc > 4) {
  1543.       interp->result =
  1544. "Bad args to PlvGlobalRegistrationCmd deleteautopairs";
  1545.       return TCL_ERROR;
  1546.     }
  1547.     TbObj* tb = NULL;
  1548.     if (argc == 4) {
  1549.       DisplayableMesh* dm = FindMeshDisplayInfo (argv[3]);
  1550.       if (!dm) {
  1551. interp->result = 
  1552.   "Bad mesh in PlvGlobalRegistrationCmd deleteautopairs";
  1553. return TCL_ERROR;
  1554.       }
  1555.       tb = dm->getMeshData();
  1556.     }
  1557.     gr->deleteAutoPairs(atof(argv[2]), tb);
  1559.   } else if (!strcmp (argv[1], "getstats")) {
  1560.     DisplayableMesh *dm1 = NULL, *dm2 = NULL;
  1561.     if (argc > 3) {
  1562.       dm1 = FindMeshDisplayInfo (argv[2]);
  1563.       dm2 = FindMeshDisplayInfo (argv[3]);
  1564.     }
  1565.     if (!dm1 || !dm2) {
  1566.       interp->result = "Bad mesh passed to getstats";
  1567.       return TCL_ERROR;
  1568.     }
  1570.     int iQuality, nPoints;
  1571.     time_t date;
  1572.     bool bManual;
  1573.     float errs[5];
  1574.     if (!gr->getPairingStats (dm1->getMeshData(), dm2->getMeshData(),
  1575.       bManual, iQuality, nPoints, date, errs)) {
  1576.       interp->result = "Can't get stats for given mesh";
  1577.       return TCL_ERROR;
  1578.     }
  1580.     struct tm* pTime = localtime (&date);
  1581.     char szStats[200];
  1582.     sprintf (szStats, "%c %d %d %.3f %.3f %.3f %.3f %.3f %4d/%02d/%02d",
  1583.      bManual ? 'M' : 'A',
  1584.      iQuality, nPoints,
  1585.      errs[0], errs[1], errs[2], errs[3], errs[4],
  1586.      1900 + pTime->tm_year, 1 + pTime->tm_mon, pTime->tm_mday);
  1587.     Tcl_SetResult (interp, szStats, TCL_VOLATILE);
  1589.   } else if (!strcmp (argv[1], "getstatsummary")) {
  1591.     int qual[4];    // unknown/bad/fair/good
  1592.     float err[3];   // min/avg/max
  1593.     int count[3];   // total/man/auto
  1595.     DisplayableMesh* dm = NULL;
  1596.     if (argc > 2)
  1597.       dm = FindMeshDisplayInfo (argv[2]);
  1598.     if (!dm && 0 != strcmp (argv[2], "*")) {
  1599.       interp->result = "Bad mesh passed to getstatsummary";
  1600.       return TCL_ERROR;
  1601.     }
  1603.     TbObj* tb = dm ? dm->getMeshData() : NULL;
  1605.     // TODO
  1606.     GlobalReg::ERRMETRIC metric = GlobalReg::errmetric_pnt;
  1608.     if (!gr->getPairingSummary (tb, metric, count, err, qual)) {
  1609.       interp->result = "Can't get summary for given mesh";
  1610.       return TCL_ERROR;
  1611.     }
  1613.     char szStats[200];
  1614.     sprintf (szStats, "%d %d %d %.3f %.3f %.3f %d %d %d %d",
  1615.      count[0], count[1], count[2],
  1616.      err[0], err[1], err[2],
  1617.      qual[0], qual[1], qual[2], qual[3]);
  1618.     Tcl_SetResult (interp, szStats, TCL_VOLATILE);
  1620.   } else {
  1621.     interp->result = "Bad argument to PlvGlobalRegistrationCmd";
  1622.     return TCL_ERROR;
  1623.   }
  1625.   return TCL_OK;
  1626. }
  1628. //////////////////////////////////////////////////////////////////
  1629. //
  1630. // 4th method: ICP
  1631. // code and UI follows
  1632. //
  1633. //////////////////////////////////////////////////////////////////
  1636. ICP<RigidScan*> icp;
  1638. int
  1639. PlvRegIcpCmd(ClientData clientData, Tcl_Interp *interp, 
  1640.   int argc, char *argv[])
  1641. {
  1642.   if (argc != 14) {
  1643.     interp->result = "Usage: plv_icpregister n"
  1644.       "t<sampling density [0,1]>n"
  1645.       "t<normal-space sampling {0|1}>n"
  1646.       "t<number of iterations [1,20]>n"
  1647.       "t<culling percentage [0,99]>n"
  1648.       "t<no boundary targets {0|1}>n"
  1649.       "t<optimization method {point|plane}>n"
  1650.       "t<source mesh>n"
  1651.       "t<target mesh>n"
  1652.       "t<edge threshold kind {abs|rel}>n"
  1653.       "t<edge threshold value>n"
  1654.       "t<save results for globalreg {0|1}>n"
  1655.       "t<save at most n pairs [0,a_big_number]>n"
  1656.       "t<quality rating [0..3]>n";
  1657.     return TCL_ERROR;
  1658.   }
  1660.   DisplayableMesh *mdSrc = FindMeshDisplayInfo(argv[7]);
  1661.   DisplayableMesh *mdTrg = FindMeshDisplayInfo(argv[8]);
  1662.   assert(mdSrc && mdTrg);
  1664.   RigidScan* mSrc = mdSrc->getMeshData();
  1665.   RigidScan* mTrg = mdTrg->getMeshData();
  1666.   assert(mSrc && mTrg);
  1668.   // run the alignment
  1669.   icp.set(mSrc, mTrg);
  1670.   icp.allow_bdry = !atoi(argv[5]);
  1671.   float avgError = icp.align(atof(argv[1]), atoi(argv[2]), atoi(argv[3]), atoi(argv[4]),
  1672.      argv[6][1] == 'o', argv[9][0] == 'a',
  1673.      atof(argv[10]));
  1675.   // if requested, add pairs to globalreg
  1676.   bool bSaveForGlobal = atoi (argv[11]);
  1677.   if (bSaveForGlobal) {
  1678.     assert (theScene->globalReg != NULL);
  1679.     vector<Pnt3> ptSrc, ptTrg, nrmSrc, nrmTrg;
  1680.     icp.get_pairs_for_global (ptSrc, nrmSrc, ptTrg, nrmTrg);
  1681.     
  1682.     if (ptSrc.size() || ptTrg.size()) {
  1683.       cout << "Aligned pairs saved for global registration: "
  1684.    << mdSrc->getName() << " and " 
  1685.    << mdTrg->getName() << endl;
  1687.       int max_pairs = atoi(argv[12]);
  1688.       int quality = atoi(argv[13]);
  1690.       theScene->globalReg->addPair(mSrc, mTrg, ptSrc, nrmSrc,
  1691.    ptTrg, nrmTrg,
  1692.    mTrg->getXform().fast_invert() *
  1693.    mSrc->getXform(),
  1694.    true, max_pairs, true, quality);
  1695.     }
  1696.   }
  1698.   char szError[20];
  1699.   sprintf (szError, "%g", avgError);
  1700.   Tcl_SetResult (interp, szError, TCL_VOLATILE);
  1702.   return TCL_OK;
  1703. }
  1706. int
  1707. PlvRegIcpMarkQualityCmd(ClientData clientData, Tcl_Interp *interp, 
  1708. int argc, char *argv[])
  1709. {
  1710.   if (argc != 4) {
  1711.     interp->result = "Bad args to PlvRegIcpMarkQualityCmd";
  1712.     return TCL_ERROR;
  1713.   }
  1715.   DisplayableMesh *mdSrc = FindMeshDisplayInfo(argv[1]);
  1716.   DisplayableMesh *mdTrg = FindMeshDisplayInfo(argv[2]);
  1717.   assert(mdSrc && mdTrg);
  1719.   RigidScan* mSrc = mdSrc->getMeshData();
  1720.   RigidScan* mTrg = mdTrg->getMeshData();
  1721.   assert(mSrc && mTrg);
  1723.   int quality = atoi (argv[3]);
  1725.   if (quality < 0 || quality > 3) {
  1726.     interp->result = "Quality out of range in PlvRegIcpMarkQualityCmd";
  1727.     return TCL_ERROR;
  1728.   }
  1730.   if (!theScene->globalReg->markPairQuality (mSrc, mTrg, quality)) {
  1731.     interp->result = "General failure in PlvRegIcpMarkQualityCmd";
  1732.     return TCL_ERROR;
  1733.   }
  1735.   return TCL_OK;
  1736. }
  1739. int
  1740. PlvShowIcpLinesCmd(ClientData clientData, Tcl_Interp *interp, 
  1741.   int argc, char *argv[])
  1742. {
  1743.   if (argc < 2) {
  1744.     interp->result = "Bad arguments to PlvShowIcpLinesCmd";
  1745.     return TCL_ERROR;
  1746.   }
  1748.   bool show = atoi (argv[1]);
  1749.   if (show)
  1750.     icp.show_lines();
  1751.   else
  1752.     icp.hide_lines();
  1754.   return TCL_OK;
  1755. }
  1758. int
  1759. SczAutoRegisterCmd(ClientData clientData, Tcl_Interp *interp, 
  1760.       int argc, char *argv[])
  1761. {
  1762.   if (argc < 7) {
  1763.     interp->result = "Usage: scz_auto_register n"
  1764.       "t[visible | current] [visible | all] [name_of_curr_mesh] "
  1765.       "[error_thresh] [nTargetPairs] [bPreserveExisting] [bNormSSample]n";
  1766.     return TCL_ERROR;
  1767.   }
  1769.   // pair with visible/all meshes
  1770.   bool bFromVisible = !strcmp("visible", argv[1]);
  1771.   bool bToVisible   = !strcmp("visible", argv[2]);
  1773.   // error threshold
  1774.   float final_error_thresh = atof(argv[4]);
  1775.   
  1776.   if (final_error_thresh <= 0) {
  1777.     interp->result = "Error threshold has to be positive!n";
  1778.     return TCL_ERROR;
  1779.   }
  1781.   // target number of pairs to yield from alignment
  1782.   int nTargetPairs = atoi (argv[5]);
  1783.   if (nTargetPairs <= 0)
  1784.     nTargetPairs = 1000;
  1786.   // overwrite existing correspondences, or ignore meshes that
  1787.   // are already  paired?
  1788.   bool bOverwrite = !atoi (argv[6]);
  1790.   bool bNormSSample = atoi(argv[7]);
  1792.   // ctor
  1793.   AutoICP<RigidScan*> auto_align (theScene->globalReg,
  1794.   final_error_thresh);
  1796.   vector<DisplayableMesh*>& scans = theScene->meshSets;
  1798.   cout << "Calculating pairs:" << endl << endl;
  1800.   DisplayableMesh *currMesh = FindMeshDisplayInfo (argv[3]);
  1801.   if (!currMesh) {
  1802.     interp->result =
  1803.       "Couldn't find the current mesh!";
  1804.     return TCL_ERROR;
  1805.   }
  1807.   DisplayableMesh** begin = scans.begin();
  1808.   DisplayableMesh** end = scans.end();
  1809.   DisplayableMesh** first, ** second;
  1811.   GlobalReg *gr = theScene->globalReg;
  1813.   int cnt = 0;
  1814.   for (first = begin; first != end; first++) {
  1815.     if (( bFromVisible && (*first)->getVisible()) ||
  1816. (!bFromVisible && *first == currMesh)) {
  1817.       for (second = begin; second != end; second++) {
  1818. if (first == second) continue;
  1820. if (bToVisible && !(*second)->getVisible()) continue;
  1822. if (bFromVisible && (*second)->getVisible() &&
  1823.     second < first) continue;
  1825. RigidScan* rs1 = (*first)->getMeshData();
  1826. RigidScan* rs2 = (*second)->getMeshData();
  1828. bool manual;
  1829. if (gr->pairRegistered(rs1, rs2, manual)) {
  1830.   if (manual)      continue;
  1831.   if (!bOverwrite) continue;
  1832. }
  1834. if (rs1->num_vertices() == 0 || 
  1835.     rs2->num_vertices() == 0) { 
  1836.   continue;
  1837. }
  1839. cnt++;
  1840.       }
  1841.     }
  1842.   }
  1844.   bool bBail = false;
  1845.   Progress progress (cnt, "Calculating pairs");
  1847.   for (first = begin; first != end; first++) {
  1848.     if (( bFromVisible && (*first)->getVisible()) ||
  1849. (!bFromVisible && *first == currMesh)) {
  1850.       for (second = begin; second != end; second++) {
  1851. if (first == second) continue;
  1853. if (bToVisible && !(*second)->getVisible()) continue;
  1855. if (bFromVisible && (*second)->getVisible() &&
  1856.     second < first) continue;
  1858. RigidScan* rs1 = (*first)->getMeshData();
  1859. RigidScan* rs2 = (*second)->getMeshData();
  1861. // are these meshes already registered?
  1862. // never override a manually registered pair...
  1863. bool manual;
  1864. if (theScene->globalReg->pairRegistered(rs1, rs2,
  1865. manual)) {
  1866.   if (manual) {
  1867.     cerr << "Don't override manual alignment for "
  1868.  << (*first)->getName()
  1869.  << " and " << (*second)->getName() << endl;
  1870.     continue;
  1871.   }
  1872.   if (!bOverwrite) {
  1873.     cerr << "Already paired: " << (*first)->getName()
  1874.  << " and " << (*second)->getName() << endl;
  1875.     continue;
  1876.   }
  1877. }
  1879. //
  1880. // ok, passes all tests... register
  1881. //
  1883. // TODO: set threshold based on mesh resolutions
  1885. if (rs1->num_vertices() == 0 || 
  1886.     rs2->num_vertices() == 0) { 
  1887.   cout << "Skipping empty scan" << endl;
  1888.   continue;
  1889. }
  1891. if (!progress.updateInc()) {
  1892.   cerr << "Warning: cancelled; "
  1893.        << "not all scan pairs considered." << endl;
  1894.   bBail = true;
  1895.   break;
  1896. }
  1898. // ok, try to align
  1899. cout << "Trying " << (*first)->getName() << " and " 
  1900.      << (*second)->getName() << endl;
  1902. // normal-space sample: bNormSSample
  1903. if (auto_align.add_pair (rs1, rs2, nTargetPairs, bNormSSample)) {
  1904.   cout << "Paired " << (*first)->getName()
  1905.        << " and " << (*second)->getName() << endl;
  1906. }
  1907.       }
  1908.     }
  1909.     
  1910.     if (bBail) break;
  1911.   }
  1912.   
  1913.   return TCL_OK;
  1914. }