Choreonoid  1.1
GaussianFilter.h
説明を見る。
1 
6 #ifndef CNOID_UTIL_GAUSSIAN_FILTER_H_INCLUDED
7 #define CNOID_UTIL_GAUSSIAN_FILTER_H_INCLUDED
8 
9 #include <cmath>
10 #include <vector>
11 
12 namespace cnoid {
13 
14  template <class T>
15  void setGaussWindow(T sigma, int range, std::vector<T>& out_window)
16  {
17  const double mu = 0.0;
18 
19  out_window.resize(range * 2 + 1);
20 
21  T x = 0.0;
22  for(int i = 0; i <= range; ++i){
23  out_window[i + range] = exp(- (x - mu) * (x - mu) / (2.0 * sigma * sigma));
24  }
25  // nomalization
26  T area_half = out_window[range] / 2.0;
27  for(int i=1; i <= range; ++i){
28  area_half += out_window[i + range];
29  }
30  const T area = area_half * 2.0;
31 
32  out_window[range] /= area;
33  for(int i=1; i <= range; ++i){
34  T& v = out_window[i + range];
35  v /= area;
36  out_window[range - i] = v;
37  }
38  }
39 
40 
41  template <class RESULTVECTOR, class SRCVECTOR, class ELEMENT, class T>
42  void applyGaussianFilter(RESULTVECTOR& result, const SRCVECTOR& src, std::vector<T>& gwin, ELEMENT zero)
43  {
44  const int range = (gwin.size() - 1) / 2;
45  const int size = src.size();
46 
47  // head
48  for(int i=0; i < range; i++){
49  ELEMENT v = zero; T ave = 0.0;
50  for(int j = - i; j <= +range; j++){
51  v += src[i+j] * gwin[j+range];
52  ave += gwin[j+range];
53  }
54  result[i] = v / ave;
55  }
56 
57  // body
58  for(int i=range; i < size - range; i++){
59  ELEMENT v = zero;
60  for(int j=-range; j <= +range; j++){
61  v += src[i+j] * gwin[j+range];
62  }
63  result[i] = v;
64  }
65 
66  // tail
67  for(int i = size - range; i < size; i++){
68  ELEMENT v = zero; T ave = 0.0;
69  for(int j=-range; j < size - i; j++){
70  v += src[i+j] * gwin[j+range];
71  ave += gwin[j+range];
72  }
73  result[i] = v / ave;
74  }
75  }
76 
77 
78  template <class RESULTVECTOR, class SRCVECTOR, class ELEMENT, class T>
79  void applyGaussianFilter(RESULTVECTOR& result, const SRCVECTOR& src, T sigma, int range, ELEMENT zero)
80  {
81  std::vector<T> gwin;
82  setGaussWindow(sigma, range, gwin);
83  applyGaussianFilter(result, src, gwin, zero);
84  }
85 }
86 
87 #endif