personalrobots-commit

[Personalrobots-commit] SF.net SVN: personalrobots: [76] pkg/trunk/libTF/simple/libTF.cc

[<tf...@us...>]

Revision: 76
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=76&view=rev
Author:   tfoote
Date:     2008-04-02 15:26:51 -0700 (Wed, 02 Apr 2008)

Log Message:
-----------
commenting and fixing case of no root

Modified Paths:
--------------
    pkg/trunk/libTF/simple/libTF.cc

Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc	2008-04-02 22:10:38 UTC (rev 75)
+++ pkg/trunk/libTF/simple/libTF.cc	2008-04-02 22:26:51 UTC (rev 76)
@@ -122,34 +122,45 @@
   unsigned int counter = 0;  //A counter to keep track of how deep we've descended
   while (true)
     {
+      if (frame == NO_PARENT)
+	break;
       mTfLs.inverseTransforms.push_back(frame);
-      if (frame == NO_PARENT  || frame == ROOT_FRAME) //Descend until we reach the root node or don't have a parent
+      if (frame == ROOT_FRAME) //Descend until we reach the root node or don't have a parent
 	break;
+
+      //Check that we arn't going somewhere illegal 
       if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
+
+      // Descent to parent frame
       //   std::cout <<"Frame: " << frame <<std::endl;
       frame = getFrame(frame)->getParent();
-      /* Check if we've gone too deep.  Probably a loop */
+
+      /* Check if we've gone too deep.  A loop in the tree would cause this */
       if (counter++ > MAX_GRAPH_DEPTH)
 	throw(MaxSearchDepth);
     }
-  //  mTfLs.inverseTransforms.push_back(0);
   
   frame = source_frame;
   counter = 0;
-  while (true)//(frame != NO_PARENT  && frame != ROOT_FRAME) //Descend until we reach the root node or don't have a parent
+  while (true)
     {
+      if (frame == NO_PARENT)
+	break;
       mTfLs.forwardTransforms.push_back(frame);
-      if (frame == NO_PARENT  || frame == ROOT_FRAME) //Descend until we reach the root node or don't have a parent
+      if (frame == ROOT_FRAME) //Descend until we reach the root node or don't have a parent
 	break;
+
+      //Check that we aren't going somewhere illegal
       if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
+
+      //Descent to parent frame
       frame = getFrame(frame)->getParent();
-      /* Check if we've gone too deep.  Probably a loop */
+
+      /* Check if we've gone too deep.  A loop in the tree would cause this*/
       if (counter++ > MAX_GRAPH_DEPTH)
 	throw(MaxSearchDepth);
     }
-  //  mTfLs.forwardTransforms.push_back(0);
   
-  
   /* Make sure the end of the search shares a parent. */
   if (mTfLs.inverseTransforms.back() != mTfLs.forwardTransforms.back())
     throw(NoFrameConnectivity);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[Personalrobots-commit] SF.net SVN: personalrobots: [138] pkg/trunk/libTF/simple/libTF.cc

[<tf...@us...>]

Revision: 138
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=138&view=rev
Author:   tfoote
Date:     2008-04-18 12:46:20 -0700 (Fri, 18 Apr 2008)

Log Message:
-----------
cleanup of debugging and magic number

Modified Paths:
--------------
    pkg/trunk/libTF/simple/libTF.cc

Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc	2008-04-18 16:57:07 UTC (rev 137)
+++ pkg/trunk/libTF/simple/libTF.cc	2008-04-18 19:46:20 UTC (rev 138)
@@ -34,7 +34,7 @@
 
 RefFrame::RefFrame() :
   Quaternion3D(),
-  parent(0)
+  parent(TransformReference::NO_PARENT)
 {
   return;
 }
@@ -81,7 +81,6 @@
   NEWMAT::Matrix myMat(4,4);
   TransformLists lists = lookUpList(target_frame, source_frame);
   myMat = computeTransformFromList(lists,time);
-  //  std::cout << myMat;
   return myMat;
 }
 
@@ -90,11 +89,6 @@
 {
   TransformLists mTfLs;
 
-  //  std::vector<unsigned int> tList;
-  //  std::vector<unsigned int> sList;
-
-  //  std::vector<unsigned int> retVec;
-
   unsigned int frame = target_frame;
   unsigned int counter = 0;  //A counter to keep track of how deep we've descended
   while (true)
@@ -109,7 +103,6 @@
       if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
 
       // Descent to parent frame
-      //   std::cout <<"Frame: " << frame <<std::endl;
       frame = getFrame(frame)->getParent();
 
       /* Check if we've gone too deep.  A loop in the tree would cause this */
@@ -143,13 +136,11 @@
     throw(NoFrameConnectivity);
 
   /* Make sure that we don't have a no parent at the top */
-  //  std::cout << "Back = " << mTfLs.inverseTransforms.back()<<" " << mTfLs.forwardTransforms.back();
   if (mTfLs.inverseTransforms.back() == NO_PARENT ||  mTfLs.forwardTransforms.back() == NO_PARENT)
     throw(NoFrameConnectivity);
 
   while (mTfLs.inverseTransforms.back() == mTfLs.forwardTransforms.back())
     {
-      //      std::cout << "removing " << mTfLs.inverseTransforms.back() << std::endl;
       mTfLs.inverseTransforms.pop_back();
       mTfLs.forwardTransforms.pop_back();
     }
@@ -192,7 +183,6 @@
   for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
     {
       mstream << lists.inverseTransforms[i]<<", ";
-      //      retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix();
     }
   mstream << std::endl;
 
@@ -200,7 +190,6 @@
   for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++) 
     {
       mstream << lists.forwardTransforms[i]<<", ";
-      //      retMat *= getFrame(lists.inverseTransforms[lists.forwardTransforms.size() -1 - i])->getMatrix(); //Do this list backwards for it was generated traveling the wrong way
     }
   mstream << std::endl;
   return mstream.str();


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.