nd.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2016-2017, the mcl_3dl authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef MCL_3DL_ND_H
31 #define MCL_3DL_ND_H
32 
33 #define _USE_MATH_DEFINES
34 #include <cmath>
35 
36 #include <Eigen/Core>
37 #include <Eigen/LU>
38 
39 namespace mcl_3dl
40 {
41 template <typename FLT_TYPE = float>
43 {
44 public:
45  explicit NormalLikelihood(const FLT_TYPE sigma)
46  {
47  a_ = 1.0 / std::sqrt(2.0 * M_PI * sigma * sigma);
48  sq2_ = sigma * sigma * 2.0;
49  }
50  FLT_TYPE operator()(const FLT_TYPE x) const
51  {
52  return a_ * expf(-x * x / sq2_);
53  }
54 
55 protected:
56  FLT_TYPE a_;
57  FLT_TYPE sq2_;
58 };
59 
60 template <typename FLT_TYPE = float, size_t DIMENSION = 6>
62 {
63 public:
64  using Matrix = Eigen::Matrix<FLT_TYPE, DIMENSION, DIMENSION>;
65  using Vector = Eigen::Matrix<FLT_TYPE, DIMENSION, 1>;
66 
67  explicit NormalLikelihoodNd(const Matrix sigma)
68  {
69  a_ = 1.0 / (std::pow(2.0 * M_PI, 0.5 * DIMENSION) * std::sqrt(sigma.determinant()));
70  sigma_inv_ = sigma.inverse();
71  }
72  FLT_TYPE operator()(const Vector x) const
73  {
74  return a_ * std::exp(static_cast<FLT_TYPE>(-0.5 * x.transpose() * sigma_inv_ * x));
75  }
76 
77 protected:
78  FLT_TYPE a_;
80 };
81 } // namespace mcl_3dl
82 
83 #endif // MCL_3DL_ND_H
FLT_TYPE operator()(const Vector x) const
Definition: nd.h:72
FLT_TYPE operator()(const FLT_TYPE x) const
Definition: nd.h:50
NormalLikelihoodNd(const Matrix sigma)
Definition: nd.h:67
NormalLikelihood(const FLT_TYPE sigma)
Definition: nd.h:45
Eigen::Matrix< FLT_TYPE, DIMENSION, DIMENSION > Matrix
Definition: nd.h:64
Eigen::Matrix< FLT_TYPE, DIMENSION, 1 > Vector
Definition: nd.h:65


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:16:29