mirror
/
small_gicp
mirror of https://github.com/koide3/small_gicp.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

test for factors (#37) 

* test for factors

* fix factor test
This commit is contained in:
koide3 2024-04-25 14:44:29 +09:00 committed by GitHub
parent 2880f9ce18
commit 06114f860d
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 41 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/python/align.cpp
View File

@ -102,10 +102,12 @@ void define_align(py::module& m) {
KdTreeOMP<PointCloud>::ConstPtr target_tree,
const Eigen::Matrix4d& init_T_target_source,
double max_correspondence_distance,
int num_threads) {
int num_threads,
int max_iterations) {
Registration<GICPFactor, ParallelReductionOMP> registration;
registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;
registration.reduction.num_threads = num_threads;
registration.optimizer.max_iterations = max_iterations;
if (target_tree == nullptr) {
target_tree = std::make_shared<KdTreeOMP<PointCloud>>(target, num_threads);
@ -117,15 +119,23 @@ void define_align(py::module& m) {
py::arg("target_tree") = nullptr,
py::arg("init_T_target_source") = Eigen::Matrix4d::Identity(),
py::arg("max_correspondence_distance") = 1.0,
py::arg("num_threads") = 1);
py::arg("num_threads") = 1,
py::arg("max_iterations") = 20);
// align
m.def(
"align",
[](const GaussianVoxelMap& target_voxelmap, const PointCloud& source, const Eigen::Matrix4d& init_T_target_source, double max_correspondence_distance, int num_threads) {
[](
const GaussianVoxelMap& target_voxelmap,
const PointCloud& source,
const Eigen::Matrix4d& init_T_target_source,
double max_correspondence_distance,
int num_threads,
int max_iterations) {
Registration<GICPFactor, ParallelReductionOMP> registration;
registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;
registration.reduction.num_threads = num_threads;
registration.optimizer.max_iterations = max_iterations;
return registration.align(target_voxelmap, source, target_voxelmap, Eigen::Isometry3d(init_T_target_source));
},
@ -133,5 +143,6 @@ void define_align(py::module& m) {
py::arg("source"),
py::arg("init_T_target_source") = Eigen::Matrix4d::Identity(),
py::arg("max_correspondence_distance") = 1.0,
py::arg("num_threads") = 1);
py::arg("num_threads") = 1,
py::arg("max_iterations") = 20);
}

26
src/test/python_test.py
View File

@ -138,6 +138,32 @@ def test_voxelmap(load_points):
assert voxelmap.size() > 0
assert voxelmap.size() == len(voxelmap)
# Factor test
def test_factors(load_points):
gt_T_target_source, target_raw_numpy, source_raw_numpy = load_points
target, target_tree = small_gicp.preprocess_points(target_raw_numpy, downsampling_resolution=0.25)
source, source_tree = small_gicp.preprocess_points(source_raw_numpy, downsampling_resolution=0.25)
result = small_gicp.align(target, source, target_tree, gt_T_target_source)
result = small_gicp.align(target, source, target_tree, result.T_target_source)
factors = [small_gicp.GICPFactor()]
rejector = small_gicp.DistanceRejector()
sum_H = numpy.zeros((6, 6))
sum_b = numpy.zeros(6)
sum_e = 0.0
for i in range(source.size()):
succ, H, b, e = factors[0].linearize(target, source, target_tree, result.T_target_source, i, rejector)
if succ:
sum_H += H
sum_b += b
sum_e += e
assert numpy.max(numpy.abs(result.H - sum_H) / result.H) < 0.05
# Registration test
def test_registration(load_points):
gt_T_target_source, target_raw_numpy, source_raw_numpy = load_points