Skip to content

Commit f31b1ac

Browse files
neka-natneka-nat
committed
add ransac registration
1 parent 433437c commit f31b1ac

14 files changed

+862
-14
lines changed

‎CMakeLists.txt

+5
Original file line numberDiff line numberDiff line change
@@ -159,6 +159,11 @@ elseif (UNIX)
159159
message(STATUS "Compiling on Unix")
160160
endif ()
161161

162+
find_package(OpenMP)
163+
if(OpenMP_FOUND)
164+
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
165+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
166+
endif()
162167
find_package(CUDA REQUIRED)
163168
if(NOT CMAKE_CUDA_ARCHITECTURES)
164169
set(CMAKE_CUDA_ARCHITECTURES 86)

‎src/cupoch/geometry/pointcloud.cu

+8
Original file line numberDiff line numberDiff line change
@@ -267,6 +267,14 @@ PointCloud &PointCloud::Transform(const Eigen::Matrix4f &transformation) {
267267
return *this;
268268
}
269269

270+
PointCloud &PointCloud::Transform(cudaStream_t stream1, cudaStream_t stream2,
271+
const Eigen::Matrix4f &transformation) {
272+
TransformPoints<3>(stream1, transformation, points_);
273+
TransformNormals(stream2, transformation, normals_);
274+
cudaSafeCall(cudaDeviceSynchronize());
275+
return *this;
276+
}
277+
270278
std::shared_ptr<PointCloud> PointCloud::Crop(
271279
const AxisAlignedBoundingBox<3> &bbox) const {
272280
if (bbox.IsEmpty()) {

‎src/cupoch/geometry/pointcloud.h

+2
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,8 @@ class PointCloud : public GeometryBase3D {
6767
AxisAlignedBoundingBox<3> GetAxisAlignedBoundingBox() const override;
6868
OrientedBoundingBox GetOrientedBoundingBox() const;
6969
PointCloud &Transform(const Eigen::Matrix4f &transformation) override;
70+
PointCloud &Transform(cudaStream_t stream1, cudaStream_t stream2,
71+
const Eigen::Matrix4f &transformation);
7072
PointCloud &Translate(const Eigen::Vector3f &translation,
7173
bool relative = true) override;
7274
PointCloud &Scale(const float scale, bool center = true) override;

‎src/cupoch/registration/colored_icp.cu

+3-1
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,7 @@ public:
5757
const geometry::PointCloud &target,
5858
const CorrespondenceSet &corres) const override;
5959
Eigen::Matrix4f ComputeTransformation(
60+
cudaStream_t stream1, cudaStream_t stream2,
6061
const geometry::PointCloud &source,
6162
const geometry::PointCloud &target,
6263
const CorrespondenceSet &corres) const override;
@@ -216,6 +217,7 @@ struct compute_jacobian_and_residual_functor
216217
};
217218

218219
Eigen::Matrix4f TransformationEstimationForColoredICP::ComputeTransformation(
220+
cudaStream_t stream1, cudaStream_t stream2,
219221
const geometry::PointCloud &source,
220222
const geometry::PointCloud &target,
221223
const CorrespondenceSet &corres) const {
@@ -244,7 +246,7 @@ Eigen::Matrix4f TransformationEstimationForColoredICP::ComputeTransformation(
244246
thrust::tie(JTJ, JTr, r2) =
245247
utility::ComputeJTJandJTr<Eigen::Matrix6f, Eigen::Vector6f, 2,
246248
compute_jacobian_and_residual_functor>(
247-
func, (int)corres.size());
249+
stream1, func, (int)corres.size());
248250

249251
bool is_success;
250252
Eigen::Matrix4f extrinsic;

‎src/cupoch/registration/correspondence_checker.cu

+171
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,171 @@
1+
/**
2+
* Copyright (c) 2024 Neka-Nat
3+
* Permission is hereby granted, free of charge, to any person obtaining a copy
4+
* of this software and associated documentation files (the "Software"), to deal
5+
* in the Software without restriction, including without limitation the rights
6+
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7+
* copies of the Software, and to permit persons to whom the Software is
8+
* furnished to do so, subject to the following conditions:
9+
*
10+
* The above copyright notice and this permission notice shall be included in
11+
* all copies or substantial portions of the Software.
12+
*
13+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14+
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15+
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16+
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17+
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18+
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19+
* IN THE SOFTWARE.
20+
**/
21+
#include "cupoch/registration/correspondence_checker.h"
22+
23+
#include <Eigen/Dense>
24+
#include <thrust/logical.h>
25+
26+
#include "cupoch/geometry/pointcloud.h"
27+
#include "cupoch/utility/console.h"
28+
29+
namespace cupoch {
30+
namespace registration {
31+
32+
namespace {
33+
struct edge_length_checker_functor {
34+
float similarity_threshold_;
35+
size_t corres_size_;
36+
const Eigen::Vector3f* source_points_;
37+
const Eigen::Vector3f* target_points_;
38+
const Eigen::Vector2i* corres_;
39+
edge_length_checker_functor(
40+
float similarity_threshold,
41+
size_t corres_size,
42+
const Eigen::Vector3f* source_points,
43+
const Eigen::Vector3f* target_points,
44+
const Eigen::Vector2i* corres)
45+
: similarity_threshold_(similarity_threshold),
46+
corres_size_(corres_size),
47+
source_points_(source_points),
48+
target_points_(target_points),
49+
corres_(corres) {}
50+
__device__ bool operator()(int idx) {
51+
int i = idx / corres_size_;
52+
int j = idx % corres_size_;
53+
if (i == j) return true;
54+
float dis_source = (source_points_[corres_[i](0)] - source_points_[corres_[j](0)]).norm();
55+
float dis_target = (target_points_[corres_[i](1)] - target_points_[corres_[j](1)]).norm();
56+
return dis_source >= dis_target * similarity_threshold_ &&
57+
dis_target >= dis_source * similarity_threshold_;
58+
}
59+
60+
};
61+
62+
struct distance_checker_functor {
63+
float distance_threshold_;
64+
const Eigen::Vector3f* source_points_;
65+
const Eigen::Vector3f* target_points_;
66+
const Eigen::Matrix4f transformation_;
67+
distance_checker_functor(
68+
float distance_threshold,
69+
const Eigen::Vector3f* source_points,
70+
const Eigen::Vector3f* target_points,
71+
const Eigen::Matrix4f transformation)
72+
: distance_threshold_(distance_threshold),
73+
source_points_(source_points),
74+
target_points_(target_points),
75+
transformation_(transformation) {}
76+
__device__ bool operator()(const Eigen::Vector2i& corr) {
77+
const auto &pt = source_points_[corr(0)];
78+
Eigen::Vector3f pt_trans =
79+
(transformation_ * Eigen::Vector4f(pt(0), pt(1), pt(2), 1.0))
80+
.block<3, 1>(0, 0);
81+
return (target_points_[
82+
corr(1)] - pt_trans).norm() <= distance_threshold_;
83+
}
84+
};
85+
86+
struct normal_checker_functor {
87+
float cos_normal_angle_threshold_;
88+
const Eigen::Vector3f* source_points_;
89+
const Eigen::Vector3f* target_points_;
90+
const Eigen::Vector3f* source_normals_;
91+
const Eigen::Vector3f* target_normals_;
92+
const Eigen::Matrix4f transformation_;
93+
normal_checker_functor(
94+
float cos_normal_angle_threshold,
95+
const Eigen::Vector3f* source_points,
96+
const Eigen::Vector3f* target_points,
97+
const Eigen::Vector3f* source_normals,
98+
const Eigen::Vector3f* target_normals,
99+
const Eigen::Matrix4f transformation)
100+
: cos_normal_angle_threshold_(cos_normal_angle_threshold),
101+
source_points_(source_points),
102+
target_points_(target_points),
103+
source_normals_(source_normals),
104+
target_normals_(target_normals),
105+
transformation_(transformation) {}
106+
__device__ bool operator()(const Eigen::Vector2i& corr) {
107+
const auto &normal = source_normals_[corr(0)];
108+
Eigen::Vector3f normal_trans =
109+
(transformation_ *
110+
Eigen::Vector4f(normal(0), normal(1), normal(2), 0.0))
111+
.block<3, 1>(0, 0);
112+
return target_normals_[corr(1)].dot(normal) >= cos_normal_angle_threshold_;
113+
}
114+
};
115+
}
116+
117+
bool CorrespondenceCheckerBasedOnEdgeLength::Check(
118+
const geometry::PointCloud &source,
119+
const geometry::PointCloud &target,
120+
const CorrespondenceSet &corres,
121+
const Eigen::Matrix4f & /*transformation*/) const {
122+
return thrust::all_of(
123+
thrust::make_counting_iterator<size_t>(0),
124+
thrust::make_counting_iterator(corres.size() * corres.size()),
125+
edge_length_checker_functor(
126+
similarity_threshold_,
127+
corres.size(),
128+
thrust::raw_pointer_cast(source.points_.data()),
129+
thrust::raw_pointer_cast(target.points_.data()),
130+
thrust::raw_pointer_cast(corres.data())));
131+
}
132+
133+
bool CorrespondenceCheckerBasedOnDistance::Check(
134+
const geometry::PointCloud &source,
135+
const geometry::PointCloud &target,
136+
const CorrespondenceSet &corres,
137+
const Eigen::Matrix4f &transformation) const {
138+
return thrust::all_of(
139+
corres.begin(), corres.end(),
140+
distance_checker_functor(
141+
distance_threshold_,
142+
thrust::raw_pointer_cast(source.points_.data()),
143+
thrust::raw_pointer_cast(target.points_.data()),
144+
transformation));
145+
}
146+
147+
bool CorrespondenceCheckerBasedOnNormal::Check(
148+
const geometry::PointCloud &source,
149+
const geometry::PointCloud &target,
150+
const CorrespondenceSet &corres,
151+
const Eigen::Matrix4f &transformation) const {
152+
if (!source.HasNormals() || !target.HasNormals()) {
153+
utility::LogWarning(
154+
"[CorrespondenceCheckerBasedOnNormal::Check] Pointcloud has no "
155+
"normals.");
156+
return true;
157+
}
158+
float cos_normal_angle_threshold = std::cos(normal_angle_threshold_);
159+
return thrust::all_of(
160+
corres.begin(), corres.end(),
161+
normal_checker_functor(
162+
cos_normal_angle_threshold,
163+
thrust::raw_pointer_cast(source.points_.data()),
164+
thrust::raw_pointer_cast(target.points_.data()),
165+
thrust::raw_pointer_cast(source.normals_.data()),
166+
thrust::raw_pointer_cast(target.normals_.data()),
167+
transformation));
168+
}
169+
170+
} // namespace registration
171+
} // namespace cupoch

‎src/cupoch/registration/correspondence_checker.h

+159
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,159 @@
1+
/**
2+
* Copyright (c) 2024 Neka-Nat
3+
* Permission is hereby granted, free of charge, to any person obtaining a copy
4+
* of this software and associated documentation files (the "Software"), to deal
5+
* in the Software without restriction, including without limitation the rights
6+
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7+
* copies of the Software, and to permit persons to whom the Software is
8+
* furnished to do so, subject to the following conditions:
9+
*
10+
* The above copyright notice and this permission notice shall be included in
11+
* all copies or substantial portions of the Software.
12+
*
13+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14+
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15+
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16+
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17+
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18+
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19+
* IN THE SOFTWARE.
20+
**/
21+
#pragma once
22+
23+
#include <Eigen/Core>
24+
#include <memory>
25+
#include <string>
26+
#include <vector>
27+
28+
#include "cupoch/registration/transformation_estimation.h"
29+
30+
namespace cupoch {
31+
32+
namespace geometry {
33+
class PointCloud;
34+
}
35+
36+
namespace registration {
37+
38+
/// \class CorrespondenceChecker
39+
///
40+
/// \brief Base class that checks if two (small) point clouds can be aligned.
41+
///
42+
/// This class is used in feature based matching algorithms (such as RANSAC and
43+
/// FastGlobalRegistration) to prune out outlier correspondences.
44+
/// The virtual function Check() must be implemented in subclasses.
45+
class CorrespondenceChecker {
46+
public:
47+
/// \brief Default Constructor.
48+
///
49+
/// \param require_pointcloud_alignment Specifies whether point cloud
50+
/// alignment is required.
51+
CorrespondenceChecker(bool require_pointcloud_alignment)
52+
: require_pointcloud_alignment_(require_pointcloud_alignment) {}
53+
virtual ~CorrespondenceChecker() {}
54+
55+
public:
56+
/// \brief Function to check if two points can be aligned.
57+
///
58+
/// \param source Source point cloud.
59+
/// \param target Target point cloud.
60+
/// \param corres Correspondence set between source and target point cloud.
61+
/// \param transformation The estimated transformation (inplace).
62+
virtual bool Check(const geometry::PointCloud &source,
63+
const geometry::PointCloud &target,
64+
const CorrespondenceSet &corres,
65+
const Eigen::Matrix4f &transformation) const = 0;
66+
67+
public:
68+
/// Some checkers do not require point clouds to be aligned, e.g., the edge
69+
/// length checker. Some checkers do, e.g., the distance checker.
70+
bool require_pointcloud_alignment_;
71+
};
72+
73+
/// \class CorrespondenceCheckerBasedOnEdgeLength
74+
///
75+
/// \brief Check if two point clouds build the polygons with similar edge
76+
/// lengths.
77+
///
78+
/// That is, checks if the lengths of any two arbitrary edges (line formed by
79+
/// two vertices) individually drawn withinin source point cloud and within the
80+
/// target point cloud with correspondences are similar. The only parameter
81+
/// similarity_threshold is a number between 0 (loose) and 1 (strict).
82+
class CorrespondenceCheckerBasedOnEdgeLength : public CorrespondenceChecker {
83+
public:
84+
/// \brief Default Constructor.
85+
///
86+
/// \param similarity_threshold specifies the threshold within which 2
87+
/// arbitrary edges are similar.
88+
CorrespondenceCheckerBasedOnEdgeLength(double similarity_threshold = 0.9)
89+
: CorrespondenceChecker(false),
90+
similarity_threshold_(similarity_threshold) {}
91+
~CorrespondenceCheckerBasedOnEdgeLength() override {}
92+
93+
public:
94+
bool Check(const geometry::PointCloud &source,
95+
const geometry::PointCloud &target,
96+
const CorrespondenceSet &corres,
97+
const Eigen::Matrix4f &transformation) const override;
98+
99+
public:
100+
/// For the check to be true,
101+
/// ||edgesource||>similarity_threshold×||edgetarget|| and
102+
/// ||edgetarget||>similarity_threshold×||edgesource|| must hold true for
103+
/// all edges.
104+
double similarity_threshold_;
105+
};
106+
107+
/// \class CorrespondenceCheckerBasedOnDistance
108+
///
109+
/// \brief Check if two aligned point clouds are close.
110+
class CorrespondenceCheckerBasedOnDistance : public CorrespondenceChecker {
111+
public:
112+
/// \brief Default Constructor.
113+
///
114+
/// \param distance_threshold Distance threshold for the check.
115+
CorrespondenceCheckerBasedOnDistance(double distance_threshold)
116+
: CorrespondenceChecker(true),
117+
distance_threshold_(distance_threshold) {}
118+
~CorrespondenceCheckerBasedOnDistance() override {}
119+
120+
public:
121+
bool Check(const geometry::PointCloud &source,
122+
const geometry::PointCloud &target,
123+
const CorrespondenceSet &corres,
124+
const Eigen::Matrix4f &transformation) const override;
125+
126+
public:
127+
/// Distance threshold for the check.
128+
double distance_threshold_;
129+
};
130+
131+
/// \class CorrespondenceCheckerBasedOnNormal
132+
///
133+
/// \brief Class to check if two aligned point clouds have similar normals.
134+
///
135+
/// It considers vertex normal affinity of any correspondences. It computes dot
136+
/// product of two normal vectors. It takes radian value for the threshold.
137+
class CorrespondenceCheckerBasedOnNormal : public CorrespondenceChecker {
138+
public:
139+
/// \brief Parameterized Constructor.
140+
///
141+
/// \param normal_angle_threshold Radian value for angle threshold.
142+
CorrespondenceCheckerBasedOnNormal(double normal_angle_threshold)
143+
: CorrespondenceChecker(true),
144+
normal_angle_threshold_(normal_angle_threshold) {}
145+
~CorrespondenceCheckerBasedOnNormal() override {}
146+
147+
public:
148+
bool Check(const geometry::PointCloud &source,
149+
const geometry::PointCloud &target,
150+
const CorrespondenceSet &corres,
151+
const Eigen::Matrix4f &transformation) const override;
152+
153+
public:
154+
/// Radian value for angle threshold.
155+
double normal_angle_threshold_;
156+
};
157+
158+
} // namespace registration
159+
} // namespace cupoch

0 commit comments

Comments
 (0)