-
Notifications
You must be signed in to change notification settings - Fork 77
/
math_sse.h
109 lines (88 loc) · 3.02 KB
/
math_sse.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
/**
* This file is part of dvo.
*
* Copyright 2012 Christian Kerl <christian.kerl@in.tum.de> (Technical University of Munich)
* For more information see <http://vision.in.tum.de/data/software/dvo>.
*
* dvo is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* dvo is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with dvo. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef MATH_SSE_H_
#define MATH_SSE_H_
#include <Eigen/Core>
namespace dvo
{
namespace core
{
#define HIDE_CTOR(type) \
private: \
type() {}; \
type(const type& other) {}; \
~type() {}; \
struct Sse
{
enum {
Enabled,
Disabled
};
};
/**
* A 6x6 self adjoint matrix with optimized "rankUpdate(u, scale)" (10x faster than Eigen impl, 1.8x faster than MathSse::addOuterProduct(...)).
*/
class OptimizedSelfAdjointMatrix6x6f
{
public:
OptimizedSelfAdjointMatrix6x6f();
void rankUpdate(const Eigen::Matrix<float, 6, 1>& u, const float& alpha);
void operator +=(const OptimizedSelfAdjointMatrix6x6f& other);
void setZero();
void toEigen(Eigen::Matrix<float, 6, 6>& m) const;
private:
enum {
Size = 24
};
EIGEN_ALIGN16 float data[Size];
};
template<int Enabled, typename NumType>
class MathSse
{
public:
static void addOuterProduct(Eigen::Matrix<NumType, 6, 6>& mat, const Eigen::Matrix<NumType, 6, 1>& vec, const NumType& scale);
static void add(Eigen::Matrix<NumType, 6, 1>& vec, const Eigen::Matrix<NumType, 6, 1>& other, const NumType& scale);
HIDE_CTOR(MathSse)
};
template<typename NumType>
class MathSse<Sse::Disabled, NumType>
{
public:
static void addOuterProduct(Eigen::Matrix<NumType, 6, 6>& mat, const Eigen::Matrix<NumType, 6, 1>& vec, const NumType& scale)
{
mat += vec * vec.transpose() * scale;
}
static void add(Eigen::Matrix<NumType, 6, 1>& vec, const Eigen::Matrix<NumType, 6, 1>& other, const NumType& scale)
{
vec += other * scale;
}
HIDE_CTOR(MathSse)
};
template<>
void MathSse<Sse::Enabled, float>::addOuterProduct(Eigen::Matrix<float, 6, 6>& mat, const Eigen::Matrix<float, 6, 1>& vec, const float& scale);
template<>
void MathSse<Sse::Enabled, double>::addOuterProduct(Eigen::Matrix<double, 6, 6>& mat, const Eigen::Matrix<double, 6, 1>& vec, const double& scale);
template<>
void MathSse<Sse::Enabled, float>::add(Eigen::Matrix<float, 6, 1>& vec, const Eigen::Matrix<float, 6, 1>& other, const float& scale);
template<>
void MathSse<Sse::Enabled, double>::add(Eigen::Matrix<double, 6, 1>& vec, const Eigen::Matrix<double, 6, 1>& other, const double& scale);
} /* namespace core */
} /* namespace dvo */
#endif /* MATH_SSE_H_ */