PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
PointObservationBuffer.cxx
Go to the documentation of this file.
1 /*=Plus=header=begin======================================================
2 Program: Plus
3 Copyright (c) Laboratory for Percutaneous Surgery. All rights reserved.
4 See License.txt for details.
5 =========================================================Plus=header=end*/
6 
7 #include "PlusConfigure.h"
8 
10 #include "igsioCommon.h"
11 
13 {
14 }
15 
16 //-----------------------------------------------------------------------------
17 
19 {
20  for ( unsigned int i = 0; i < this->Size(); i++ )
21  {
22  delete this->observations.at( i );
23  }
24  this->observations.clear();
25 }
26 
27 //-----------------------------------------------------------------------------
28 
29 PointObservationBuffer::PointObservationVector::size_type PointObservationBuffer::Size() const
30 {
31  return this->observations.size();
32 }
33 
34 //-----------------------------------------------------------------------------
35 
37 {
38  return this->observations.at( index );
39 }
40 
41 //-----------------------------------------------------------------------------
42 
44 {
45  this->observations.push_back( newObservation );
46 }
47 
48 
49 //----------------------------------------------------------------------------
51 {
52  this->observations.clear();
53 }
54 
55 //-----------------------------------------------------------------------------
56 
57 void PointObservationBuffer::Translate( std::vector<double> translation )
58 {
59  for ( unsigned int i = 0; i < this->Size(); i++ )
60  {
61  this->GetObservation( i )->Translate( translation );
62  }
63 }
64 
65 //-----------------------------------------------------------------------------
66 
68 {
69  // Assume that it is already mean zero
70  const double CONDITION_THRESHOLD = 1e-3;
71 
72  // Let us construct the data matrix
73  vnl_matrix<double>* DataMatrix = new vnl_matrix<double>( PointObservation::SIZE, PointObservation::SIZE, 0.0 );
74 
75  // Pick two dimensions, and find their data matrix entry
76  for ( int d1 = 0; d1 < PointObservation::SIZE; d1++ )
77  {
78  for ( int d2 = 0; d2 < PointObservation::SIZE; d2++ )
79  {
80  // Iterate over all times
81  for ( unsigned int i = 0; i < this->Size(); i++ )
82  {
83  DataMatrix->put( d1, d2, DataMatrix->get( d1, d2 ) + fromPoints->GetObservation( i )->Observation.at( d1 ) * this->GetObservation( i )->Observation.at( d2 ) );
84  }
85  }
86  }
87 
88  // Now we can calculate its svd
89  vnl_svd<double>* SVDMatrix = new vnl_svd<double>( *DataMatrix, 0.0 );
90  if ( SVDMatrix->well_condition() < CONDITION_THRESHOLD ) // This is the inverse of the condition number
91  {
92  LOG_ERROR( "Failed - spherical registration is ill-conditioned!" );
93  } // TODO: Error if ill-conditioned
94 
95  return new vnl_matrix<double>( SVDMatrix->V() * SVDMatrix->U().transpose() );
96 }
97 
98 //-----------------------------------------------------------------------------
99 
100 vnl_matrix<double>* PointObservationBuffer::TranslationalRegistration( std::vector<double> toCentroid, std::vector<double> fromCentroid, vnl_matrix<double>* rotation )
101 {
102  // Make matrices out of the centroids
103  vnl_matrix<double>* toMatrix = new vnl_matrix<double>( PointObservation::SIZE, 1, 0.0 );
104  vnl_matrix<double>* fromMatrix = new vnl_matrix<double>( PointObservation::SIZE, 1, 0.0 );
105 
106  for ( int i = 0; i < PointObservation::SIZE; i++ )
107  {
108  toMatrix->put( i, 0, toCentroid.at( i ) );
109  fromMatrix->put( i, 0, fromCentroid.at( i ) );
110  }
111 
112  return new vnl_matrix<double>( ( *toMatrix ) - ( *rotation ) * ( *fromMatrix ) );
113 }
114 
115 //-----------------------------------------------------------------------------
116 
118 {
119  std::vector<double> centroid = this->CalculateCentroid();
120  vnl_matrix<double>* cov = this->CovarianceMatrix( centroid );
121 
122  //Calculate the eigenvectors of the covariance matrix
123  vnl_matrix<double> eigenvectors( PointObservation::SIZE, PointObservation::SIZE, 0.0 );
124  vnl_vector<double> eigenvalues( PointObservation::SIZE, 0.0 );
125  vnl_symmetric_eigensystem_compute( *cov, eigenvectors, eigenvalues );
126  // Note: eigenvectors are ordered in increasing eigenvalue ( 0 = smallest, end = biggest )
127 
128  // Grab only the most important eigenvectors
129  std::vector<double> Eigenvector1( PointObservation::SIZE, 0.0 ); // Smallest
130  std::vector<double> Eigenvector2( PointObservation::SIZE, 0.0 ); // Medium
131  std::vector<double> Eigenvector3( PointObservation::SIZE, 0.0 ); // Largest
132 
133  Eigenvector1.at( 0 ) = eigenvectors.get( 0, 0 );
134  Eigenvector1.at( 1 ) = eigenvectors.get( 1, 0 );
135  Eigenvector1.at( 2 ) = eigenvectors.get( 2, 0 );
136 
137  Eigenvector2.at( 0 ) = eigenvectors.get( 0, 1 );
138  Eigenvector2.at( 1 ) = eigenvectors.get( 1, 1 );
139  Eigenvector2.at( 2 ) = eigenvectors.get( 2, 1 );
140 
141  Eigenvector3.at( 0 ) = eigenvectors.get( 0, 2 );
142  Eigenvector3.at( 1 ) = eigenvectors.get( 1, 2 );
143  Eigenvector3.at( 2 ) = eigenvectors.get( 2, 2 );
144 
145  // The threshold noise is twice the extraction threshold
146  if ( dof == 0 )
147  {
148  return new Point( centroid );
149  }
150  if ( dof == 1 )
151  {
152  return new Line( centroid, LinearObject::Add( centroid, Eigenvector3 ) );
153  }
154  if ( dof == 2 )
155  {
156  return new Plane( centroid, LinearObject::Add( centroid, Eigenvector2 ), LinearObject::Add( centroid, Eigenvector3 ) );
157  }
158 
159  LinearObject* obj = NULL;
160  return obj;
161 
162 }
163 
164 //-----------------------------------------------------------------------------
165 
166 void PointObservationBuffer::Filter( LinearObject* object, int filterWidth )
167 {
168  const int THRESHOLD = 1e-3; // Deal with the case of very little noise
169  bool changed = true;
170 
171  while ( changed )
172  {
173  std::vector<double> distances( this->Size(), 0 );
174  double meanDistance = 0;
175  double stdev = 0;
176 
177  // Calculate the distance of each point to the linear object
178  for ( unsigned int i = 0; i < this->Size(); i++ )
179  {
180  distances.at( i ) = object->DistanceToVector( this->GetObservation( i )->Observation );
181  meanDistance = meanDistance + distances.at( i );
182  stdev = stdev + distances.at( i ) * distances.at( i );
183  }
184  meanDistance = meanDistance / this->Size();
185  stdev = stdev / this->Size();
186  stdev = sqrt( stdev - meanDistance * meanDistance );
187 
188  // Keep only the points that are within certain number of standard deviations
189  std::vector<PointObservation*> newObservations;
190  for ( unsigned int i = 0; i < this->Size(); i++ )
191  {
192  if ( distances.at( i ) < filterWidth * stdev || distances.at( i ) < THRESHOLD )
193  {
194  newObservations.push_back( this->GetObservation( i ) );
195  }
196  }
197 
198  if ( newObservations.size() < this->Size() )
199  {
200  changed = true;
201  }
202  else
203  {
204  changed = false;
205  }
206 
207  this->observations = newObservations;
208 
209  }
210 
211 }
212 
213 //-----------------------------------------------------------------------------
214 
216 {
217  std::ostringstream xmlstring;
218 
219  for ( unsigned int i = 0; i < this->Size(); i++ )
220  {
221  xmlstring << this->GetObservation( i )->ToXMLString();
222  }
223 
224  return xmlstring.str();
225 }
226 
227 //-----------------------------------------------------------------------------
228 
229 void PointObservationBuffer::FromXMLElement( vtkXMLDataElement* element )
230 {
231  PointObservation* blankObservation = new PointObservation();
232  this->observations = std::vector<PointObservation*>( 0, blankObservation );
233 
234  int numElements = element->GetNumberOfNestedElements();
235 
236  for ( int i = 0; i < numElements; i++ )
237  {
238  vtkXMLDataElement* noteElement = element->GetNestedElement( i );
239 
240  PointObservation* newObservation = new PointObservation();
241  newObservation->FromXMLElement( noteElement );
242  this->AddObservation( newObservation );
243 
244  }
245 
246 }
247 
248 //-----------------------------------------------------------------------------
249 
250 vnl_matrix<double>* PointObservationBuffer::CovarianceMatrix( std::vector<double> centroid )
251 {
252  // Construct a buffer for the zero mean data; initialize covariance matrix
253  PointObservationBuffer* zeroMeanBuffer = new PointObservationBuffer();
254  vnl_matrix<double>* cov = new vnl_matrix<double>( PointObservation::SIZE, PointObservation::SIZE );
255  cov->fill( 0.0 );
256 
257  // Subtract the mean from each observation
258  for ( unsigned int i = 0; i < this->Size(); i++ )
259  {
260  PointObservation* newObservation = new PointObservation();
261  for( int d = 0; d < PointObservation::SIZE; d++ )
262  {
263  newObservation->Observation.push_back( this->GetObservation( i )->Observation.at( d ) - centroid.at( d ) );
264  }
265  zeroMeanBuffer->AddObservation( newObservation );
266  }
267 
268  // Pick two dimensions, and find their covariance
269  for ( int d1 = 0; d1 < PointObservation::SIZE; d1++ )
270  {
271  for ( int d2 = 0; d2 < PointObservation::SIZE; d2++ )
272  {
273  // Iterate over all times
274  for ( unsigned int i = 0; i < this->Size(); i++ )
275  {
276  cov->put( d1, d2, cov->get( d1, d2 ) + zeroMeanBuffer->GetObservation( i )->Observation.at( d1 ) * zeroMeanBuffer->GetObservation( i )->Observation.at( d2 ) );
277  }
278  // Divide by the number of records
279  cov->put( d1, d2, cov->get( d1, d2 ) / this->Size() );
280  }
281  }
282 
283  return cov;
284 
285 }
286 
287 //-----------------------------------------------------------------------------
288 
289 std::vector<double> PointObservationBuffer::CalculateCentroid()
290 {
291  // Calculate the centroid
292  std::vector<double> centroid( PointObservation::SIZE, 0.0 );
293  for ( unsigned int i = 0; i < this->Size(); i++ )
294  {
295  for ( int d = 0; d < PointObservation::SIZE; d++ )
296  {
297  centroid.at( d ) = centroid.at( d ) + this->GetObservation( i )->Observation.at( d );
298  }
299  }
300  for ( int d = 0; d < PointObservation::SIZE; d++ )
301  {
302  centroid.at( d ) = centroid.at( d ) / this->Size();
303  }
304 
305  return centroid;
306 }
307 
308 //-----------------------------------------------------------------------------
309 
310 std::vector<PointObservationBuffer*> PointObservationBuffer::ExtractLinearObjects( int collectionFrames, double extractionThreshold, std::vector<int>* dof )
311 {
312  // First, let us identify the segmentation points and the associated DOFs, then we can divide up the points
313  int TEST_INTERVAL = 21;
314 
315  PointObservationBuffer* eigenBuffer = new PointObservationBuffer(); // Note: 1 < 2 < 3
316  int currStartIndex, currEndIndex;
317  bool collecting = false;
318 
319  std::vector<PointObservationBuffer*> linearObjects;
320 
321  // Note: i is the start of the interval over which we will exam for linearity
322  for ( unsigned int i = 0; i < this->Size() - TEST_INTERVAL; i++ )
323  {
324  // Create a smaller point observation buffer to work with at each iteration with the points of interest
326  for ( unsigned int j = i; j < i + TEST_INTERVAL; j++ )
327  {
328  tempBuffer->AddObservation( this->GetObservation( j ) );
329  }
330 
331  // Find the eigenvalues of covariance matrix
332  std::vector<double> centroid = tempBuffer->CalculateCentroid();
333  vnl_matrix<double>* cov = tempBuffer->CovarianceMatrix( centroid );
334 
335  //Calculate the eigenvectors of the covariance matrix
336  vnl_matrix<double> eigenvectors( PointObservation::SIZE, PointObservation::SIZE, 0.0 );
337  vnl_vector<double> eigenvalues( PointObservation::SIZE, 0.0 );
338  vnl_symmetric_eigensystem_compute( *cov, eigenvectors, eigenvalues );
339  // Note: eigenvectors are ordered in increasing eigenvalue ( 0 = smallest, end = biggest )
340 
341  std::vector<double> eigen( 3, 0.0 );
342  eigen.at( 0 ) = eigenvalues.get( 0 );
343  eigen.at( 1 ) = eigenvalues.get( 1 );
344  eigen.at( 2 ) = eigenvalues.get( 2 );
345  eigenBuffer->AddObservation( new PointObservation( eigen ) );
346 
347  if ( !collecting )
348  {
349  currStartIndex = i;
350  }
351 
352  if ( eigenvalues.get( 0 ) < extractionThreshold )
353  {
354  collecting = true;
355  continue;
356  }
357  collecting = false;
358  currEndIndex = i;
359 
360  // Suppose that we have reached the end of a collecting section
361  // If its too short then skip
362  if ( currEndIndex - currStartIndex < collectionFrames )
363  {
364  continue;
365  }
366 
367 
368  // Now search for the largest interval of points which has the fewest DOF and satisfies the minimum interval
369  for ( int e = PointObservation::SIZE - 1; e >= 0; e-- )
370  {
371  // Find the intervals where the eigenvalue is less than the threshold
372  std::vector<int> dofInterval;
373  dofInterval.push_back( currStartIndex );
374  for ( int j = currStartIndex; j < currEndIndex; j++ )
375  {
376  if ( eigenBuffer->GetObservation( j )->Observation.at( e ) > extractionThreshold )
377  {
378  dofInterval.push_back( j );
379  }
380  }
381  dofInterval.push_back( currEndIndex );
382 
383  // Find the longest such interval
384  int maxIntervalLength = 0;
385  int maxIntervalIndex = 0;
386  for ( unsigned int j = 0; j < dofInterval.size() - 1; j++ )
387  {
388  if ( dofInterval.at( j + 1 ) - dofInterval.at( j ) > maxIntervalLength )
389  {
390  maxIntervalLength = dofInterval.at( j + 1 ) - dofInterval.at( j );
391  maxIntervalIndex = j;
392  }
393  }
394 
395  // If the longest interval is too short, then ignore
396  if ( maxIntervalLength < collectionFrames )
397  {
398  continue;
399  }
400 
401  // Otherwise, this is a collected linear object
402  PointObservationBuffer* foundBuffer = new PointObservationBuffer();
403  for ( int j = dofInterval.at( maxIntervalIndex ); j < dofInterval.at( maxIntervalIndex + 1 ); j++ )
404  {
405  foundBuffer->AddObservation( this->GetObservation( j + TEST_INTERVAL ) );
406  }
407 
408  linearObjects.push_back( foundBuffer );
409  dof->push_back( PointObservation::SIZE - 1 - e );
410  break;
411  }
412  }
413 
414  return linearObjects;
415 }
void Translate(std::vector< double > translation)
void Filter(LinearObject *object, int filterWidth)
Definition: Line.h:17
static std::vector< double > Add(std::vector< double > v1, std::vector< double > v2)
vnl_matrix< double > * SphericalRegistration(PointObservationBuffer *fromPoints)
for i
Definition: Point.h:18
Definition: Plane.h:18
LinearObject * LeastSquaresLinearObject(int dof)
static const int SIZE
void AddObservation(PointObservation *newObservation)
void FromXMLElement(vtkXMLDataElement *element)
std::vector< double > Observation
PointObservation * GetObservation(int index) const
std::string ToXMLString()
uint32_t(* distances)[8]
Definition: phidget22.h:4639
void Translate(std::vector< double > translation)
void FromXMLElement(vtkXMLDataElement *element)
PointObservationVector::size_type Size() const
std::vector< PointObservationBuffer * > ExtractLinearObjects(int collectionFrames, double extractionThreshold, std::vector< int > *dof)
vnl_matrix< double > * TranslationalRegistration(std::vector< double > toCentroid, std::vector< double > fromCentroid, vnl_matrix< double > *rotation)