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
110
111
112
113
114
115
116
117
|
#include <manif/manif.h>
#include <benchmark/benchmark.h>
template<typename Scalar>
void normalizeQuat(Eigen::Quaternion<Scalar>& q) {
// see https://github.com/zarathustr/quat_norm
q.coeffs() /= q.coeffs().cwiseAbs().maxCoeff();
for (char j = 0; j < 3; ++j) {
const Scalar N = q.squaredNorm();
q.coeffs() *= (5.0 + N) / (2.0 + 4.0 * N);
}
}
Eigen::Quaternion<double> randQuat() {
// @note: We are using:
// http://mathworld.wolfram.com/HyperspherePointPicking.html
using std::sqrt;
using Scalar = double;
using Quaternion = Eigen::Quaternion<Scalar>;
Scalar u1, u2;
do {
u1 = Eigen::internal::random<Scalar>(-1, 1),
u2 = Eigen::internal::random<Scalar>(-1, 1);
} while (u1 * u1 + u2 * u2 > Scalar(1));
Scalar u3, u4, n;
do {
u3 = Eigen::internal::random<Scalar>(-1, 1),
u4 = Eigen::internal::random<Scalar>(-1, 1);
n = u3 * u3 + u4 * u4;
} while (n > Scalar(1.0));
const Scalar zw_factor = sqrt((Scalar(1) - u1 * u1 - u2 * u2) / n);
return Quaternion(u1, u2, u3 * zw_factor, u4 * zw_factor);
}
// Normalization benchmark
static void BM_EigenNormalize(benchmark::State& state) {
Eigen::Quaternion<double> q;
// Use the underlying vector random so that
// the quaternion isn't normalized already
q.coeffs().setRandom();
double norm=0;
for (auto _ : state) {
q.normalize();
state.PauseTiming();
norm += q.norm();
q.coeffs().setRandom();
state.ResumeTiming();
}
state.counters["normAvg"] = benchmark::Counter(
norm, benchmark::Counter::kAvgIterations
);
}
BENCHMARK(BM_EigenNormalize);
static void BM_normalizeQuat(benchmark::State& state) {
Eigen::Quaternion<double> q;
// Use the underlying vector random so that
// the quaternion isn't normalized already
q.coeffs().setRandom();
double norm=0;
for (auto _ : state) {
normalizeQuat(q);
state.PauseTiming();
norm += q.norm();
q.coeffs().setRandom();
state.ResumeTiming();
}
state.counters["normAvg"] = benchmark::Counter(
norm, benchmark::Counter::kAvgIterations
);
}
BENCHMARK(BM_normalizeQuat);
// Random benchmark
static void BM_UnitRandom(benchmark::State& state) {
Eigen::Quaternion<double> q;
double norm=0;
for (auto _ : state) {
benchmark::DoNotOptimize(q = manif::randQuat<double>());
state.PauseTiming();
norm += q.norm();
state.ResumeTiming();
}
state.counters["normAvg"] = benchmark::Counter(
norm, benchmark::Counter::kAvgIterations
);
}
BENCHMARK(BM_UnitRandom);
static void BM_RandQuat(benchmark::State& state) {
Eigen::Quaternion<double> q;
double norm=0;
for (auto _ : state) {
benchmark::DoNotOptimize(q = randQuat());
state.PauseTiming();
norm += q.norm();
state.ResumeTiming();
}
state.counters["normAvg"] = benchmark::Counter(
norm, benchmark::Counter::kAvgIterations
);
}
BENCHMARK(BM_RandQuat);
BENCHMARK_MAIN();
|
