gadget::PositionCalibrationFilter

gadget::PositionCalibrationFilter Class Reference

A calibration filter that uses a calibration table to correct electromagnetic interference error from the tracker. More...

#include <gadget/Filter/Position/PositionCalibrationFilter.h>

Inheritance diagram for gadget::PositionCalibrationFilter:

Inheritance graph
[legend]
Collaboration diagram for gadget::PositionCalibrationFilter:

Collaboration graph
[legend]
List of all members.

Public Member Functions

 PositionCalibrationFilter ()
 Default Ctor.
 ~PositionCalibrationFilter ()
bool config (jccl::ConfigElementPtr e)
 Configuration for the filter.
void apply (std::vector< PositionData > &posSample)
 Applies the position filter to the given sample in place.

Static Public Member Functions

static std::string getElementType ()
 Returns the string rep of the element type used to config this device.

Detailed Description

A calibration filter that uses a calibration table to correct electromagnetic interference error from the tracker.

Definition at line 56 of file PositionCalibrationFilter.h.

Constructor & Destructor Documentation

gadget::PositionCalibrationFilter::PositionCalibrationFilter (  )  [inline]

Default Ctor.

Definition at line 63 of file PositionCalibrationFilter.h.

00064          : mWMatrix(NULL)
00065       {}

gadget::PositionCalibrationFilter::~PositionCalibrationFilter (  ) 

Definition at line 58 of file PositionCalibrationFilter.cpp.

00059    {
00060       for (size_t i = 0; i < mTable.size(); ++i)
00061       {
00062          delete[] mWMatrix[i];
00063       }
00064       delete[] mWMatrix;
00065    }

Member Function Documentation

bool gadget::PositionCalibrationFilter::config ( jccl::ConfigElementPtr  e  )  [virtual]

Configuration for the filter.

Returns:
Returns true if configured correctly.

Implements gadget::PositionFilter.

Definition at line 74 of file PositionCalibrationFilter.cpp.

References getElementType().

00075    {
00076       vprASSERT(e->getID() == PositionCalibrationFilter::getElementType());
00077       vprDEBUG_OutputGuard(vprDBG_ALL, vprDBG_VERB_LVL,
00078                            std::string("PositionCalibrationFilter::config:  ") +
00079                            e->getFullName() + std::string(":") + e->getID() + 
00080                            std::string("\n"),
00081                     std::string("PositionCalibrationFilter::config: done.\n") );
00082       std::string file_name = e->getProperty<std::string>("calibration_file",0);
00083       mFileName = jccl::ParseUtil::expandFileName(file_name, std::string("") );
00084 
00085       // Parse the calibration file.
00086       vprDEBUG(vprDBG_ALL, vprDBG_VERB_LVL)
00087          << "[PositionCalibrationFilter::config] Parsing " << mFileName << "\n"
00088          << vprDEBUG_FLUSH;
00089 
00090       cppdom::ContextPtr context( new cppdom::Context() );
00091       cppdom::DocumentPtr document( new cppdom::Document(context) );
00092       
00093       try
00094       {
00095          document->loadFile(mFileName);
00096       }
00097       catch (cppdom::Error e)
00098       {
00099          vprDEBUG(vprDBG_ERROR, vprDBG_CONFIG_LVL)
00100             << "[PositionCalibrationFilter::config] Unable to load calibration "
00101             << "file " << mFileName << ".  " << e.getString() << " at "
00102             << e.getInfo() << vprDEBUG_FLUSH;
00103          return false;
00104       }
00105 
00106       cppdom::NodePtr root = document->getChild("CalibrationTable");
00107       if (NULL == root.get())
00108       {
00109          vprDEBUG(vprDBG_ERROR, vprDBG_CONFIG_LVL)
00110             << "[PositionCalibrationFilter::config] Bad calibration file "
00111             << mFileName << "; could not retrieve the root node "
00112             << "'CalibrationTable'.  " << vprDEBUG_FLUSH;
00113          return false;
00114       }
00115       
00116       cppdom::NodeList offset_list;
00117       offset_list = root->getChildren("Offset");
00118       cppdom::NodeListIterator itr;
00119       
00120       for (itr = offset_list.begin(); itr != offset_list.end(); ++itr)
00121       {
00122          gmtl::Vec3d real_position;
00123          gmtl::Vec3d dev_position;
00124          
00125          if ( (*itr)->hasAttribute("X") && 
00126               (*itr)->hasAttribute("Y") && 
00127               (*itr)->hasAttribute("Z") )
00128          {
00129             real_position.set( (*itr)->getAttribute("X").getValue<double>(),
00130                                (*itr)->getAttribute("Y").getValue<double>(),
00131                                (*itr)->getAttribute("Z").getValue<double>() );
00132             std::istringstream offset_stream((*itr)->getCdata());
00133             //XXX:  Error checking on the Cdata needs to be more robust.
00134             offset_stream >> dev_position[gmtl::Xelt];
00135             offset_stream >> dev_position[gmtl::Yelt];
00136             offset_stream >> dev_position[gmtl::Zelt];
00137 
00138             vprDEBUG(vprDBG_ALL, vprDBG_VERB_LVL)
00139                << "[PositionCalibrationFilter::config()] Added "
00140                << real_position << " --> " << dev_position << " "
00141                << "to the table.\n"
00142                << vprDEBUG_FLUSH;
00143             
00144             mTable.push_back( std::make_pair(real_position, dev_position) );
00145          }
00146          else
00147          {
00148             vprDEBUG(vprDBG_ERROR, vprDBG_CONFIG_LVL)
00149                << "[PositionCalibrationFilter::config()] Malformed Offset "
00150                << "Element.  The element does not contain X, Y, and Z "
00151                << "attributes that represent a position; skipping.\n"
00152                << vprDEBUG_FLUSH;
00153          }
00154       }
00155 
00156       // Now that we have the offsets, loop through the alpha elements to
00157       // obtain the coefficients.
00158       mAlphaVec.reserve(mTable.size());
00159       cppdom::NodeList alpha_list = root->getChildren("Alpha");
00160       if (alpha_list.empty())
00161       {
00162          vprDEBUG(vprDBG_ERROR, vprDBG_CONFIG_LVL)
00163             << "[PositionCalibrationFilter::config()] Bad calibration file "
00164             << "'" << mFileName << "'\n" << "This file contains no Alpha "
00165             << " elements; these coefficients are necessary for calibration!\n"
00166             << vprDEBUG_FLUSH;
00167          return false;
00168       }
00169       for (itr = alpha_list.begin(); itr != alpha_list.end(); ++itr)
00170       {
00171          if ( (*itr)->hasAttribute("X") && 
00172               (*itr)->hasAttribute("Y") && 
00173               (*itr)->hasAttribute("Z") )
00174          {
00175             gmtl::Vec3d alpha_value(
00176                   (*itr)->getAttribute("X").getValue<double>(),
00177                   (*itr)->getAttribute("Y").getValue<double>(),
00178                   (*itr)->getAttribute("Z").getValue<double>() );
00179             vprDEBUG(vprDBG_ALL, vprDBG_VERB_LVL)
00180                << "[PositionCalibrationFilter::config()] Added "
00181                << alpha_value 
00182                << " to the alpha vector.\n"
00183                << vprDEBUG_FLUSH;
00184             mAlphaVec.push_back(alpha_value);
00185          }
00186          else
00187          {
00188             vprDEBUG(vprDBG_ERROR, vprDBG_CONFIG_LVL)
00189                << "[PositionCalibrationFilter::config()] "
00190                << "Malformed Alpha Element; this element does not contain "
00191                << "X="", Y="", Z="" attributes.\n"
00192                << vprDEBUG_FLUSH;
00193          }
00194       }
00195 
00196       // Now that we have the calibration table, compute the W Matrix and then
00197       // solve for the Alpha Vector.
00198 
00199       vprDEBUG(vprDBG_ALL, vprDBG_VERB_LVL)
00200          << "[PositionCalibrationFilter::config()] Calibration table parsed; "
00201          << "preparing W Matrix...\n" << vprDEBUG_FLUSH;
00202 
00203       // The "W" Matrix is an NxN matrix where N = mTable.size()
00204       // It is composed of elements computed using the w(p) function:
00205       // w[j](p) = sqrt( length(p - p[j])^2 + R^2 )
00206       // where 10 <= R^2 <= 1000.
00207       // The matrix looks like:
00208       // ( w[1](p[1]) w[2](p[1]) w[3](p[1]) ... w[N](p[1]) )
00209       // ( w[1](p[2]) w[2](p[2]) w[3](p[2]) ... w[N](p[2]) )
00210       // ( w[1](p[3]) w[2](p[3]) w[3](p[3]) ... w[N](p[3]) )
00211       // (                      .                          )
00212       // (                      .                          )
00213       // (                      .                          )
00214       // ( w[1](p[N]) w[2](p[N]) w[3](p[N]) ... w[N](p[N]) )
00215 
00216       vprDEBUG(vprDBG_ALL, vprDBG_DETAILED_LVL)
00217          << "[PositionCalibrationFilter::config()] The W matrix is " 
00218          << mTable.size() << "x" << mTable.size() << ".\n" 
00219          << vprDEBUG_FLUSH;
00220       
00221       double r_squared(0.4f);
00222       double r(0.63245553203367588f);
00223       mWMatrix = new double*[mTable.size()];
00224       for (unsigned int i = 0; i < mTable.size(); ++i)
00225       {
00226          mWMatrix[i] = new double[mTable.size()];
00227          for (unsigned int j = 0; j < mTable.size(); ++j)
00228          {
00229             if (i == j)
00230             {
00231                // Shortcut; the diagonal will always be equal to R.
00232                mWMatrix[i][j] = r;
00233             }
00234             else
00235             {
00236                // XXX: This is a workaround for GMTL CVS Head which supports 
00237                //      template meta-programming for vectors; unfortunately, 
00238                //      gmtl::length(v1 - v2) will not compile since the types 
00239                //      are NOT gmtl::Vec.
00240                gmtl::Vec3d difference = mTable[i].second - mTable[j].second;
00241                //double length = gmtl::length( difference );
00242                mWMatrix[i][j] = gmtl::Math::sqrt( 
00243                                 gmtl::dot(difference, difference) + 
00244                                 r_squared );
00245             }
00246             vprDEBUG(vprDBG_ALL, vprDBG_HEX_LVL)
00247                << "[PositionCalibrationFilter::config()] Assigning " 
00248                << mWMatrix[i][j]
00249                << " to mWMatrix( " << i << ", " << j << ").\n"
00250                << vprDEBUG_FLUSH;
00251          }
00252       }
00253 
00254       return true;  
00255    }

void gadget::PositionCalibrationFilter::apply ( std::vector< PositionData > &  posSample  )  [virtual]

Applies the position filter to the given sample in place.

Postcondition:
posSample is updated with a modified version.
Parameters:
posSample The sample to modify in place.

Implements gadget::PositionFilter.

Definition at line 258 of file PositionCalibrationFilter.cpp.

00259    {
00260       vprDEBUG(vprDBG_ALL, vprDBG_DETAILED_LVL)
00261          << "[PositionCalibrationFilter::apply()] Received " << posSample.size()
00262          << " samples.\n"
00263          << vprDEBUG_FLUSH;
00264       std::vector< PositionData >::iterator itr;
00265       for (itr = posSample.begin(); itr != posSample.end(); ++itr)
00266       {
00267          // Prepare the position matrix for Hardy's multi-quadric method.
00268          // 
00269          // Right now, we only calibrate the position, not the orientation, so
00270          // first, we need to pull out the translation matrix of posSample.
00271          // We do this using the standard method:  
00272          // pull the rotation matrix R out
00273          // of the transformation matrix Tr and multiply R-inverse by Tr, ie,
00274          // inverse(R) * Tr = T
00275          // and since rotation matrices are orthogonal,
00276          // inverse(R) = transpose(R), so...
00277          // transpose(R) * Tr = T
00278          gmtl::Matrix44f rotation(itr->getPosition());
00279          rotation[0][3] = 0;
00280          rotation[1][3] = 0;
00281          rotation[2][3] = 0;
00282          rotation[3][3] = 1;
00283          
00284          gmtl::Matrix44f translation = gmtl::transpose(rotation) * itr->getPosition();
00285          vprDEBUG(vprDBG_ALL, vprDBG_VERB_LVL)
00286             << "[PositionCalibrationFilter::apply()] Received tracked "
00287             << "position\n" << translation << "\n"
00288             << vprDEBUG_FLUSH;
00289          gmtl::Vec3d tracked_pos( translation[0][3], 
00290                                   translation[1][3], 
00291                                   translation[2][3] );
00292 
00293          // Now, we apply the following to the tracked position:
00294          // real_pos = alpha[j] * w[j](tracked_pos) + ... + alpha[N] * 
00295          // w[N](tracked_pos)
00296          // where w[j](p) = sqrt( length( p - p[j] )^2 + R^2 )
00297          // where 10 <= R^2 <= 1000.
00298          gmtl::Vec3d real_pos(0.0f, 0.0f, 0.0f);
00299          double r_squared = 40.0f;
00300          vprDEBUG(vprDBG_ALL, vprDBG_DETAILED_LVL)
00301             << "[PositionCalibrationFilter::apply()] Summing real position...\n"
00302             << vprDEBUG_FLUSH;
00303          for (unsigned int i = 0; i < mTable.size(); ++i)
00304          {
00305             // XXX: This is a workaround for GMTL CVS Head which supports 
00306             //      template meta-programming for vectors; unfortunately, 
00307             //      gmtl::length(v1 - v2) will not compile since the types are 
00308             //      NOT gmtl::Vec.
00309             gmtl::Vec3d difference = tracked_pos - mTable[i].second;
00310             //double length = gmtl::length(difference);
00311             real_pos += mAlphaVec[i] * 
00312                        gmtl::Math::sqrt( gmtl::dot(difference, difference) + 
00313                                          r_squared ); 
00314             vprDEBUG(vprDBG_ALL, vprDBG_DETAILED_LVL)
00315                << "[PositionCalibrationFilter::apply()] real_pos: "
00316                << real_pos << "\n" << vprDEBUG_FLUSH;
00317          }
00318 
00319          
00320          // Now we clobber the old transformation and replace it with a 
00321          // translation to our real position.
00322          gmtl::Matrix44f new_translation;
00323          new_translation[0][3] = static_cast<float>(real_pos[0]);
00324          new_translation[1][3] = static_cast<float>(real_pos[1]);
00325          new_translation[2][3] = static_cast<float>(real_pos[2]);
00326          
00327          vprDEBUG(vprDBG_ALL, vprDBG_VERB_LVL)
00328             << "[PositionCalibrationFilter::apply()] Replaced " 
00329             << tracked_pos << " with \n"
00330             << real_pos << "\n"
00331             << vprDEBUG_FLUSH;
00332          vprDEBUG(vprDBG_ALL, vprDBG_VERB_LVL)
00333             << "[PositionCalibrationFilter::apply()] Replaced \n" << translation
00334             << "\nwith " << new_translation
00335             << vprDEBUG_FLUSH;
00336 
00337          // Rebuild the position sample (transformation matrix).
00338          itr->setPosition(rotation * new_translation);
00339       }
00340    }

std::string gadget::PositionCalibrationFilter::getElementType (  )  [static]

Returns the string rep of the element type used to config this device.

This string is used by the device factory to look up device drivers based up the type of element it is trying to load.

Reimplemented from gadget::PositionFilter.

Definition at line 68 of file PositionCalibrationFilter.cpp.

Referenced by config().

00069    {
00070       return std::string("position_calibration_filter");
00071    }

The documentation for this class was generated from the following files:
Generated on Thu Jan 4 10:44:43 2007 for Gadgeteer by  doxygen 1.5.1