Covariance.h
Go to the documentation of this file.
1 #ifndef __GfxTL_COVARIANCE_HEADER__
2 #define __GfxTL_COVARIANCE_HEADER__
3 #include <GfxTL/ScalarTypeDeferer.h>
4 #include <GfxTL/VectorXD.h>
5 #include <GfxTL/WeightFunc.h>
6 
7 namespace GfxTL
8 {
9  template <class MatrixT>
10  class IncrementalCovarianceMatrix
11  {
12  public:
13  typedef typename MatrixT::ScalarType ScalarType;
14  typedef MatrixT MatrixType;
15  typedef VectorXD<MatrixT::Rows, ScalarType> PointType;
16 
17  IncrementalCovarianceMatrix()
18  {
19  m_matrix.Zero();
20  m_mean.Zero();
21  m_totalWeight = 0;
22  }
23 
24  template <class PointT>
25  void
26  Add(const PointT& p)
27  {
28  Add(1, p);
29  }
30 
31  template <class WeightT, class PointT>
32  void
33  Add(WeightT w, const PointT& p)
34  {
35  ScalarType oldTotalWeight = m_totalWeight;
36  m_totalWeight += ScalarType(w);
37  PointType diff = p - m_mean;
38  PointType r = (ScalarType(w) / ScalarType(m_totalWeight)) * diff;
39  m_mean += r;
40  m_matrix += OuterProduct(oldTotalWeight * r, diff);
41  }
42 
43  void
44  Matrix(MatrixT* m) const
45  {
46  *m = m_matrix;
47  if (m_totalWeight > 0)
48  {
49  *m /= m_totalWeight;
50  }
51  }
52 
53  void
54  Mean(PointType* mean) const
55  {
56  *mean = m_mean;
57  }
58 
59  void
60  MeanAndMatrix(PointType* mean, MatrixT* m) const
61  {
62  Mean(mean);
63  Matrix(m);
64  }
65 
66  void
67  Reset()
68  {
69  m_matrix.Zero();
70  m_mean.Zero();
71  m_totalWeight = 0;
72  }
73 
74  private:
75  MatrixType m_matrix;
76  PointType m_mean;
77  ScalarType m_totalWeight;
78  };
79 
80  template <class ScalarT>
81  class IncrementalCovarianceMatrix<MatrixXX<3, 3, ScalarT>>
82  {
83  public:
84  typedef ScalarT ScalarType;
85  typedef MatrixXX<3, 3, ScalarT> MatrixType;
86  typedef VectorXD<3, ScalarType> PointType;
87 
88  IncrementalCovarianceMatrix()
89  {
90  m_matrix.Zero();
91  m_mean.Zero();
92  m_totalWeight = 0;
93  }
94 
95  template <class PointT>
96  void
97  Add(const PointT& p)
98  {
99  Add(1, p);
100  }
101 
102  template <class WeightT, class PointT>
103  void
104  Add(WeightT w, const PointT& p)
105  {
106  ScalarType oldTotalWeight = m_totalWeight;
107  m_totalWeight += ScalarType(w);
108  PointType diff = p - m_mean;
109  ScalarType dummy;
110  PointType r = (ScalarType(w) / m_totalWeight) * diff;
111  m_mean += r;
112  m_matrix[0][0] += (dummy = oldTotalWeight * diff[0]) * r[0];
113  m_matrix[0][1] += dummy * r[1];
114  m_matrix[0][2] += dummy * r[2];
115  m_matrix[1][1] += (dummy = oldTotalWeight * diff[1]) * r[1];
116  m_matrix[1][2] += dummy * r[2];
117  m_matrix[2][2] += oldTotalWeight * diff[2] * r[2];
118  }
119 
120  void
121  Matrix(MatrixType* m) const
122  {
123  if (m_totalWeight > 0)
124  {
125  (*m)[0][0] = m_matrix[0][0] / m_totalWeight;
126  (*m)[0][1] = m_matrix[0][1] / m_totalWeight;
127  (*m)[0][2] = m_matrix[0][2] / m_totalWeight;
128  (*m)[1][1] = m_matrix[1][1] / m_totalWeight;
129  (*m)[1][2] = m_matrix[1][2] / m_totalWeight;
130  (*m)[2][2] = m_matrix[2][2] / m_totalWeight;
131  }
132  else
133  {
134  (*m)[0][0] = m_matrix[0][0];
135  (*m)[0][1] = m_matrix[0][1];
136  (*m)[0][2] = m_matrix[0][2];
137  (*m)[1][1] = m_matrix[1][1];
138  (*m)[1][2] = m_matrix[1][2];
139  (*m)[2][2] = m_matrix[2][2];
140  }
141  (*m)[1][0] = (*m)[0][1];
142  (*m)[2][0] = (*m)[0][2];
143  (*m)[2][1] = (*m)[1][2];
144  }
145 
146  void
147  Mean(PointType* mean) const
148  {
149  *mean = m_mean;
150  }
151 
152  void
153  MeanAndMatrix(PointType* mean, MatrixType* m) const
154  {
155  Mean(mean);
156  Matrix(m);
157  }
158 
159  void
160  Reset()
161  {
162  m_matrix.Zero();
163  m_mean.Zero();
164  m_totalWeight = 0;
165  }
166 
167  private:
168  MatrixType m_matrix;
169  PointType m_mean;
170  ScalarType m_totalWeight;
171  };
172 
173  template <class PointT, class PointsForwardIt, class WeightsForwardIt, class MatrixT>
174  void
175  CovarianceMatrix(const PointT& center,
176  PointsForwardIt begin,
177  PointsForwardIt end,
178  WeightsForwardIt weights,
179  MatrixT* matrix)
180  {
181  typedef typename MatrixT::ScalarType ScalarType;
182  matrix->Zero();
183  ScalarType totalWeight = 0;
184  for (; begin != end; ++begin, ++weights)
185  {
186  typename PointT::TransposedType diffT;
187  PointT diff = ((PointT)(*begin));
188  diff -= center;
189  diff.Transpose(&diffT);
190  ScalarType w = ScalarType(*weights);
191  totalWeight += w;
192  diff *= w;
193  MatrixT dd = diff * diffT;
194  *matrix += dd;
195  }
196  *matrix /= totalWeight;
197  }
198 
199  template <class PointT, class PointsForwardIt, class WeightsForwardIt>
200  void
201  CovarianceMatrix(const PointT& center,
202  PointsForwardIt begin,
203  PointsForwardIt end,
204  WeightsForwardIt weights,
205  MatrixXX<3, 3, typename PointT::ScalarType>* matrix)
206  {
207  typedef typename PointT::ScalarType ScalarType;
208  matrix->Zero();
209  ScalarType totalWeight = 0, dummy;
210  for (; begin != end; ++begin, ++weights)
211  {
212  PointT diff = PointT(*begin);
213  diff -= center;
214  ScalarType w = ScalarType(*weights);
215  (*matrix)[0][0] += (dummy = w * diff[0]) * diff[0];
216  (*matrix)[0][1] += dummy * diff[1];
217  (*matrix)[0][2] += dummy * diff[2];
218  (*matrix)[1][1] += (dummy = w * diff[1]) * diff[1];
219  (*matrix)[1][2] += dummy * diff[2];
220  (*matrix)[2][2] += w * diff[2] * diff[2];
221  totalWeight += w;
222  }
223  (*matrix)[1][0] = (*matrix)[0][1];
224  (*matrix)[2][0] = (*matrix)[0][2];
225  (*matrix)[2][1] = (*matrix)[1][2];
226  (*matrix) /= totalWeight;
227  }
228 
229  template <class PointT, class PointsForwardIt, class MatrixT>
230  void
231  CovarianceMatrix(const PointT& center,
232  PointsForwardIt begin,
233  PointsForwardIt end,
234  MatrixT* matrix)
235  {
236  CovarianceMatrix(center, begin, end, UnitWeightIterator(), matrix);
237  }
238 
239  // one pass covariance
240  template <class PointsIteratorT, class WeightsIteratorT, class PointT, class MatrixT>
241  void
242  CovarianceMatrix(PointsIteratorT begin,
243  PointsIteratorT end,
244  WeightsIteratorT weights,
245  PointT* mean,
246  MatrixT* matrix)
247  {
248  IncrementalCovarianceMatrix<MatrixT> cov;
249  for (; begin != end; ++begin, ++weights)
250  {
251  cov.Add(*weights, *begin);
252  }
253  cov.MeanAndMatrix(mean, matrix);
254  }
255 
256  // one pass covariance (optimized for 3x3 matrix)
257  template <class PointsIteratorT, class WeightsIteratorT, class PointT, class ScalarT>
258  void
259  CovarianceMatrix(PointsIteratorT begin,
260  PointsIteratorT end,
261  WeightsIteratorT weights,
262  PointT* mean,
263  MatrixXX<3, 3, ScalarT>* matrix)
264  {
265  typedef typename std::iterator_traits<WeightsIteratorT>::value_type WeightType;
266  typedef typename ScalarTypeDeferer<
267  typename std::iterator_traits<PointsIteratorT>::value_type>::ScalarType ScalarType;
268  matrix->Zero();
269  if (begin == end)
270  {
271  return;
272  }
273  WeightType w, totalWeight, oldTotalWeight;
274  w = *weights;
275  *mean = *begin;
276  totalWeight = w;
277  for (++begin, ++weights; begin != end; ++begin, ++weights)
278  {
279  oldTotalWeight = totalWeight;
280  w = *weights;
281  totalWeight += w;
282  PointT diff;
283  for (unsigned int i = 0; i < 3; ++i)
284  {
285  diff[i] = (*begin)[i] - (*mean)[i];
286  }
287  ScalarType dummy, ww;
288  ww = ScalarType(oldTotalWeight);
289  PointT r = (ScalarType(w) / ScalarType(totalWeight)) * diff;
290  *mean += r;
291  (*matrix)[0][0] += (dummy = ww * diff[0]) * r[0];
292  (*matrix)[0][1] += dummy * r[1];
293  (*matrix)[0][2] += dummy * r[2];
294  (*matrix)[1][1] += (dummy = ww * diff[1]) * r[1];
295  (*matrix)[1][2] += dummy * r[2];
296  (*matrix)[2][2] += ww * diff[2] * r[2];
297  }
298  (*matrix)[0][0] /= ScalarType(totalWeight);
299  (*matrix)[0][1] /= ScalarType(totalWeight);
300  (*matrix)[0][2] /= ScalarType(totalWeight);
301  (*matrix)[1][1] /= ScalarType(totalWeight);
302  (*matrix)[1][2] /= ScalarType(totalWeight);
303  (*matrix)[2][2] /= ScalarType(totalWeight);
304  (*matrix)[1][0] = (*matrix)[0][1];
305  (*matrix)[2][0] = (*matrix)[0][2];
306  (*matrix)[2][1] = (*matrix)[1][2];
307  }
308 }; // namespace GfxTL
309 
310 #endif
GfxTL::IncrementalCovarianceMatrix::Add
void Add(const PointT &p)
Definition: Covariance.h:26
GfxTL::VectorXD< MatrixT::Rows, ScalarType >
GfxTL::ScalarTypeDeferer
Definition: ScalarTypeDeferer.h:13
GfxTL::OuterProduct
MatrixXX< B, A, T > OuterProduct(const VectorXD< A, T > &a, const VectorXD< B, T > &b)
Definition: VectorXD.h:492
WeightFunc.h
GfxTL::MatrixXX
Definition: MatrixXX.h:28
GfxTL::IncrementalCovarianceMatrix::Matrix
void Matrix(MatrixT *m) const
Definition: Covariance.h:44
VectorXD.h
GfxTL::IncrementalCovarianceMatrix
Definition: Covariance.h:10
GfxTL::IncrementalCovarianceMatrix::Mean
void Mean(PointType *mean) const
Definition: Covariance.h:54
GfxTL::VectorXD::Zero
void Zero()
Definition: VectorXD.h:282
GfxTL::IncrementalCovarianceMatrix< MatrixXX< 3, 3, ScalarT > >::MatrixType
MatrixXX< 3, 3, ScalarT > MatrixType
Definition: Covariance.h:85
GfxTL::IncrementalCovarianceMatrix< MatrixXX< 3, 3, ScalarT > >::IncrementalCovarianceMatrix
IncrementalCovarianceMatrix()
Definition: Covariance.h:88
GfxTL::IncrementalCovarianceMatrix< MatrixXX< 3, 3, ScalarT > >::PointType
VectorXD< 3, ScalarType > PointType
Definition: Covariance.h:86
armarx::mean
std::optional< float > mean(const boost::circular_buffer< NameValueMap > &buffer, const std::string &key)
Definition: KinematicUnitGuiPlugin.cpp:1620
GfxTL::IncrementalCovarianceMatrix::PointType
VectorXD< MatrixT::Rows, ScalarType > PointType
Definition: Covariance.h:15
armarx::PointT
pcl::PointXYZRGBL PointT
Definition: Common.h:30
GfxTL::IncrementalCovarianceMatrix::ScalarType
MatrixT::ScalarType ScalarType
Definition: Covariance.h:13
GfxTL::IncrementalCovarianceMatrix::IncrementalCovarianceMatrix
IncrementalCovarianceMatrix()
Definition: Covariance.h:17
GfxTL
Definition: AABox.h:9
GfxTL::IncrementalCovarianceMatrix::MatrixType
MatrixT MatrixType
Definition: Covariance.h:14
GfxTL::IncrementalCovarianceMatrix::Add
void Add(WeightT w, const PointT &p)
Definition: Covariance.h:33
GfxTL::IncrementalCovarianceMatrix::Reset
void Reset()
Definition: Covariance.h:67
GfxTL::IncrementalCovarianceMatrix< MatrixXX< 3, 3, ScalarT > >::ScalarType
ScalarT ScalarType
Definition: Covariance.h:84
GfxTL::CovarianceMatrix
void CovarianceMatrix(const PointT &center, PointsForwardIt begin, PointsForwardIt end, WeightsForwardIt weights, MatrixT *matrix)
Definition: Covariance.h:175
GfxTL::IncrementalCovarianceMatrix::MeanAndMatrix
void MeanAndMatrix(PointType *mean, MatrixT *m) const
Definition: Covariance.h:60
GfxTL::IncrementalCovarianceMatrix< MatrixXX< 3, 3, ScalarT > >::Matrix
void Matrix(MatrixType *m) const
Definition: Covariance.h:121
GfxTL::IncrementalCovarianceMatrix< MatrixXX< 3, 3, ScalarT > >::MeanAndMatrix
void MeanAndMatrix(PointType *mean, MatrixType *m) const
Definition: Covariance.h:153
GfxTL::IncrementalCovarianceMatrix< MatrixXX< 3, 3, ScalarT > >::Add
void Add(WeightT w, const PointT &p)
Definition: Covariance.h:104
ScalarTypeDeferer.h
GfxTL::IncrementalCovarianceMatrix< MatrixXX< 3, 3, ScalarT > >::Reset
void Reset()
Definition: Covariance.h:160
GfxTL::UnitWeightIterator
Definition: WeightFunc.h:80
GfxTL::IncrementalCovarianceMatrix< MatrixXX< 3, 3, ScalarT > >::Mean
void Mean(PointType *mean) const
Definition: Covariance.h:147
GfxTL::IncrementalCovarianceMatrix< MatrixXX< 3, 3, ScalarT > >::Add
void Add(const PointT &p)
Definition: Covariance.h:97