WeightFunc.h
Go to the documentation of this file.
1 #ifndef __GfxTL_WEIGHTFUNC_HEADER__
2 #define __GfxTL_WEIGHTFUNC_HEADER__
3 #include <iterator>
4 
5 #include <GfxTL/MathHelper.h>
6 
7 namespace GfxTL
8 {
9 
10  template <class ScalarT>
12  {
13  public:
14  typedef ScalarT ScalarType;
15 
17  {
18  _r = r;
19  _f = ScalarType(1.5) / r;
20  }
21 
23  operator()(ScalarType dist) const
24  {
25  if (dist >= _r)
26  {
27  return 0;
28  }
29  return EvalBSpline(_f * dist);
30  }
31 
32  private:
34  EvalBSpline(ScalarType t) const
35  {
36  //B-spline (degree = 2)
37  if (t < ScalarType(0.5))
38  {
39  return -t * t + ScalarType(0.75);
40  }
41  else
42  return ScalarType(0.5) * (ScalarType(1.5) - t) * (ScalarType(1.5) - t);
43  /*ScalarType x = t + (ScalarType)1.5; // shifted value
44  if(((ScalarType)-1.5 <= t) && (t < (ScalarType)-0.5))
45  return (ScalarType)0.5 * x * x;
46  if(((ScalarType)-0.5 <= t) && (t < (ScalarType)0.5))
47  return (ScalarType)-1.0 * x * x + (ScalarType)3.0 * x
48  - (ScalarType)(3.0 / 2.0);
49  if(((ScalarType)0.5 <= t) && (t < (ScalarType)1.5))
50  return (ScalarType)0.5 * x * x - (ScalarType)3.0 * x
51  + (ScalarType)(9.0 / 2.0);
52  return (ScalarType)0.0; // bspline zero*/
53  }
54 
55  private:
56  ScalarType _f;
57  ScalarType _r;
58  };
59 
60  template <class Point>
62  {
63  public:
64  typedef Point PointType;
65  typedef typename Point::ScalarType ScalarType;
66 
67  double
68  Weight(const Point&)
69  {
70  return (double)1;
71  }
72 
73  double
74  operator()(const Point&)
75  {
76  return (double)1;
77  }
78  };
79 
81  {
82  public:
83  typedef unsigned int value_type;
84  typedef unsigned int* pointer;
85  typedef unsigned int& reference;
86  typedef std::forward_iterator_tag iterator_category;
87  typedef size_t difference_type;
88 
89  const unsigned int
90  operator*() const
91  {
92  return 1;
93  }
94 
96  operator++() const
97  {
98  return *this;
99  }
100  };
101 
102  template <class PointT>
104  {
105  public:
106  typedef PointT PointType;
107  typedef typename PointType::ScalarType ScalarType;
108 
110  _center(center), _radius(radius)
111  {
112  }
113 
114  ScalarType
115  operator()(const PointType& p) const
116  {
117  ScalarType d = (p - _center).Length();
118  return operator()(d);
119  }
120 
121  ScalarType
123  {
124  ScalarType a = Math<ScalarType>::Max(_radius - d, 0) / (_radius * d);
125  return a * a;
126  }
127 
128  private:
129  PointType _center;
130  ScalarType _radius;
131  };
132 
133  template <class PointT>
135  {
136  public:
137  typedef PointT PointType;
138  typedef typename PointType::ScalarType ScalarType;
139 
141  _center(center)
142  {
143  _scale = ScalarType(11.38335808) / sqrRadius;
144  }
145 
147  {
148  _center.Zero();
149  _scale = 1;
150  }
151 
152  ScalarType
153  operator()(const PointType& p) const
154  {
155  return operator()(_center.SqrDistance(p));
156  }
157 
158  ScalarType
159  operator()(ScalarType sqrDist) const
160  {
161  sqrDist *= _scale;
162  return std::exp(-sqrDist) / sqrDist;
163  }
164 
165  bool
166  WeightAndDerivative(const PointType& x, ScalarType* weight, PointType* derivative)
167  {
168  ScalarType sqrDist = _scale * _center.SqrDistance(x);
169  if (sqrDist < 1e-6)
170  {
171  return false;
172  }
173  *weight = std::exp(-sqrDist) / sqrDist;
174  for (unsigned int i = 0; i < PointType::Dim; ++i)
175  {
176  (*derivative)[i] = *weight * (2 * (x[i] - _center[i]));
177  (*derivative)[i] += (*derivative)[i] / sqrDist;
178  }
179  return true;
180  }
181 
182  private:
183  PointType _center;
184  ScalarType _scale;
185  };
186 
187  template <class PointT>
189  {
190  public:
191  typedef PointT PointType;
192  typedef typename PointType::ScalarType ScalarType;
193 
195  _radius(radius), _sqrRadius(_radius * _radius), _cbcRadius(_sqrRadius * _radius)
196  {
197  }
198 
200  _center(center),
201  _radius(radius),
202  _sqrRadius(_radius * _radius),
203  _cbcRadius(_sqrRadius * _radius)
204  {
205  }
206 
207  ScalarType
208  operator()(const PointType& p) const
209  {
210  return operator()((p - _center).Length());
211  }
212 
213  ScalarType
214  operator()(ScalarType dist) const
215  {
216  ScalarType sqrDist = dist * dist;
217  return 2 * sqrDist * dist / _cbcRadius - 3 * sqrDist / _sqrRadius + 1;
218  }
219 
220  bool
221  WeightAndDerivative(const PointType& x, ScalarType* weight, PointType* derivative)
222  {
223  return false;
224  }
225 
226  private:
227  ScalarType _radius, _sqrRadius, _cbcRadius;
228  PointType _center;
229  };
230 }; // namespace GfxTL
231 
232 #endif
GfxTL::CubicGaussApproximationWeightFunc::operator()
ScalarType operator()(ScalarType dist) const
Definition: WeightFunc.h:214
GfxTL::UnitWeightFunc::Weight
double Weight(const Point &)
Definition: WeightFunc.h:68
GfxTL::CubicGaussApproximationWeightFunc::ScalarType
PointType::ScalarType ScalarType
Definition: WeightFunc.h:192
GfxTL::CubicGaussApproximationWeightFunc::WeightAndDerivative
bool WeightAndDerivative(const PointType &x, ScalarType *weight, PointType *derivative)
Definition: WeightFunc.h:221
GfxTL::InterpolatingExponentialWeightFunc::InterpolatingExponentialWeightFunc
InterpolatingExponentialWeightFunc(const PointType &center, ScalarType sqrRadius)
Definition: WeightFunc.h:140
GfxTL::UnitWeightFunc::PointType
Point PointType
Definition: WeightFunc.h:64
GfxTL::InverseDistanceSingularWeightFunc::InverseDistanceSingularWeightFunc
InverseDistanceSingularWeightFunc(const PointType &center, ScalarType radius)
Definition: WeightFunc.h:109
GfxTL::BSplineWeightFunc::BSplineWeightFunc
BSplineWeightFunc(ScalarType r)
Definition: WeightFunc.h:16
GfxTL::CubicGaussApproximationWeightFunc::CubicGaussApproximationWeightFunc
CubicGaussApproximationWeightFunc(PointType center, ScalarType radius)
Definition: WeightFunc.h:199
GfxTL::InterpolatingExponentialWeightFunc::operator()
ScalarType operator()(ScalarType sqrDist) const
Definition: WeightFunc.h:159
Point::ScalarType
float ScalarType
Definition: PointCloud.h:28
GfxTL::InverseDistanceSingularWeightFunc::ScalarType
PointType::ScalarType ScalarType
Definition: WeightFunc.h:107
GfxTL::UnitWeightFunc::operator()
double operator()(const Point &)
Definition: WeightFunc.h:74
GfxTL::UnitWeightIterator::reference
unsigned int & reference
Definition: WeightFunc.h:85
GfxTL::InterpolatingExponentialWeightFunc::PointType
PointT PointType
Definition: WeightFunc.h:137
GfxTL::InverseDistanceSingularWeightFunc
Definition: WeightFunc.h:103
GfxTL::InverseDistanceSingularWeightFunc::operator()
ScalarType operator()(const PointType &p) const
Definition: WeightFunc.h:115
GfxTL::InterpolatingExponentialWeightFunc
Definition: WeightFunc.h:134
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
GfxTL::CubicGaussApproximationWeightFunc::operator()
ScalarType operator()(const PointType &p) const
Definition: WeightFunc.h:208
GfxTL::InterpolatingExponentialWeightFunc::operator()
ScalarType operator()(const PointType &p) const
Definition: WeightFunc.h:153
Point
Definition: PointCloud.h:21
GfxTL::InterpolatingExponentialWeightFunc::InterpolatingExponentialWeightFunc
InterpolatingExponentialWeightFunc()
Definition: WeightFunc.h:146
armarx::PointT
pcl::PointXYZRGBL PointT
Definition: Common.h:30
GfxTL::BSplineWeightFunc::ScalarType
ScalarT ScalarType
Definition: WeightFunc.h:14
GfxTL::UnitWeightIterator::operator*
const unsigned int operator*() const
Definition: WeightFunc.h:90
GfxTL::UnitWeightFunc::ScalarType
Point::ScalarType ScalarType
Definition: WeightFunc.h:65
GfxTL
Definition: AABox.h:9
GfxTL::UnitWeightIterator::operator++
UnitWeightIterator operator++() const
Definition: WeightFunc.h:96
GfxTL::BSplineWeightFunc::operator()
ScalarType operator()(ScalarType dist) const
Definition: WeightFunc.h:23
GfxTL::UnitWeightIterator::iterator_category
std::forward_iterator_tag iterator_category
Definition: WeightFunc.h:86
GfxTL::InverseDistanceSingularWeightFunc::operator()
ScalarType operator()(ScalarType d) const
Definition: WeightFunc.h:122
GfxTL::InverseDistanceSingularWeightFunc::PointType
PointT PointType
Definition: WeightFunc.h:106
GfxTL::InterpolatingExponentialWeightFunc::ScalarType
PointType::ScalarType ScalarType
Definition: WeightFunc.h:138
GfxTL::CubicGaussApproximationWeightFunc::CubicGaussApproximationWeightFunc
CubicGaussApproximationWeightFunc(ScalarType radius)
Definition: WeightFunc.h:194
GfxTL::CubicGaussApproximationWeightFunc::PointType
PointT PointType
Definition: WeightFunc.h:191
GfxTL::UnitWeightIterator::value_type
unsigned int value_type
Definition: WeightFunc.h:83
GfxTL::CubicGaussApproximationWeightFunc
Definition: WeightFunc.h:188
armarx::view_selection::skills::direction::state::center
state::Type center(state::Type previous)
Definition: LookDirection.cpp:233
GfxTL::UnitWeightFunc
Definition: WeightFunc.h:61
GfxTL::UnitWeightIterator::difference_type
size_t difference_type
Definition: WeightFunc.h:87
GfxTL::InterpolatingExponentialWeightFunc::WeightAndDerivative
bool WeightAndDerivative(const PointType &x, ScalarType *weight, PointType *derivative)
Definition: WeightFunc.h:166
GfxTL::Math::Max
static ScalarT Max(ScalarT a, ScalarT b)
Definition: MathHelper.h:53
GfxTL::UnitWeightIterator::pointer
unsigned int * pointer
Definition: WeightFunc.h:84
MathHelper.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
GfxTL::UnitWeightIterator
Definition: WeightFunc.h:80
GfxTL::BSplineWeightFunc
Definition: WeightFunc.h:11