Kernel.cpp
Go to the documentation of this file.
1 // (C) Copyright Renaud Detry 2007-2015.
2 // Distributed under the GNU General Public License and under the
3 // BSD 3-Clause License (See accompanying file LICENSE.txt).
4 
5 /** @file */
6 
7 #include <nuklei/Kernel.h>
8 #include <nuklei/Common.h>
9 #include <nuklei/Indenter.h>
10 #include <boost/static_assert.hpp>
11 
12 namespace nuklei
13 {
14 
15 
16  namespace kernel
17  {
18 
19  const std::string base::TypeNames[] = { "r3", "r3xs2", "r3xs2p", "se3" };
20 
21  std::ostream& operator<<(std::ostream &out, const kernel::base &k)
22  {
23  return k.polyPrint(out);
24  }
25 
26  // SE(3)
27 
28  void se3::assertConsistency() const
29  {
30  NUKLEI_TRACE_BEGIN();
31  NUKLEI_ASSERT(w_ >= 0);
32  la::check(ori_, __FUNCTION__);
33  NUKLEI_ASSERT(loc_h_ >= 0);
34  NUKLEI_ASSERT(ori_h_ >= 0);
35  NUKLEI_TRACE_END();
36  }
37 
38  std::ostream& se3::print(std::ostream &out) const
39  {
40  Indenter idt(out);
41  idt << "Kernel SE(3): [ weight = " << w_ << " ]" << std::endl;
42  {
43  Indenter idt2(out);
44  idt2 << "Location: " << loc_ << std::endl;
45  idt2 << "LocWidth: " << loc_h_ << std::endl;
46  idt2 << "Orientation: ";
47  ::nuklei::operator<<(idt2, ori_) << std::endl;
48  idt2 << "OriWidth: " << ori_h_ << std::endl;
49  }
50  return out;
51  }
52 
53  kernel::base::Type se3::type() const
54  {
55  return SE3;
56  }
57 
58  //coord_t se3::eval(const kernel::se3& k) const
59  //cf. header.
60 
61  kernel::se3 se3::sample() const
62  {
63  kernel::se3 s;
64  typedef nuklei::sampler<PositionKernel> PositionSampler;
65 
66  typedef nuklei::sampler<OrientationKernel> OrientationSampler;
67 
68  s.loc_ = PositionSampler::s(loc_, loc_h_);
69  s.ori_ = OrientationSampler::s(ori_, ori_h_);
70 
71  if (hasDescriptor()) s.setDescriptor(getDescriptor());
72  return s;
73  }
74 
75  kernel::se3 se3::se3Sample() const
76  {
77  return sample();
78  }
79 
80  kernel::se3 se3::se3Proj() const
81  {
82  kernel::se3 k;
83  k.loc_ = loc_;
84  k.ori_ = ori_;
85  if (hasDescriptor()) k.setDescriptor(getDescriptor());
86  return *this;
87  }
88 
89  kernel::se3 se3::projectedOn(const kernel::se3& k) const
90  {
91  kernel::se3 p;
92  la::project(p.loc_, p.ori_, k.loc_, k.ori_, loc_, ori_);
93  if (hasDescriptor()) p.setDescriptor(getDescriptor());
94  return p;
95  }
96 
97  kernel::se3 se3::transformedWith(const kernel::se3& k) const
98  {
99  kernel::se3 p;
100  la::transform(p.loc_, p.ori_, k.loc_, k.ori_, loc_, ori_);
101  if (hasDescriptor()) p.setDescriptor(getDescriptor());
102  return p;
103  }
104 
105  void se3::makeTransformWith(const kernel::se3& k)
106  {
107  la::transform(loc_, ori_, k.loc_, k.ori_, loc_, ori_);
108  }
109 
110  kernel::se3 se3::transformationFrom(const kernel::se3& k) const
111  {
112  kernel::se3 p;
113  la::transfoTo(p.loc_, p.ori_, k.loc_, k.ori_, loc_, ori_);
114  if (hasDescriptor()) p.setDescriptor(getDescriptor());
115  return p;
116  }
117 
118  kernel::se3 se3::inverseTransformation() const
119  {
120  kernel::se3 origin;
121  kernel::se3 it = origin.transformationFrom(*this);
122  if (hasDescriptor()) it.setDescriptor(getDescriptor());
123  return it;
124  }
125 
126  kernel::se3
127  kernel::se3::linearInterpolation(const kernel::se3& k,
128  const coord_t x) const
129  {
130  NUKLEI_ASSERT(0 <= x && x <= 1);
131  kernel::se3 i;
132  i.loc_ = (1-x) * loc_ + x * k.loc_;
133  Quaternion q = k.ori_;
134  if (ori_.Dot(q) < 0) q = - q;
135  //fixme: this should use slerp.
136  i.ori_ = (1-x) * ori_ + x * q;
137  i.ori_.Normalize();
138  return i;
139  }
140 
141  void kernel::se3::updateWidth(const kernel::se3& k,
142  const coord_t x)
143  {
144  loc_h_ = std::sqrt( (1-x) * loc_h_*loc_h_ +
145  x * std::pow(dist<groupS::r3>::d(loc_, k.loc_), 2) );
146  ori_h_ = std::sqrt( (1-x) * ori_h_*ori_h_ +
147  x * std::pow(dist<groupS::so3>::d(ori_, k.ori_), 2) );
148  }
149 
150  coord_pair kernel::se3::distanceTo(const kernel::se3& k) const
151  {
152  return std::make_pair(dist<groupS::r3>::d(loc_, k.loc_),
153  dist<groupS::so3>::d(ori_, k.ori_));
154  }
155 
156  // S^2(_+)
157 
158  // R^3
159 
160  void r3::assertConsistency() const
161  {
162  NUKLEI_TRACE_BEGIN();
163  NUKLEI_ASSERT(w_ >= 0);
164  NUKLEI_ASSERT(loc_h_ >= 0);
165  NUKLEI_TRACE_END();
166  }
167 
168  std::ostream& r3::print(std::ostream &out) const
169  {
170  Indenter idt(out);
171  idt << "Kernel R^3: [ weight = " << w_ << " ]" << std::endl;
172  {
173  Indenter idt2(out);
174  idt2 << "Location: " << loc_ << std::endl;
175  idt2 << "LocWidth: " << loc_h_ << std::endl;
176  }
177  return out;
178  }
179 
180  kernel::base::Type r3::type() const
181  {
182  return R3;
183  }
184 
185  //coord_t r3::eval(const kernel::r3& k) const
186  //cf. header.
187 
188  kernel::r3 r3::sample() const
189  {
190  kernel::r3 s;
191 
192  typedef nuklei::sampler<PositionKernel> PositionSampler;
193 
194  s.loc_ = PositionSampler::s(loc_, loc_h_);
195 
196  if (hasDescriptor()) s.setDescriptor(getDescriptor());
197  return s;
198  }
199 
200  kernel::se3 r3::se3Sample() const
201  {
202  kernel::r3 r3k = sample();
203  kernel::se3 se3k;
204  se3k.loc_ = r3k.loc_;
205  se3k.ori_ = random_element<groupS::so3>::r();
206  if (hasDescriptor()) se3k.setDescriptor(getDescriptor());
207  return se3k;
208  }
209 
210  kernel::se3 r3::se3Proj() const
211  {
212  kernel::r3 r3k = *this;
213  kernel::se3 se3k;
214  se3k.loc_ = r3k.loc_;
215  se3k.ori_ = random_element<groupS::so3>::r();
216  if (hasDescriptor()) se3k.setDescriptor(getDescriptor());
217  return se3k;
218  }
219 
220  kernel::r3 r3::projectedOn(const kernel::se3& k) const
221  {
222  kernel::r3 p;
223  p.loc_ = la::project(k.loc_, k.ori_, loc_);
224  if (hasDescriptor()) p.setDescriptor(getDescriptor());
225  return p;
226  }
227 
228  kernel::r3 r3::transformedWith(const kernel::se3& k) const
229  {
230  kernel::r3 p;
231  p.loc_ = la::transform(k.loc_, k.ori_, loc_);
232  if (hasDescriptor()) p.setDescriptor(getDescriptor());
233  return p;
234  }
235 
236  void r3::makeTransformWith(const kernel::se3& k)
237  {
238  loc_ = la::transform(k.loc_, k.ori_, loc_);
239  }
240 
241  kernel::r3
242  kernel::r3::linearInterpolation
243  (const kernel::r3& k,
244  const coord_t x) const
245  {
246  NUKLEI_ASSERT(0 <= x && x <= 1);
247  kernel::r3 i;
248  i.loc_ = (1-x) * loc_ + x * k.loc_;
249  return i;
250  }
251 
252  void kernel::r3::updateWidth(const kernel::r3& k,
253  const coord_t x)
254  {
255  loc_h_ = std::sqrt( (1-x) * loc_h_*loc_h_ +
256  x * std::pow(dist<groupS::r3>::d(loc_, k.loc_), 2) );
257  }
258 
259  coord_pair
260  kernel::r3::distanceTo(const kernel::r3& k) const
261  {
262  return std::make_pair(dist<groupS::r3>::d(loc_, k.loc_), -1.);
263  }
264 
265  }
266 
267  namespace enforce_data_align {
268 
269  BOOST_STATIC_ASSERT(sizeof(kernel::base) % 8 == 0);
270 
271  }
272 
273 }
274 
275 #if BOOST_VERSION < 104100
276 
277 BOOST_CLASS_EXPORT_GUID(nuklei::kernel::se3, "mdfh_kernel_se3")
278 BOOST_CLASS_EXPORT_GUID(nuklei::kernel::r3xs2, "mdfh_kernel_r3xs2")
279 BOOST_CLASS_EXPORT_GUID(nuklei::kernel::r3xs2p, "mdfh_kernel_r3xs2p")
280 BOOST_CLASS_EXPORT_GUID(nuklei::kernel::r3, "mdfh_kernel_r3")
281 
282 #else
283 
284 NUKLEI_SERIALIZATION_REGISTER_TYPE(nuklei::kernel::se3)
285 NUKLEI_SERIALIZATION_REGISTER_TYPE(nuklei::kernel::r3xs2)
286 NUKLEI_SERIALIZATION_REGISTER_TYPE(nuklei::kernel::r3xs2p)
287 NUKLEI_SERIALIZATION_REGISTER_TYPE(nuklei::kernel::r3)
288 
289 #endif // BOOST_VERSION
nuklei::kernel::se3::ori_
Quaternion ori_
Kernel orientation.
Definition: Kernel.h:482
nuklei::kernel::base::Type
Type
Explicit query of a kernel's type. See Type Queries for more info.
Definition: Kernel.h:53
Kernel.h
nuklei::kernel::se3::ori_h_
coord_t ori_h_
Orientation bandwidth, in radians.
Definition: Kernel.h:486
nuklei
Public namespace.
Definition: Color.cpp:9
nuklei::coord_pair
std::pair< coord_t, coord_t > coord_pair
Pair of coord_t.
Definition: Definitions.h:27
nuklei::kernel::r3::sample
kernel::r3 sample() const
Returns a sample taken from this kernel.
Definition: Kernel.cpp:188
nuklei::kernel::se3
Definition: Kernel.h:404
nuklei::kernel::se3::loc_
Vector3 loc_
Kernel location.
Definition: Kernel.h:480
nuklei::Indenter
Definition: Indenter.h:16
nuklei::kernel::r3xs2p
r3xs2_base< groupS::s2p > r3xs2p
Definition: Kernel.h:623
nuklei::la::transform
Vector3 transform(const Vector3 &x, const Matrix3 &X, const Vector3 &y)
Returns .
Definition: LinearAlgebra.h:382
NUKLEI_ASSERT
#define NUKLEI_ASSERT(expression)
Throws an Error if expression is not true.
Definition: Common.h:113
nuklei::kernel::base
Polymorphic kernel class.
Definition: Kernel.h:45
nuklei::kernel::r3xs2
r3xs2_base< groupS::s2 > r3xs2
Definition: Kernel.h:618
Indenter.h
nuklei::kernel::se3::sample
kernel::se3 sample() const
Returns a sample taken from this kernel.
Definition: Kernel.cpp:61
nuklei::kernel::r3xs2_base
Definition: Kernel.h:505
nuklei::coord_t
double coord_t
Type for point coordinates.
Definition: Definitions.h:25
nuklei::kernel::r3
Definition: Kernel.h:626
nuklei::kernel::se3::loc_h_
coord_t loc_h_
Location bandwidth.
Definition: Kernel.h:484
Common.h
nuklei::random_element
Definition: GenericKernel.h:158
nuklei::kernel::base::TypeNames
static const std::string TypeNames[]
Explicit query of a kernel's type. See Type Queries for more info.
Definition: Kernel.h:57
nuklei::kernel::base::polyPrint
virtual std::ostream & polyPrint(std::ostream &out) const =0
Prints the kernel parameters to the provided stream.
nuklei::sampler
Definition: GenericKernel.h:597
nuklei::la::project
Vector3 project(const Vector3 &x, const Matrix3 &X, const Vector3 &z)
Returns .
Definition: LinearAlgebra.h:434
nuklei::la::transfoTo
void transfoTo(Vector3 &x, Matrix3 &X, const Vector3 &y, const Matrix3 &Y, const Vector3 &z, const Matrix3 &Z)
Definition: LinearAlgebra.h:486
© Copyright 2007-2013 Renaud Detry.
Distributed under the terms of the GNU General Public License (GPL).
(See accompanying file LICENSE.txt or copy at http://www.gnu.org/copyleft/gpl.html.)
Revised Sun Sep 13 2020 19:10:06.