Skip to content

Commit 4cfd41f

Browse files
committed
avoid using thrust in header
1 parent d74f3df commit 4cfd41f

7 files changed

Lines changed: 42 additions & 19 deletions
Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
// SPDX-License-Identifier: MIT
2+
// Copyright (c) 2025 Kenji Koide (k.koide@aist.go.jp)
3+
4+
#pragma once
5+
6+
#ifdef __CUDACC__
7+
#define GTSAM_POINTS_HOST_DEVICE __host__ __device__
8+
#else
9+
#define GTSAM_POINTS_HOST_DEVICE
10+
#endif
11+
12+
namespace gtsam_points {
13+
14+
/// @brief A pair of source and target point indices representing a correspondence.
15+
struct Correspondence {
16+
GTSAM_POINTS_HOST_DEVICE Correspondence() : source_idx(-1), target_idx(-1) {}
17+
GTSAM_POINTS_HOST_DEVICE Correspondence(int source_idx, int target_idx) : source_idx(source_idx), target_idx(target_idx) {}
18+
19+
int source_idx;
20+
int target_idx;
21+
};
22+
23+
} // namespace gtsam_points

include/gtsam_points/cuda/kernels/gicp_derivatives.cuh

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
#include <Eigen/Core>
77
#include <thrust/device_vector.h>
88

9+
#include <gtsam_points/cuda/kernels/correspondence.hpp>
910
#include <gtsam_points/cuda/kernels/pose.cuh>
1011
#include <gtsam_points/cuda/kernels/linearized_system.cuh>
1112

@@ -24,9 +25,9 @@ struct gicp_derivatives_kernel {
2425
source_means_ptr(source_means),
2526
source_covs_ptr(source_covs) {}
2627

27-
__device__ LinearizedSystem6 operator()(const thrust::pair<int, int>& source_target_correspondence) const {
28-
const int source_idx = source_target_correspondence.first;
29-
const int target_idx = source_target_correspondence.second;
28+
__device__ LinearizedSystem6 operator()(const Correspondence& source_target_correspondence) const {
29+
const int source_idx = source_target_correspondence.source_idx;
30+
const int target_idx = source_target_correspondence.target_idx;
3031
if (source_idx < 0 || target_idx < 0) {
3132
return LinearizedSystem6::zero();
3233
}
@@ -93,9 +94,9 @@ struct gicp_error_kernel {
9394
source_means_ptr(source_means),
9495
source_covs_ptr(source_covs) {}
9596

96-
__device__ float operator()(const thrust::pair<int, int>& source_target_correspondence) const {
97-
const int source_idx = source_target_correspondence.first;
98-
const int target_idx = source_target_correspondence.second;
97+
__device__ float operator()(const Correspondence& source_target_correspondence) const {
98+
const int source_idx = source_target_correspondence.source_idx;
99+
const int target_idx = source_target_correspondence.target_idx;
99100
if (source_idx < 0 || target_idx < 0) {
100101
return 0.0f;
101102
}

include/gtsam_points/factors/integrated_gicp_derivatives.cuh

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,9 @@
66
#include <memory>
77
#include <Eigen/Core>
88
#include <Eigen/Geometry>
9-
#include <thrust/pair.h>
109

1110
#include <gtsam_points/ann/kdtree_gpu.hpp>
11+
#include <gtsam_points/cuda/kernels/correspondence.hpp>
1212
#include <gtsam_points/types/point_cloud_gpu.hpp>
1313

1414
struct CUstream_st;
@@ -78,6 +78,6 @@ private:
7878

7979
int num_inliers;
8080
int* num_inliers_gpu;
81-
thrust::pair<int, int>* source_target_correspondences;
81+
Correspondence* source_target_correspondences;
8282
};
8383
} // namespace gtsam_points

src/gtsam_points/factors/integrated_gicp_derivatives_compute.cu

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -34,8 +34,8 @@ struct kdtree_correspondence_kernel {
3434
target_indices(target_indices),
3535
target_nodes(target_nodes) {}
3636

37-
__device__ thrust::pair<int, int> operator()(const thrust::pair<int, int>& source_target) const {
38-
const int source_idx = source_target.first;
37+
__device__ Correspondence operator()(const Correspondence& source_target) const {
38+
const int source_idx = source_target.source_idx;
3939
if (source_idx < 0) {
4040
return source_target;
4141
}
@@ -50,7 +50,7 @@ struct kdtree_correspondence_kernel {
5050

5151
const auto [nn_idx, sq_dist] = nn_search(transed_pt);
5252

53-
return thrust::make_pair(source_idx, static_cast<int>(nn_idx));
53+
return Correspondence(source_idx, static_cast<int>(nn_idx));
5454
}
5555

5656
const Eigen::Isometry3f* linearization_point_ptr;

src/gtsam_points/factors/integrated_gicp_derivatives_inliers.cu

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,6 @@
88
#include <thrust/sequence.h>
99
#include <thrust/host_vector.h>
1010
#include <thrust/transform.h>
11-
#include <thrust/pair.h>
1211

1312
#include <gtsam_points/cuda/check_error.cuh>
1413
#include <gtsam_points/cuda/kernels/pose.cuh>
@@ -20,7 +19,7 @@ namespace gtsam_points {
2019
namespace {
2120

2221
struct init_source_target_indices_kernel {
23-
__host__ __device__ thrust::pair<int, int> operator()(const int i) const { return thrust::make_pair(i, -1); }
22+
__host__ __device__ Correspondence operator()(const int i) const { return Correspondence(i, -1); }
2423
};
2524

2625
} // namespace
@@ -35,7 +34,7 @@ void IntegratedGICPDerivatives::reset_inliers(const Eigen::Isometry3f& x, const
3534
inlier_evaluation_point_gpu = d_x;
3635

3736
check_error << cudaFreeAsync(source_target_correspondences, stream);
38-
check_error << cudaMallocAsync(&source_target_correspondences, sizeof(thrust::pair<int, int>) * source->size(), stream);
37+
check_error << cudaMallocAsync(&source_target_correspondences, sizeof(Correspondence) * source->size(), stream);
3938
thrust::transform(
4039
thrust::cuda::par_nosync.on(stream),
4140
thrust::make_counting_iterator<int>(0),

src/gtsam_points/factors/integrated_gicp_derivatives_linearize.cu

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -39,8 +39,8 @@ struct kdtree_correspondence_kernel {
3939
target_nodes(target_nodes),
4040
max_correspondence_distance_sq(max_correspondence_distance_sq) {}
4141

42-
__device__ thrust::pair<int, int> operator()(const thrust::pair<int, int>& source_target) const {
43-
const int source_idx = source_target.first;
42+
__device__ Correspondence operator()(const Correspondence& source_target) const {
43+
const int source_idx = source_target.source_idx;
4444
if (source_idx < 0) {
4545
return source_target;
4646
}
@@ -56,10 +56,10 @@ struct kdtree_correspondence_kernel {
5656
const auto [nn_idx, sq_dist] = nn_search(transed_pt);
5757

5858
if (sq_dist > max_correspondence_distance_sq) {
59-
return thrust::make_pair(source_idx, -1);
59+
return Correspondence(source_idx, -1);
6060
}
6161

62-
return thrust::make_pair(source_idx, static_cast<int>(nn_idx));
62+
return Correspondence(source_idx, static_cast<int>(nn_idx));
6363
}
6464

6565
const Eigen::Isometry3f* linearization_point_ptr;

src/gtsam_points/factors/integrated_gicp_factor_gpu.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -120,7 +120,7 @@ size_t IntegratedGICPFactorGPU::memory_usage() const {
120120
}
121121

122122
size_t IntegratedGICPFactorGPU::memory_usage_gpu() const {
123-
return sizeof(Eigen::Isometry3f) + sizeof(int) + sizeof(std::pair<int, int>) * derivatives->get_num_inliers();
123+
return sizeof(Eigen::Isometry3f) + sizeof(int) + sizeof(Correspondence) * derivatives->get_num_inliers();
124124
}
125125

126126
void IntegratedGICPFactorGPU::set_enable_offloading(bool enable) {

0 commit comments

Comments
 (0)