[Personalrobots-commit] SF.net SVN: personalrobots: [161] pkg/trunk/libTF

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

Revision: 161
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=161&view=rev
Author:   tfoote
Date:     2008-04-22 18:41:30 -0700 (Tue, 22 Apr 2008)

Log Message:
-----------
removing zero angle bug in matrix to quaternion.  And decreasing to 100 seconds from newest data for interpolation

Modified Paths:
--------------
    pkg/trunk/libTF/include/libTF/Quaternion3D.h
    pkg/trunk/libTF/src/simple/Quaternion3D.cpp

Modified: pkg/trunk/libTF/include/libTF/Quaternion3D.h
===================================================================
--- pkg/trunk/libTF/include/libTF/Quaternion3D.h	2008-04-22 23:58:24 UTC (rev 160)
+++ pkg/trunk/libTF/include/libTF/Quaternion3D.h	2008-04-23 01:41:30 UTC (rev 161)
@@ -116,7 +116,7 @@
   
 private:
   /**** Linked List stuff ****/
-  static const long long MAX_STORAGE_TIME = 1000000000; // max of 100 seconds storage
+  static const long long MAX_STORAGE_TIME = 100000000; // max of 100 seconds storage
  
   struct data_LL{
     Quaternion3DStorage data;

Modified: pkg/trunk/libTF/src/simple/Quaternion3D.cpp
===================================================================
--- pkg/trunk/libTF/src/simple/Quaternion3D.cpp	2008-04-22 23:58:24 UTC (rev 160)
+++ pkg/trunk/libTF/src/simple/Quaternion3D.cpp	2008-04-23 01:41:30 UTC (rev 161)
@@ -93,8 +93,7 @@
   //If the trace of the matrix is equal to zero then identify
   // which major diagonal element has the greatest value.
   //  Depending on this, calculate the following:
-
-      if ( mat[0] > mat[5] && mat[0] > mat[10] ) {// Column 0: 
+  else if ( mat[0] > mat[5] && mat[0] > mat[10] ) {// Column 0: 
         double S  = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
         temp.xr = 0.25 * S;
         temp.yr = (mat[1] + mat[4] ) / S;
@@ -113,6 +112,7 @@
         temp.zr = 0.25 * S;
         temp.w = (mat[1] - mat[4] ) / S;
       }
+
       add_value(temp);
 };
 
@@ -363,6 +363,7 @@
 
 void Quaternion3D::insertNode(const Quaternion3DStorage & new_val)
 {
+
   data_LL* p_current;
   data_LL* p_old;
 
@@ -428,7 +429,6 @@
 
 void Quaternion3D::pruneList()
 {
-  unsigned long long current_time = Qgettime();
   data_LL* p_current = last;
 
   //  cout << "Pruning List" << endl;
@@ -436,6 +436,9 @@
   //Empty Set
   if (last == NULL) return;
 
+  unsigned long long current_time = first->data.time;;
+
+
   //While time stamps too old
   while (p_current->data.time + max_storage_time < current_time)
     {


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