Fraxinus  18.10
An IGT application
intersection_set.hpp
Go to the documentation of this file.
1 #ifndef INTERSECTION_SET_HPP
2 #define INTERSECTION_SET_HPP
3 #include "intersection.hpp"
4 
5 #include <vector>
6 
12 template<typename T>
13 class IntersectionSet : public std::vector<Intersection<T> >
14 {
15 public:
16 
26  {
27  m_direction = 0.0;
28  m_have_direction = false;
29  m_have_velocity_ls = false;
30  m_velocity_ls = 0.0;
31  m_dir_a = 0.07;
32  m_dir_b = 0.9;
33  m_dir_A = 10.0;
34  m_vel_a = 0.17;
35  m_vel_b = 1.0;
36  }
37 public:
42  void
44  {
45  std::pair<T,T> vel_weight;
46  struct
47  {
48  T A;
49  T a;
50  T b;
51  std::pair<T,T>& operator()(std::pair<T,T>& ret, Intersection<T>& i)
52  {
53 
54  T weight = i.sampleWeight(A, a,b);
55  T tmp = i.getAverage()*i.getCosTheta();
56  tmp = weight*tmp/abs(tmp);
57 
58  if(!std::isnan(tmp))
59  {
60  ret.first += tmp;
61  ret.second += weight;
62 
63  }
64  return ret;
65 
66  }
67  } weight_accum;
68 
69  weight_accum.a = m_dir_a;
70  weight_accum.b = m_dir_b;
71  weight_accum.A = m_dir_A;
72  vel_weight = std::accumulate(this->begin(), this->end(), std::make_pair(0.0, 0.0),
73  weight_accum);
74  m_direction = vel_weight.first/vel_weight.second;
75  m_have_direction = true;
76  if(std::isnan(m_direction))
77  {
78  m_direction = 0.0;
79  }
80  }
81 
86  void
88  {
89  std::pair<T,T> top_bottom;
90 
91  struct
92  {
93  T a;
94  T b;
95  std::pair<T,T>& operator()(std::pair<T,T> &ret, Intersection<T> &i)
96  {
97  if(abs(i.getCosTheta()) < a || abs(i.getCosTheta()) > b){
98  return ret;
99  }
100  double tmp1, tmp2;
101 
102  tmp1 = i.getAverage()*i.getCosTheta();
103  tmp2 = i.getCosTheta()*i.getCosTheta();
104  if(!std::isnan(tmp1) && !std::isnan(tmp2))
105  {
106  ret.first += tmp1;
107  ret.second += tmp2;
108  }
109  return ret;
110  }
111  } accum;
112 
113  accum.a = m_vel_a;
114  accum.b = m_vel_b;
115  top_bottom = std::accumulate(this->begin(), this->end(),
116  std::make_pair(0.0,0.0), accum);
117  m_velocity_ls = top_bottom.first/top_bottom.second;
118  m_have_velocity_ls = true;
119 
120  if(std::isnan(m_velocity_ls ))
121  {
122  m_velocity_ls = 0.0;
123  }
124  }
125 
130  void
132  {
133  for(auto &intersection : *this)
134  {
135  intersection.correctAliasing(m_direction,Vnyq);
136  }
137  }
138 
145  void setDirectionEstimationParameters(T A, T lower, T upper)
146  {
147  m_dir_A = A;
148  m_dir_a = lower;
149  m_dir_b = upper;
150  }
151 
157  void setVelocityEstimationCutoff(T lower, T upper)
158  {
159  m_vel_a = lower;
160  m_vel_b = upper;
161  }
162 
168  {
169  if(!m_have_direction)
171  return m_direction;
172  }
173 
179  {
180  if(!m_have_velocity_ls)
182  return m_velocity_ls;
183  }
184 
185 
186 
187 private:
188  T m_direction;
189  bool m_have_direction;
190  bool m_have_velocity_ls;
191  T m_velocity_ls;
192  T m_dir_a, m_dir_b;
193  T m_vel_a, m_vel_b;
194  T m_dir_A;
195 
196 };
197 
198 
199 
200 
201 #endif
Scalar * end()
T getCosTheta() const
Scalar * begin()
void setVelocityEstimationCutoff(T lower, T upper)
T sampleWeight(const T A, const T a, const T b) const
void setDirectionEstimationParameters(T A, T lower, T upper)
DoubleBoundingBox3D intersection(DoubleBoundingBox3D a, DoubleBoundingBox3D b)
void correctAliasing(T Vnyq)