Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [87] pkg/trunk/libTF/simple
|
From: <tf...@us...> - 2008-04-03 19:37:06
|
Revision: 87
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=87&view=rev
Author: tfoote
Date: 2008-04-03 12:37:11 -0700 (Thu, 03 Apr 2008)
Log Message:
-----------
cleaning up and documenting libTF much more
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-04-03 08:58:53 UTC (rev 86)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-03 19:37:11 UTC (rev 87)
@@ -229,9 +229,9 @@
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);
+ // 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())
{
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-03 08:58:53 UTC (rev 86)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-03 19:37:11 UTC (rev 87)
@@ -33,10 +33,12 @@
/* Generate and return the transform associated with getting out of this frame. */
NEWMAT::Matrix getInverseMatrix();
+
+
private:
/* Storage of the parametsrs
* NOTE: Depending on if this is a 6dof or DH parameter the storage will be different. */
- double params[6];
+ double params[6];
/* A helper function to build a homogeneous transform based on 6dof parameters */
static bool fill_transformation_matrix(NEWMAT::Matrix& matrix_pointer, double ax,
@@ -57,26 +59,40 @@
class TransformReference
{
public:
- static const unsigned int ROOT_FRAME = 1;
- static const unsigned int NO_PARENT = 0;
+ /************* Constants ***********************/
+ static const unsigned int ROOT_FRAME = 1; //Hard Value for ROOT_FRAME
+ static const unsigned int NO_PARENT = 0; //Value for NO_PARENT
- /* 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;
+ static const unsigned int MAX_NUM_FRAMES = 100; /* The maximum number of frames possible */
+ static const unsigned int MAX_GRAPH_DEPTH = 100; /* The maximum number of times to descent before determining that graph has a loop. */
+ /* Constructor */
+ TransformReference();
+
+ /********** Mutators **************/
/* Set a new frame or update an old one. */
void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double);
+ // Possible exceptions TransformReference::LookupException
- /* Get the transform between two frames. */
+ /*********** Accessors *************/
+
+ /* Get the transform between two frames by frame ID. */
NEWMAT::Matrix get(unsigned int target_frame, unsigned int source_frame);
+ // Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
+ // TransformReference::MaxDepthException
- /* Constructor */
- TransformReference();
-
/* Debugging function that will print to std::cout the transformation matrix */
void view(unsigned int target_frame, unsigned int source_frame);
-
+ // Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
+ // TransformReference::MaxDepthException
+
+
+
+
+
+
+ /************ Possible Exceptions ****************************/
+
/* An exception class to notify of bad frame number */
class LookupException : public std::exception
{
@@ -92,7 +108,7 @@
private:
} NoFrameConnectivity;
- /* An exception class to notify of no connection */
+ /* An exception class to notify that the search for connectivity descended too deep. */
class MaxDepthException : public std::exception
{
public:
@@ -100,24 +116,26 @@
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];};
+ /******************** Internal Storage ****************/
/* 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. */
+ * The frames will be dynamically allocated 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 passed to computeTransformFromList. */
+ /* This struct is how the list of transforms are stored before being passed to computeTransformFromList. */
typedef struct
{
std::vector<unsigned int> inverseTransforms;
std::vector<unsigned int> forwardTransforms;
} TransformLists;
+ /************************* Internal Functions ****************************/
+
+ /* 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];};
+
/* Find the list of connected frames necessary to connect two different frames */
TransformLists lookUpList(unsigned int target_frame, unsigned int source_frame);
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-03 08:58:53 UTC (rev 86)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-03 19:37:11 UTC (rev 87)
@@ -9,135 +9,67 @@
TransformReference mTR;
- // while (true)
+
+ dx = dy= dz = 0;
+ dyaw = dp = dr = .1;
+
+
+ //Fill in some transforms
+ 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,1,dx,dy,dz,dyaw,dp,dr);
+ mTR.set(6,5,dx,dy,dz,dyaw,dp,dr);
+ mTR.set(7,6,1,1,1,dyaw,dp,dr);
+ mTR.set(8,7,1,1,1,dyaw,dp,dr);
+
+
+ //Demonstrate InvalidFrame LookupException
+ try
{
- /* cout << "delta x:";
- cin >> dx;
- cout << "delta y:";
- cin >> dy;
- cout << "delta z:";
- cin >> dz;
- cout << "delta yaw:";
- cin >> dyaw;
- cout << "delta pitch:";
- cin >> dp;
- cout << "delta roll:";
- cin >> dr;
- */
- dx = dy= dz = 0;
- dyaw = dp = dr = .1;
-
-
-
- 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,1,dx,dy,dz,dyaw,dp,dr);
- mTR.set(6,5,dx,dy,dz,dyaw,dp,dr);
- mTR.set(7,6,1,1,1,dyaw,dp,dr);
- mTR.set(8,7,1,1,1,dyaw,dp,dr);
- try
- {
- mTR.view(10,9);
- }
- catch (TransformReference::LookupException &ex)
- {
- std::cout << "Caught " << ex.what()<<std::endl;
- }
-
- std::cout<<"Viewing (10,8)";
- mTR.view(10,8);
-
+ mTR.view(10,9);
+ }
+ catch (TransformReference::LookupException &ex)
+ {
+ std::cout << "Caught " << ex.what()<<std::endl;
+ }
+
+
+ // See the list of transforms to get between the frames
+ std::cout<<"Viewing (10,8):"<<std::endl;
+ mTR.view(10,8);
+
+
+ //See the resultant transform
+ std::cout <<"Calling get(10,8)"<<std::endl;
+ // NEWMAT::Matrix mat = mTR.get(1,1);
+ NEWMAT::Matrix mat = mTR.get(10,8);
+
+ std::cout << "Result of get(10,8):" << std::endl << mat<< std::endl;
-
- std::cout <<"Calling get(10,8)"<<std::endl;
- // NEWMAT::Matrix mat = mTR.get(1,1);
- NEWMAT::Matrix mat = mTR.get(10,8);
-
- std::cout << "Result" << std::endl << mat<< std::endl;
-
-
-
-
- mTR.set(6,7,dx,dy,dz,dyaw,dp,dr);
-
- try {
- mTR.view(10,8);
- }
- catch (TransformReference::MaxDepthException &ex)
- {
- std::cout <<"caught loop in graph"<<std::endl;
- }
-
- mTR.set(6,0,dx,dy,dz,dyaw,dp,dr);
-
- try {
- mTR.view(10,8);
- }
- catch (TransformReference::ConnectivityException &ex)
- {
- std::cout <<"caught unconnected frame"<<std::endl;
- }
-
-
-
-
-
-#ifdef DONOTUSE
-
- // NEWMAT::Matrix mat(4,4);
- NEWMAT::Matrix MatI(4,4);
- NEWMAT::Matrix MatI2(4,4);
-
-
- // double vec[] = {dx,dy,dz,dyaw,dp,dr};
- // mat <<vec;
- double init_time = (double)clock()/(double) CLOCKS_PER_SEC;
- int iterations = 1;
- for (int i = 0; i < iterations; i++)
- {
- MatI = mat.i();
- }
- std::cout <<"Straight Inverse" <<std::endl<< MatI;
- std::cout << "This should be Identity if Inverse is right: "<<std::endl << MatI * mat;
- std::cout << "Time Elapsed for "<<iterations<<"iterations is:"<<(double)clock()/(double) CLOCKS_PER_SEC - init_time <<std::endl;
- std::cout << "Net: "<<((double)clock()/(double) CLOCKS_PER_SEC -init_time)/(double)iterations <<std::endl;
-
-
- NEWMAT::Matrix trans_mask(4,4);
- trans_mask << 0 << 0 << 0 << -1
- << 0 << 0 << 0 << -1
- << 0 << 0 << 0 << -1
- << 0 << 0 << 0 << 1;
-
- NEWMAT::Matrix trans_fix(4,4);
- trans_fix << 1 << 0 << 0 << 0
- << 0 << 1 << 0 << 0
- << 0 << 0 << 1 << 0
- << 0 << 0 << 0 << 0;
-
-
- NEWMAT::Matrix rot_mask(4,4);
- rot_mask << 1 << 1 << 1 << 0
- << 1 << 1 << 1 << 0
- << 1 << 1 << 1 << 0
- << 0 << 0 << 0 << 1;
-
- init_time = (double)clock()/(double) CLOCKS_PER_SEC;
- for (int i = 0; i < iterations; i++)
- {
- MatI2 = SP(mat, rot_mask).t();
- MatI2 *= (SP(mat,trans_mask) + trans_fix);
- }
- std::cout <<"My Inverse:" <<std::endl<< MatI2;
- std::cout << "Product: " <<std::endl<< MatI2 * mat;
- std::cout << "Time Elapsed for method 2 "<<iterations<<" iterations is:"<<(double)clock()/(double) CLOCKS_PER_SEC - init_time <<std::endl;
- std::cout << "Net: "<<((double)clock()/(double) CLOCKS_PER_SEC - init_time) /(double)iterations <<std::endl;
-
-
- std::cout <<"difference: "<<std::endl<<MatI - MatI2;
-#endif //DONOTUSE
-
+
+
+ //Break the graph, making it loop and demonstrate catching MaxDepthException
+ mTR.set(6,7,dx,dy,dz,dyaw,dp,dr);
+
+ try {
+ mTR.view(10,8);
+ }
+ catch (TransformReference::MaxDepthException &ex)
+ {
+ std::cout <<"caught loop in graph"<<std::endl;
}
+
+ //Break the graph, making it disconnected, and demonstrate catching ConnectivityException
+ mTR.set(6,0,dx,dy,dz,dyaw,dp,dr);
+
+ try {
+ mTR.view(10,8);
+ }
+ catch (TransformReference::ConnectivityException &ex)
+ {
+ std::cout <<"caught unconnected frame"<<std::endl;
+ }
+
return 0;
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
