gtsam_points
Loading...
Searching...
No Matches
scan_matching_reduction.hpp
1// SPDX-License-Identifier: MIT
2// Copyright (c) 2024 Kenji Koide (k.koide@aist.go.jp)
3#pragma once
4
5#include <Eigen/Core>
6#include <gtsam_points/config.hpp>
7
8#ifdef GTSAM_POINTS_USE_TBB
9#include <tbb/blocked_range.h>
10#include <tbb/parallel_reduce.h>
11#endif
12
13namespace gtsam_points {
14
15template <typename Transform>
16double scan_matching_reduce_omp(
17 const Transform f,
18 int num_points,
19 int num_threads,
20 Eigen::Matrix<double, 6, 6>* H_target,
21 Eigen::Matrix<double, 6, 6>* H_source,
22 Eigen::Matrix<double, 6, 6>* H_target_source,
23 Eigen::Matrix<double, 6, 1>* b_target,
24 Eigen::Matrix<double, 6, 1>* b_source) {
25 double sum_errors = 0.0;
26
27 const int num_Hs = H_target ? num_threads : 0;
28 std::vector<Eigen::Matrix<double, 6, 6>> Hs_target(num_Hs, Eigen::Matrix<double, 6, 6>::Zero());
29 std::vector<Eigen::Matrix<double, 6, 6>> Hs_source(num_Hs, Eigen::Matrix<double, 6, 6>::Zero());
30 std::vector<Eigen::Matrix<double, 6, 6>> Hs_target_source(num_Hs, Eigen::Matrix<double, 6, 6>::Zero());
31 std::vector<Eigen::Matrix<double, 6, 1>> bs_target(num_Hs, Eigen::Matrix<double, 6, 1>::Zero());
32 std::vector<Eigen::Matrix<double, 6, 1>> bs_source(num_Hs, Eigen::Matrix<double, 6, 1>::Zero());
33
34#pragma omp parallel for num_threads(num_threads) schedule(guided, 8) reduction(+ : sum_errors)
35 for (int i = 0; i < num_points; i++) {
36 int thread_num = 0;
37#ifdef _OPENMP
38 thread_num = omp_get_thread_num();
39#endif
40
41 double error = 0.0;
42 if (Hs_target.empty()) {
43 error = f(i, nullptr, nullptr, nullptr, nullptr, nullptr);
44 } else {
45 error = f(i, &Hs_target[thread_num], &Hs_source[thread_num], &Hs_target_source[thread_num], &bs_target[thread_num], &bs_source[thread_num]);
46 }
47
48 sum_errors += error;
49 }
50
51 if (H_target) {
52 *H_target = Hs_target[0];
53 *H_source = Hs_source[0];
54 *H_target_source = Hs_target_source[0];
55 *b_target = bs_target[0];
56 *b_source = bs_source[0];
57
58 for (int i = 1; i < num_threads; i++) {
59 *H_target += Hs_target[i];
60 *H_source += Hs_source[i];
61 *H_target_source += Hs_target_source[i];
62 *b_target += bs_target[i];
63 *b_source += bs_source[i];
64 }
65 }
66
67 return sum_errors;
68}
69
70#ifdef GTSAM_POINTS_USE_TBB
71
72template <typename Transform>
73struct ScanMatchingReductionTBBError {
74public:
75 ScanMatchingReductionTBBError(const Transform f) : f(f), sum_errors(0.0) {}
76 ScanMatchingReductionTBBError(const ScanMatchingReductionTBBError& other, tbb::split) : f(other.f), sum_errors(0.0) {}
77
78 void operator()(const tbb::blocked_range<int>& range) {
79 double local_sum_errors = sum_errors;
80 for (int i = range.begin(); i != range.end(); i++) {
81 local_sum_errors += f(i, nullptr, nullptr, nullptr, nullptr, nullptr);
82 }
83 sum_errors = local_sum_errors;
84 }
85
86 void join(const ScanMatchingReductionTBBError& other) { sum_errors += other.sum_errors; }
87
88public:
89 const Transform f;
90 double sum_errors;
91};
92
93template <typename Transform>
94struct ScanMatchingReductionTBBLinearize {
95public:
96 ScanMatchingReductionTBBLinearize(const Transform f)
97 : f(f),
98 sum_errors(0.0),
99 H_target(Eigen::Matrix<double, 6, 6>::Zero()),
100 H_source(Eigen::Matrix<double, 6, 6>::Zero()),
101 H_target_source(Eigen::Matrix<double, 6, 6>::Zero()),
102 b_target(Eigen::Matrix<double, 6, 1>::Zero()),
103 b_source(Eigen::Matrix<double, 6, 1>::Zero()) {}
104
105 ScanMatchingReductionTBBLinearize(const ScanMatchingReductionTBBLinearize& other, tbb::split)
106 : f(other.f),
107 sum_errors(0.0),
108 H_target(Eigen::Matrix<double, 6, 6>::Zero()),
109 H_source(Eigen::Matrix<double, 6, 6>::Zero()),
110 H_target_source(Eigen::Matrix<double, 6, 6>::Zero()),
111 b_target(Eigen::Matrix<double, 6, 1>::Zero()),
112 b_source(Eigen::Matrix<double, 6, 1>::Zero()) {}
113
114 void operator()(const tbb::blocked_range<int>& range) {
115 double local_sum_errors = sum_errors;
116 Eigen::Matrix<double, 6, 6> local_H_target = Eigen::Matrix<double, 6, 6>::Zero();
117 Eigen::Matrix<double, 6, 6> local_H_source = Eigen::Matrix<double, 6, 6>::Zero();
118 Eigen::Matrix<double, 6, 6> local_H_target_source = Eigen::Matrix<double, 6, 6>::Zero();
119 Eigen::Matrix<double, 6, 1> local_b_target = Eigen::Matrix<double, 6, 1>::Zero();
120 Eigen::Matrix<double, 6, 1> local_b_source = Eigen::Matrix<double, 6, 1>::Zero();
121
122 for (int i = range.begin(); i != range.end(); i++) {
123 local_sum_errors += f(i, &local_H_target, &local_H_source, &local_H_target_source, &local_b_target, &local_b_source);
124 }
125
126 sum_errors = local_sum_errors;
127 H_target += local_H_target;
128 H_source += local_H_source;
129 H_target_source += local_H_target_source;
130 b_target += local_b_target;
131 b_source += local_b_source;
132 }
133
134 void join(const ScanMatchingReductionTBBLinearize& other) {
135 sum_errors += other.sum_errors;
136 H_target += other.H_target;
137 H_source += other.H_source;
138 H_target_source += other.H_target_source;
139 b_target += other.b_target;
140 b_source += other.b_source;
141 }
142
143public:
144 const Transform f;
145 double sum_errors;
146 Eigen::Matrix<double, 6, 6> H_target;
147 Eigen::Matrix<double, 6, 6> H_source;
148 Eigen::Matrix<double, 6, 6> H_target_source;
149 Eigen::Matrix<double, 6, 1> b_target;
150 Eigen::Matrix<double, 6, 1> b_source;
151};
152
153template <typename Transform>
154double scan_matching_reduce_tbb(
155 const Transform f,
156 int num_points,
157 Eigen::Matrix<double, 6, 6>* H_target,
158 Eigen::Matrix<double, 6, 6>* H_source,
159 Eigen::Matrix<double, 6, 6>* H_target_source,
160 Eigen::Matrix<double, 6, 1>* b_target,
161 Eigen::Matrix<double, 6, 1>* b_source) {
162 if (H_target) {
163 ScanMatchingReductionTBBLinearize<Transform> reduction(f);
164 tbb::parallel_reduce(tbb::blocked_range<int>(0, num_points, 32), reduction);
165 *H_target = reduction.H_target;
166 *H_source = reduction.H_source;
167 *H_target_source = reduction.H_target_source;
168 *b_target = reduction.b_target;
169 *b_source = reduction.b_source;
170 return reduction.sum_errors;
171 } else {
172 ScanMatchingReductionTBBError<Transform> reduction(f);
173 tbb::parallel_reduce(tbb::blocked_range<int>(0, num_points, 32), reduction);
174 return reduction.sum_errors;
175 }
176}
177
178#else
179template <typename Transform>
180double scan_matching_reduce_tbb(
181 const Transform f,
182 int num_points,
183 Eigen::Matrix<double, 6, 6>* H_target,
184 Eigen::Matrix<double, 6, 6>* H_source,
185 Eigen::Matrix<double, 6, 6>* H_target_source,
186 Eigen::Matrix<double, 6, 1>* b_target,
187 Eigen::Matrix<double, 6, 1>* b_source) {
188 std::cerr << "warning : TBB is not available" << std::endl;
189 return scan_matching_reduce_omp(f, num_points, 1, H_target, H_source, H_target_source, b_target, b_source);
190}
191#endif
192
193} // namespace gtsam_points
Generated by doxygen 1.9.8