Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [73] pkg/trunk/libTF/simple
|
From: <tf...@us...> - 2008-04-02 18:06:36
|
Revision: 73
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=73&view=rev
Author: tfoote
Date: 2008-04-02 11:06:39 -0700 (Wed, 02 Apr 2008)
Log Message:
-----------
adding NO_PARENT and ROOT_FRAME semantics and catching a loop in search by using max depth
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-03-31 20:56:54 UTC (rev 72)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-02 18:06:39 UTC (rev 73)
@@ -119,29 +119,37 @@
// std::vector<unsigned int> retVec;
unsigned int frame = target_frame;
- while (frame != 0)
+ unsigned int counter = 0; //A counter to keep track of how deep we've descended
+ while (frame != NO_PARENT && frame != ROOT_FRAME) //Descend until we reach the root node or don't have a parent
{
if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
mTfLs.inverseTransforms.push_back(frame);
// std::cout <<"Frame: " << frame <<std::endl;
frame = getFrame(frame)->getParent();
+ /* Check if we've gone too deep. Probably a loop */
+ if (counter++ > MAX_GRAPH_DEPTH)
+ throw(MaxSearchDepth);
}
mTfLs.inverseTransforms.push_back(0);
frame = source_frame;
- while (frame != 0)
+ counter = 0;
+ while (frame != NO_PARENT && frame != ROOT_FRAME) //Descend until we reach the root node or don't have a parent
{
if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
mTfLs.forwardTransforms.push_back(frame);
frame = getFrame(frame)->getParent();
+ /* Check if we've gone too deep. Probably a loop */
+ 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);
- //todo fixme throw something
- // if (tList.size() <= 1) exit(-1);
- // if (sList.size() <= 1) exit(-1); // I think below will catch all these anyway
-
while (mTfLs.inverseTransforms.back() == mTfLs.forwardTransforms.back())
{
// std::cout << "removing " << mTfLs.inverseTransforms.back() << std::endl;
@@ -185,7 +193,7 @@
std::cout << "Inverse Transforms:" <<std::endl;
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
- std::cout << lists.inverseTransforms[i];
+ std::cout << lists.inverseTransforms[i]<<", ";
// retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix();
}
std::cout << std::endl;
@@ -193,7 +201,7 @@
std::cout << "Forward Transforms: "<<std::endl ;
for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
{
- std::cout << lists.forwardTransforms[i];
+ std::cout << 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
}
std::cout << std::endl;
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-03-31 20:56:54 UTC (rev 72)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-02 18:06:39 UTC (rev 73)
@@ -48,7 +48,13 @@
class TransformReference
{
public:
+ static const unsigned int ROOT_FRAME = 1;
+ static const unsigned int NO_PARENT = 0;
+
+ /* The maximum number of frames possible */
static const unsigned int MAX_NUM_FRAMES = 100;
+ /* The maximum number of times to descent before determining that graph has a loop. */
+ static const unsigned int MAX_GRAPH_DEPTH = 100;
/* Set a new frame or update an old one. */
void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double);
@@ -68,6 +74,7 @@
public:
virtual const char* what() const throw() { return "InvalidFrame"; }
} InvalidFrame;
+
/* An exception class to notify of no connection */
class ConnectivityException : public std::exception
{
@@ -75,14 +82,27 @@
virtual const char* what() const throw() { return "No connection between frames"; }
private:
} NoFrameConnectivity;
+
+ /* An exception class to notify of no connection */
+ class MaxDepthException : public std::exception
+ {
+ public:
+ virtual const char* what() const throw() { return "Search exceeded max depth. Probably a loop in the tree."; }
+ private:
+ } MaxSearchDepth;
+
+
private:
+ /* An accessor to get a frame, which will throw an exception if the frame is no there. */
inline RefFrame* getFrame(unsigned int frame_number) { if (frames[frame_number] == NULL) throw InvalidFrame; else return frames[frame_number];};
- /* The frames that the tree can be made of */
+
+ /* The pointers to potential frames that the tree can be made of.
+ * The frames will be dynamically created at run time when set the first time. */
RefFrame* frames[MAX_NUM_FRAMES];
- /* This struct is how the list of transforms are stored before being generated. */
+ /* This struct is how the list of transforms are stored before being passed to computeTransformFromList. */
typedef struct
{
std::vector<unsigned int> inverseTransforms;
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-03-31 20:56:54 UTC (rev 72)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-02 18:06:39 UTC (rev 73)
@@ -29,7 +29,7 @@
- mTR.set(1,2,1,1,1,dyaw,dp,dr);
+ mTR.set(10,2,1,1,1,dyaw,dp,dr);
mTR.set(2,3,1,1,1,dyaw,dp,dr);
mTR.set(3,5,dx,dy,dz,dyaw,dp,dr);
mTR.set(5,0,dx,dy,dz,dyaw,dp,dr);
@@ -38,20 +38,20 @@
mTR.set(8,7,1,1,1,dyaw,dp,dr);
try
{
- mTR.view(1,9);
+ mTR.view(10,9);
}
catch (TransformReference::LookupException &ex)
{
std::cout << "Caught " << ex.what()<<std::endl;
- mTR.view(1,8);
+ mTR.view(10,8);
}
- std::cout <<"Calling get(1,8)"<<std::endl;
+ std::cout <<"Calling get(10,8)"<<std::endl;
// NEWMAT::Matrix mat = mTR.get(1,1);
- NEWMAT::Matrix mat = mTR.get(1,8);
+ NEWMAT::Matrix mat = mTR.get(10,8);
std::cout << "Result" << std::endl << mat<< std::endl;
#ifdef DONOTUSE
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
