smrt-aist/fast_gicp

A collection of GICP-based fast point cloud registration algorithms

fast_gicp's Issues

reduce failed to synchronize: cudaErrorIllegalAddress: an illegal memory access was encountered

Hi,@koide3
I am following the latest master branch.
It occasionally appear when utilizing fast_gicp::FastVGICPCuda.
Any advice?
thanks.

Full message:
terminate called after throwing an instance of 'thrust::system::system_error'
what(): reduce failed to synchronize: cudaErrorIllegalAddress: an illegal memory access was encountered

it tracks back to :

return transform_reduce(thrust::detail::derived_cast(thrust::detail::strip_const(exec)), first, last, unary_op, init, binary_op);

full stack::
__GI_raise 0x00007f18959dafb7
__GI_abort 0x00007f18959dc921
0x00007f18963cf957
0x00007f18963d5ae6
std::terminate() 0x00007f18963d5b21
__cxa_throw 0x00007f18963d5d54
thrust::cuda_cub::throw_on_error util.h:213
thrust::cuda_cub::detail::reduce_n_impl<thrust::cuda_cub::tag, thrust::cuda_cub::transform_input_iterator_t<thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type>, thrust::zip_iterator<thrust::tuple<thrust::detail::normal_iterator<thrust::device_ptr<Eigen::Matrix<float, 3, 1, 0, 3, 1> const> >, thrust::detail::normal_iterator<thrust::device_ptr<Eigen::Matrix<float, 3, 3, 0, 3, 3> const> >, thrust::detail::normal_iterator<thrust::device_ptr >, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type> >, fast_gicp::cuda::(anonymous namespace)::compute_derivatives_kernel<thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type> > >, long, thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type>, fast_gicp::cuda::(anonymous namespace)::sum_errors_kernel> reduce.h:982
thrust::cuda_cub::reduce_n<thrust::cuda_cub::tag, thrust::cuda_cub::transform_input_iterator_t<thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type>, thrust::zip_iterator<thrust::tuple<thrust::detail::normal_iterator<thrust::device_ptr<Eigen::Matrix<float, 3, 1, 0, 3, 1> const> >, thrust::detail::normal_iterator<thrust::device_ptr<Eigen::Matrix<float, 3, 3, 0, 3, 3> const> >, thrust::detail::normal_iterator<thrust::device_ptr >, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type> >, fast_gicp::cuda::(anonymous namespace)::compute_derivatives_kernel<thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type> > >, long, thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type>, fast_gicp::cuda::(anonymous namespace)::sum_errors_kernel> reduce.h:1017
thrust::cuda_cub::transform_reduce<thrust::cuda_cub::tag, thrust::zip_iterator<thrust::tuple<thrust::detail::normal_iterator<thrust::device_ptr<Eigen::Matrix<float, 3, 1, 0, 3, 1> const> >, thrust::detail::normal_iterator<thrust::device_ptr<Eigen::Matrix<float, 3, 3, 0, 3, 3> const> >, thrust::detail::normal_iterator<thrust::device_ptr >, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type> >, fast_gicp::cuda::(anonymous namespace)::compute_derivatives_kernel<thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type> >, thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type>, fast_gicp::cuda::(anonymous namespace)::sum_errors_kernel> transform_reduce.h:58
thrust::transform_reduce<thrust::cuda_cub::tag, thrust::zip_iterator<thrust::tuple<thrust::detail::normal_iterator<thrust::device_ptr<Eigen::Matrix<float, 3, 1, 0, 3, 1> const> >, thrust::detail::normal_iterator<thrust::device_ptr<Eigen::Matrix<float, 3, 3, 0, 3, 3> const> >, thrust::detail::normal_iterator<thrust::device_ptr >, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type> >, fast_gicp::cuda::(anonymous namespace)::compute_derivatives_kernel<thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type> >, thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type>, fast_gicp::cuda::(anonymous namespace)::sum_errors_kernel> transform_reduce.inl:47
thrust::transform_reduce<thrust::zip_iterator<thrust::tuple<thrust::detail::normal_iterator<thrust::device_ptr<Eigen::Matrix<float, 3, 1, 0, 3, 1> const> >, thrust::detail::normal_iterator<thrust::device_ptr<Eigen::Matrix<float, 3, 3, 0, 3, 3> const> >, thrust::detail::normal_iterator<thrust::device_ptr >, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type> >, fast_gicp::cuda::(anonymous namespace)::compute_derivatives_kernel<thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type> >, thrust::tuple<float, Eigen::Matrix<float, 6, 6, 0, 6, 6>, Eigen::Matrix<float, 6, 1, 0, 6, 1>, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type, thrust::null_type>, fast_gicp::cuda::(anonymous namespace)::sum_errors_kernel> transform_reduce.inl:67
fast_gicp::cuda::compute_derivatives compute_derivatives.cu:156
fast_gicp::cuda::FastVGICPCudaCore::compute_error fast_vgicp_cuda.cu:219

FastVGICPCuda not working while FastVGICP does

I am trying to test both variants of VGICP using the same data set and the cuda variant seems to have a bug and is simply not returning real results.

When I use the cuda variant it seems to be taking some time to do the covariance calcs, but the LM optimization just returns all zeros and or NAN. Switching to gauss newton results in it immediately returning the same input transform (identity in this case).

If it test using VGICP on the same data / params I get the expected results.

I am using cuda 11.4 if that makes any difference.

Any ideas what may be wrong here?

Compilation problems...

Hi, Thank you for your great work.
But I'm stuck with a compilation problem.

• Errors
  /home/swbot/slamware/slamros/forest_ws/SC_LeGO_LOAM_addGPS/devel/lib/libfast_gicp.so: undefined reference topcl::search::Searchpcl::PointXYZI::getName() const'
  collect2: error: ld returned 1 exit status
  fast_gicp/CMakeFiles/gicp_align.dir/build.make:429: recipe for target '/home/swbot/slamware/slamros/forest_ws/SC_LeGO_LOAM_addGPS/devel/lib/fast_gicp/gicp_align' failed
  make[2]: *** [/home/swbot/slamware/slamros/forest_ws/SC_LeGO_LOAM_addGPS/devel/lib/fast_gicp/gicp_align] Error 1
  CMakeFiles/Makefile2:504: recipe for target 'fast_gicp/CMakeFiles/gicp_align.dir/all' failed
  make[1]: *** [fast_gicp/CMakeFiles/gicp_align.dir/all] Error 2
  make[1]: *** Waiting for unfinished jobs....
  /home/swbot/slamware/slamros/forest_ws/SC_LeGO_LOAM_addGPS/devel/lib/libfast_gicp.so: undefined reference to pcl::search::Search<pcl::PointXYZ>::getName() const' /home/swbot/slamware/slamros/forest_ws/SC_LeGO_LOAM_addGPS/devel/lib/libfast_gicp.so: undefined reference to pcl::search::Searchpcl::PointXYZI::getName() const'
  collect2: error: ld returned 1 exit status
  fast_gicp/CMakeFiles/gicp_kitti.dir/build.make:429: recipe for target '/home/swbot/slamware/slamros/forest_ws/SC_LeGO_LOAM_addGPS/devel/lib/fast_gicp/gicp_kitti' failed
  `

I tried adding #include <pcl/search/impl/search.hpp> to all the files in SRC directory.
But they don't seem to help.

• Environment
  Ubunto 18.04
  PCL 1.8

Sincerely.

warning: calling a __host__ function from a __host__ __device__ function is not allowed

when i using this:
warning: calling a host function from a host device function is not allowed

Not performing well when duplicate points appear.

Thanks for your attention, I encountered two problems:

• Not able to converge in all cases even though the matching score was high.

• Not performing well when duplicate points appear. The test [.pcd] (https://drive.google.com/open?id=1aQjl0Pf8HW0tGF9ss3jZVvq6aX-KYVom) is here.

When I am using the octree.approxNearestSearch algorithm of PCL, duplicate points will be found and added to the point cloud. The GICP_MT and VGICP methods do not work well in this situation. But the algorithm of PCL library works well.

I am looking forward to the answer and thank you again!

implementation of a function not found error

/home/rajath/libraries/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_st_impl.hpp:73:24: error: no matching function for call to ‘fast_gicp::FastGICPSingleThread<pcl::PointXYZ, pcl::PointXYZ>::calculate_covariances(const PointCloudSourceConstPtr&, pcl::search::KdTree<pcl::PointXYZ, pcl::KdTreeFLANN<pcl::PointXYZ, flann::L2_Simple > >&, std::vector<Eigen::Matrix<float, 4, 4>, Eigen::aligned_allocator<Eigen::Matrix<float, 4, 4> > >&)’
calculate_covariances(cloud, *source_kdtree, source_covs);

This is the error i keep getting when i try to do a make in the build directory. I see that the function has been declared in the header file in include but have no idea why this error keeps showing up when I compile

How to get the transformation matrix from these fast GICP registration?

Hi，

Thanks for your great work!

I could not find the interface to get the transformation matrix after these fast GICP registration.
Could you point out how to use the transformation matrix?

Thanks.

registration problem for model bunny and chew.

Hi, I use your algorithm to registe bunny and chew, it shows that the registration results are poor. And i saw your paper does't has the compare on these model. Can you give me some advices on these result? Thank you!

Hello, I’m very curious, why is the result of execution different from 100 times?

0.999882 -0.0152821 0.00142344 -0.495192
0.0152735 0.999866 0.00585251 -0.136181
-0.00151269 -0.00583008 0.999982 0.0277126
0 0 0 1
0.999886 0.0150565 -0.00145653 0.500585
-0.015065 0.999869 -0.00596729 0.124684
0.00136649 0.00598855 0.999981 -0.0288469
0 0 0 1
0.999882 -0.0152818 0.00142326 -0.495191
0.0152732 0.999866 0.00585304 -0.13619
-0.00151251 -0.00583061 0.999982 0.0277164
0 0 0 1
0.999886 0.0150566 -0.00145653 0.500586
-0.015065 0.999869 -0.00596728 0.124684
0.0013665 0.00598854 0.999981 -0.0288469
0 0 0 1
0.999882 -0.0152803 0.00142338 -0.495188
0.0152718 0.999866 0.00583876 -0.136183
-0.0015124 -0.00581633 0.999982 0.0277216
0 0 0 1
0.999886 0.0150565 -0.00145653 0.500585
-0.015065 0.999869 -0.00596728 0.124684
0.00136649 0.00598854 0.999981 -0.0288469
0 0 0 1
0.999882 -0.0152828 0.00142324 -0.495191
0.0152742 0.999866 0.00585421 -0.136192
-0.00151252 -0.00583179 0.999982 0.0277111
0 0 0 1
0.999886 0.0150565 -0.00145653 0.500585
-0.015065 0.999869 -0.00596727 0.124684
0.00136649 0.00598853 0.999981 -0.0288469
0 0 0 1
0.999882 -0.015282 0.00142347 -0.495192
0.0152734 0.999866 0.00585232 -0.136179
-0.00151272 -0.00582989 0.999982 0.0277121
0 0 0 1
0.999886 0.0150565 -0.00145652 0.500585
-0.015065 0.999869 -0.00596727 0.124684
0.00136649 0.00598853 0.999981 -0.0288469
0 0 0 1
0.999882 -0.0152802 0.00142369 -0.495187
0.0152716 0.999866 0.00583821 -0.136181
-0.00151271 -0.00581578 0.999982 0.0277311
0 0 0 1
0.999886 0.0150566 -0.00145654 0.500585
-0.015065 0.999869 -0.00596723 0.124684
0.0013665 0.00598849 0.999981 -0.0288473
0 0 0 1
0.999882 -0.015282 0.00142347 -0.495192
0.0152735 0.999866 0.00585235 -0.13618
-0.00151271 -0.00582992 0.999982 0.0277117
0 0 0 1
0.999886 0.0150587 -0.00145659 0.500592
-0.0150671 0.999869 -0.00596486 0.124693
0.00136657 0.00598612 0.999981 -0.0288498
0 0 0 1
0.999882 -0.015283 0.00142336 -0.495188
0.0152745 0.999866 0.00585429 -0.136186
-0.00151264 -0.00583186 0.999982 0.0277165
0 0 0 1
0.999886 0.0150565 -0.00145653 0.500585
-0.015065 0.999869 -0.00596727 0.124684
0.00136649 0.00598853 0.999981 -0.0288469
0 0 0 1
0.999882 -0.0152803 0.00142337 -0.495187
0.0152718 0.999866 0.00583879 -0.136182
-0.0015124 -0.00581636 0.999982 0.0277216
0 0 0 1
0.999886 0.0150566 -0.00145654 0.500585
-0.015065 0.999869 -0.00596725 0.124684
0.0013665 0.00598851 0.999981 -0.0288471
0 0 0 1
0.999882 -0.015282 0.00142351 -0.495192
0.0152734 0.999866 0.00585235 -0.136178
-0.00151276 -0.00582992 0.999982 0.0277156
0 0 0 1
0.999886 0.0150583 -0.00145695 0.500587
-0.0150667 0.999869 -0.00596744 0.124683
0.0013669 0.00598871 0.999981 -0.0288314
0 0 0 1
0.999882 -0.0152866 0.00142242 -0.495196
0.0152781 0.999866 0.00584714 -0.136171
-0.00151162 -0.00582472 0.999982 0.0277106
0 0 0 1
0.999886 0.0150566 -0.00145653 0.500585
-0.015065 0.999869 -0.00596729 0.124684
0.00136649 0.00598855 0.999981 -0.0288469
0 0 0 1
0.999882 -0.0152818 0.00142326 -0.495191
0.0152732 0.999866 0.00585296 -0.136189
-0.00151252 -0.00583054 0.999982 0.0277163
0 0 0 1
0.999886 0.0150565 -0.00145653 0.500585
-0.015065 0.999869 -0.00596727 0.124684
0.00136649 0.00598853 0.999981 -0.0288469
0 0 0 1
0.999882 -0.0152821 0.00142346 -0.495192
0.0152735 0.999866 0.00585235 -0.13618
-0.00151271 -0.00582991 0.999982 0.0277117
0 0 0 1
0.999886 0.0150583 -0.00145695 0.500587
-0.0150667 0.999869 -0.00596744 0.124683
0.0013669 0.00598871 0.999981 -0.0288313
0 0 0 1
0.999882 -0.0152818 0.00142327 -0.495191
0.0152732 0.999866 0.00585299 -0.13619
-0.00151253 -0.00583056 0.999982 0.0277169
0 0 0 1
100times_reuse:6893.68[msec] fitness_score:0.0905329

=========================================================
1 times_reuse:90.8963[msec] fitness_score:0.206262
--- vgicp_st ---
0.999886 0.0150565 -0.00145653 0.500585
-0.015065 0.999869 -0.00596727 0.124684
0.00136649 0.00598853 0.999981 -0.0288469
0 0 0 1
1 times_reuse:122.578[msec] fitness_score:0.207399
--- vgicp_mt ---
0.999886 0.0150566 -0.00145654 0.500586
-0.015065 0.999869 -0.00596722 0.124683
0.00136651 0.00598848 0.999981 -0.0288474
0 0 0 1
1 times_reuse:61.292[msec] fitness_score:0.207399

Problem about package dependencies when building from source

I have a problem building the package in my catkin workspace. It has a bunch of error information about the eigen3 library. So I would like to know which version code base of eigen3 are you using to compile the package? My current eigen3 library is the newest one built from source.

Here is a part of the build error information:
/home/jason/Workspace/catkin_ws/src/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:5:27: required from here
/home/jason/Workspace/catkin_ws/src/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:163:30: error: call to ‘double fast_gicp::FastGICP<PointSource, PointTarget>::compute_error(const Isometry3d&, Eigen::Matrix<double, 6, 6>, Eigen::Matrix<double, 6, 1>) const [with PointSource = pcl::PointXYZI; PointTarget = pcl::PointXYZI; Eigen::Isometry3d = Eigen::Transform<double, 3, 1>]’ uses the default argument for parameter 2, which is not yet defined
/home/jason/Workspace/catkin_ws/src/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:163:30: error: call to ‘double fast_gicp::FastGICP<PointSource, PointTarget>::compute_error(const Isometry3d&, Eigen::Matrix<double, 6, 6>, Eigen::Matrix<double, 6, 1>) const [with PointSource = pcl::PointXYZI; PointTarget = pcl::PointXYZI; Eigen::Isometry3d = Eigen::Transform<double, 3, 1>]’ uses the default argument for parameter 3, which is not yet defined
make[2]: *** [CMakeFiles/fast_gicp.dir/src/fast_gicp/gicp/fast_gicp_st.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
make[2]: *** [CMakeFiles/fast_gicp.dir/src/fast_gicp/gicp/fast_vgicp.cpp.o] Error 1
make[2]: *** [CMakeFiles/fast_gicp.dir/src/fast_gicp/gicp/fast_gicp.cpp.o] Error 1
make[1]: *** [CMakeFiles/fast_gicp.dir/all] Error 2
make: *** [all] Error 2

Thank you if you can provide it.

Question about the fitness change

Hi,
I am curious about the recent update, especially the Benchmark part.
As I recall, the old ones for fgicp_mt vgicp_mt and so on are around 0.09.
However, the new one is backward to 0.2.
Caused by optimization method change from the gauss newton to Levenberg-Marquardt? That is GausNewton is better?
Or the parameters change?
After the recent update, I get a similar result, like the new benchmark.

The new one vs. the old one

target:17249[pts] source:17518[pts]
--- pcl_gicp ---
single:127.508[msec] 100times:12549.4[msec] fitness_score:0.204892
single:116.732[msec] 100times:10867.1[msec] fitness_score:0.204306
--- pcl_ndt ---
single:53.5904[msec] 100times:5467.16[msec] fitness_score:0.229616
single:52.8007[msec] 100times:5220.49[msec] fitness_score:0.226416
--- fgicp_st ---
single:111.324[msec] 100times:10662.7[msec] 100times_reuse:6794.59[msec] fitness_score:0.204379
single:110.343[msec] 100times:10651.2[msec] 100times_reuse:6962.1[msec] fitness_score:0.0922969
--- fgicp_mt ---
single:20.1602[msec] 100times:1585[msec] 100times_reuse:1017.74[msec] fitness_score:0.204412
single:24.3643[msec] 100times:2716.7[msec] 100times_reuse:1799.1[msec] fitness_score:0.0922969
--- vgicp_st ---
single:112.001[msec] 100times:7959.9[msec] 100times_reuse:4408.22[msec] fitness_score:0.204067
single:115.041[msec] 100times:8759.43[msec] 100times_reuse:4784.57[msec] fitness_score:0.0912174
--- vgicp_mt ---
single:18.1106[msec] 100times:1381[msec] 100times_reuse:806.53[msec] fitness_score:0.204067
single:19.705[msec] 100times:1963.74[msec] 100times_reuse:1044.29[msec] fitness_score:0.0912174
--- vgicp_cuda (parallel_kdtree) ---
single:15.9587[msec] 100times:1451.85[msec] 100times_reuse:695.48[msec] fitness_score:0.204061
single:16.1846[msec] 100times:1611.89[msec] 100times_reuse:779.65[msec] fitness_score:0.0709287
--- vgicp_cuda (gpu_bruteforce) ---
single:53.9113[msec] 100times:3463.5[msec] 100times_reuse:1703.41[msec] fitness_score:0.204049
single:49.7294[msec] 100times:3145.78[msec] 100times_reuse:1541.36[msec] fitness_score:0.0710122

Add PCL style shared_ptr shortcuts for the implemented registration methods

Its just a minor thing to add and most probably not so many ppl. are using it, but to keep up the PCL implementation style all the specialized registration methods could provide the shared_ptr shortcut ::Ptr.

As an example we can take a look into the public part of pcl::gicp class:

      using Ptr = shared_ptr< GeneralizedIterativeClosestPoint<PointSource, PointTarget> >;
      using ConstPtr = shared_ptr< const GeneralizedIterativeClosestPoint<PointSource, PointTarget> >;

Paper link

Would you please provide the download link of your paper about this program ? Thank you.

LM getting stuck between two poses

I have had issues with some point data where LM optimization gets stuck jumping between two poses where the delta between them is sufficient enough that it does not terminate the search, and instead runs until num_iterations and terminates "without converging"

I added a check to cache every other pose then compare that to the current pose and if the delta is still consider the algorithm to be converged. I am unsure how/why the optimization is jumping between these two points, but its something that seems to happen with my data sets pretty regularly.

I can send you a PR of this hacky fix unless you have an idea what might be happening with the LM. Its also worth noting this only happens when I have a very small transformation_epsilon_ 1e-9. But this number is not atypical for other registration limits and the algorithm usually works with that limit in most cases.

Sophus version

Hi @koide3,
What's the dependency lib Sophus version and branch? Did you use master branch default, could you please give some advice
about this to build fast_icp on ubuntu16.04?
Thanks for your help!
Jiao

Muliple errors compiling on Ubuntu 20.04

System

Ubuntu 20.04
PCL 1.11
gcc/g++ version 7.5.0 (Ubuntu 7.5.0-6ubuntu2)
boost 1.71.0.0ubuntu2

Steps done

git clone https://github.com/SMRT-AIST/fast_gicp --recursive
git submodule update --init --recursive
mkdir fast_gicp/build
cd fast_gicp/build
cmake .. -DCMAKE_BUILD_TYPE=Release
make # no -j to keep amount of printed errors small

Errors

[  8%] Linking CXX shared library devel/lib/libfast_vgicp_cuda.so
[ 16%] Built target fast_vgicp_cuda
[ 25%] Building CXX object CMakeFiles/fast_gicp.dir/src/fast_gicp/gicp/fast_gicp.cpp.o
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:1:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:75:43: error: ‘shared_ptr’ in namespace ‘boost’ does not name a template type
   bool calculate_covariances(const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances);
                                           ^~~~~~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:75:53: error: expected ‘,’ or ‘...’ before ‘<’ token
   bool calculate_covariances(const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances);
                                                     ^
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:2:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp: In member function ‘Eigen::VectorXf fast_gicp::FastGICP<PointSource, PointTarget>::loss_ls(const Eigen::Matrix<float, 6, 1>&, Eigen::MatrixXf*) const’:
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:201:8: error: ‘atomic_int’ is not a member of ‘std’
   std::atomic_int count(0);
        ^~~~~~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:201:8: note: suggested alternative: ‘atomic_load’
   std::atomic_int count(0);
        ^~~~~~~~~~
        atomic_load
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:232:13: error: ‘count’ was not declared in this scope
     int n = count++;
             ^~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:232:13: note: suggested alternative:
In file included from /usr/include/c++/7/algorithm:62:0,
                 from /usr/include/eigen3/Eigen/Core:288,
                 from software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:4,
                 from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:1:
/usr/include/c++/7/bits/stl_algo.h:4076:5: note:   ‘std::count’
     count(_InputIterator __first, _InputIterator __last, const _Tp& __value)
     ^~~~~
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:2:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:238:20: error: ‘count’ was not declared in this scope
   int final_size = count;
                    ^~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:238:20: note: suggested alternative:
In file included from /usr/include/c++/7/algorithm:62:0,
                 from /usr/include/eigen3/Eigen/Core:288,
                 from software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:4,
                 from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:1:
/usr/include/c++/7/bits/stl_algo.h:4076:5: note:   ‘std::count’
     count(_InputIterator __first, _InputIterator __last, const _Tp& __value)
     ^~~~~
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:2:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp: At global scope:
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:245:77: error: ‘shared_ptr’ in namespace ‘boost’ does not name a template type
 tGICP<PointSource, PointTarget>::calculate_covariances(const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances) {
                                                                     ^~~~~~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:245:87: error: expected ‘,’ or ‘...’ before ‘<’ token
 tSource, PointTarget>::calculate_covariances(const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances) {
                                                                     ^
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp: In member function ‘bool fast_gicp::FastGICP<PointSource, PointTarget>::calculate_covariances(int)’:
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:246:3: error: ‘kdtree’ was not declared in this scope
   kdtree.setInputCloud(cloud);
   ^~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:246:3: note: suggested alternative: ‘free’
   kdtree.setInputCloud(cloud);
   ^~~~~~
   free
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:246:24: error: ‘cloud’ was not declared in this scope
   kdtree.setInputCloud(cloud);
                        ^~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:246:24: note: suggested alternative: ‘close’
   kdtree.setInputCloud(cloud);
                        ^~~~~
                        close
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:247:3: error: ‘covariances’ was not declared in this scope
   covariances.resize(cloud->size());
   ^~~~~~~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:250:3: error: invalid controlling predicate
   for(int i = 0; i < cloud->size(); i++) {
   ^~~
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:2:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp: In instantiation of ‘void fast_gicp::FastGICP<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr&) [with PointSource = pcl::PointXYZ; PointTarget = pcl::PointXYZ; fast_gicp::FastGICP<PointSource, PointTarget>::PointCloudSourceConstPtr = std::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >]’:
software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:4:27:   required from here
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:95:24: error: no matching function for call to ‘fast_gicp::FastGICP<pcl::PointXYZ, pcl::PointXYZ>::calculate_covariances(const PointCloudSourceConstPtr&, pcl::search::KdTree<pcl::PointXYZ, pcl::KdTreeFLANN<pcl::PointXYZ, flann::L2_Simple<float> > >&, std::vector<Eigen::Matrix<float, 4, 4>, Eigen::aligned_allocator<Eigen::Matrix<float, 4, 4> > >&)’
   calculate_covariances(cloud, *source_kdtree, source_covs);
   ~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:1:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:75:8: note: candidate: template<class PointT> bool fast_gicp::FastGICP<PointSource, PointTarget>::calculate_covariances(int) [with PointT = PointT; PointSource = pcl::PointXYZ; PointTarget = pcl::PointXYZ]
   bool calculate_covariances(const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances);
        ^~~~~~~~~~~~~~~~~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:75:8: note:   template argument deduction/substitution failed:
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:2:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:95:24: note:   candidate expects 1 argument, 3 provided
   calculate_covariances(cloud, *source_kdtree, source_covs);
   ~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp: In instantiation of ‘void fast_gicp::FastGICP<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr&) [with PointSource = pcl::PointXYZ; PointTarget = pcl::PointXYZ; fast_gicp::FastGICP<PointSource, PointTarget>::PointCloudTargetConstPtr = std::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >]’:
software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:4:27:   required from here
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:104:24: error: no matching function for call to ‘fast_gicp::FastGICP<pcl::PointXYZ, pcl::PointXYZ>::calculate_covariances(const PointCloudTargetConstPtr&, pcl::search::KdTree<pcl::PointXYZ, pcl::KdTreeFLANN<pcl::PointXYZ, flann::L2_Simple<float> > >&, std::vector<Eigen::Matrix<float, 4, 4>, Eigen::aligned_allocator<Eigen::Matrix<float, 4, 4> > >&)’
   calculate_covariances(cloud, *target_kdtree, target_covs);
   ~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:1:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:75:8: note: candidate: template<class PointT> bool fast_gicp::FastGICP<PointSource, PointTarget>::calculate_covariances(int) [with PointT = PointT; PointSource = pcl::PointXYZ; PointTarget = pcl::PointXYZ]
   bool calculate_covariances(const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances);
        ^~~~~~~~~~~~~~~~~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:75:8: note:   template argument deduction/substitution failed:
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:2:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:104:24: note:   candidate expects 1 argument, 3 provided
   calculate_covariances(cloud, *target_kdtree, target_covs);
   ~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp: In instantiation of ‘void fast_gicp::FastGICP<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr&) [with PointSource = pcl::PointXYZI; PointTarget = pcl::PointXYZI; fast_gicp::FastGICP<PointSource, PointTarget>::PointCloudSourceConstPtr = std::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >]’:
software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:5:27:   required from here
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:95:24: error: no matching function for call to ‘fast_gicp::FastGICP<pcl::PointXYZI, pcl::PointXYZI>::calculate_covariances(const PointCloudSourceConstPtr&, pcl::search::KdTree<pcl::PointXYZI, pcl::KdTreeFLANN<pcl::PointXYZI, flann::L2_Simple<float> > >&, std::vector<Eigen::Matrix<float, 4, 4>, Eigen::aligned_allocator<Eigen::Matrix<float, 4, 4> > >&)’
   calculate_covariances(cloud, *source_kdtree, source_covs);
   ~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:1:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:75:8: note: candidate: template<class PointT> bool fast_gicp::FastGICP<PointSource, PointTarget>::calculate_covariances(int) [with PointT = PointT; PointSource = pcl::PointXYZI; PointTarget = pcl::PointXYZI]
   bool calculate_covariances(const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances);
        ^~~~~~~~~~~~~~~~~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:75:8: note:   template argument deduction/substitution failed:
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:2:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:95:24: note:   candidate expects 1 argument, 3 provided
   calculate_covariances(cloud, *source_kdtree, source_covs);
   ~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp: In instantiation of ‘void fast_gicp::FastGICP<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr&) [with PointSource = pcl::PointXYZI; PointTarget = pcl::PointXYZI; fast_gicp::FastGICP<PointSource, PointTarget>::PointCloudTargetConstPtr = std::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >]’:
software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:5:27:   required from here
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:104:24: error: no matching function for call to ‘fast_gicp::FastGICP<pcl::PointXYZI, pcl::PointXYZI>::calculate_covariances(const PointCloudTargetConstPtr&, pcl::search::KdTree<pcl::PointXYZI, pcl::KdTreeFLANN<pcl::PointXYZI, flann::L2_Simple<float> > >&, std::vector<Eigen::Matrix<float, 4, 4>, Eigen::aligned_allocator<Eigen::Matrix<float, 4, 4> > >&)’
   calculate_covariances(cloud, *target_kdtree, target_covs);
   ~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:1:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:75:8: note: candidate: template<class PointT> bool fast_gicp::FastGICP<PointSource, PointTarget>::calculate_covariances(int) [with PointT = PointT; PointSource = pcl::PointXYZI; PointTarget = pcl::PointXYZI]
   bool calculate_covariances(const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances);
        ^~~~~~~~~~~~~~~~~~~~~
software/Software-local/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp:75:8: note:   template argument deduction/substitution failed:
In file included from software/Software-local/fast_gicp/src/fast_gicp/gicp/fast_gicp.cpp:2:0:
software/Software-local/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp:104:24: note:   candidate expects 1 argument, 3 provided
   calculate_covariances(cloud, *target_kdtree, target_covs);
   ~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Question about buckets in GaussianVoxelMap

Hello! Can someone give an intuition about the purpose of buckets in GaussianVoxelMap, please? What is the difference between a voxel and a bucket? I want to implement store-and-load feature

Question about buckets in GaussianVoxelMap

Hello! Can someone give an intuition about the purpose of buckets in GaussianVoxelMap, please? What is the difference between a voxel and a bucket? I want to implement store-and-load feature

Make Failed with Ubantu16.04, CUDA9.2

I am trying to use the CUDA version of the algorithm, but I encountered the following two problems:

• error: incomplete type is not allowed,

• error: no suitable user-defined conversion from "fast_gicp::FastVGICPCudaCore::Points" to "const thrust::device_vector<Eigen::Vector3f, thrust::device_malloc_allocatorEigen::Vector3f>" exists

The first error occurs when using if(source_points && target_points) or if(source_neighbors && target_neighbors)

The second error occurs when using the function such as voxelmap->create_voxelmap(*target_points, *target_covariances)

I am wondering these problems are caused by the version of CUDA. Thanks for your answer.

The complete log file is make_log.txt

Give me some advice

CMakeFiles/gicp_kitti.dir/src/kitti.cpp.o：在函数‘main’中：
kitti.cpp:(.text.startup+0x3fb)：对‘pcl::visualization::PCLVisualizer::PCLVisualizer(std::string const&, bool)’未定义的引用
collect2: error: ld returned 1 exit status
CMakeFiles/gicp_kitti.dir/build.make:429: recipe for target 'devel/lib/fast_gicp/gicp_kitti' failed
make[2]: *** [devel/lib/fast_gicp/gicp_kitti] Error 1
CMakeFiles/Makefile2:173: recipe for target 'CMakeFiles/gicp_kitti.dir/all' failed
make[1]: *** [CMakeFiles/gicp_kitti.dir/all] Error 2
make[1]: *** 正在等待未完成的任务....
[100%] Linking CXX executable devel/lib/fast_gicp/gicp_align
[100%] Built target gicp_align
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2

How to deal with these errors? I beg that you can help me.

Error occurs when I install pygicp using Python3... (ImportError)

Your C++ code compiles without any error! All ok!
Then when i install pygicp, no error again.

creating build/bdist.linux-x86_64/egg
copying build/lib.linux-x86_64-3.8/libfast_gicp.so -> build/bdist.linux-x86_64/egg
copying build/lib.linux-x86_64-3.8/pygicp.cpython-38-x86_64-linux-gnu.so -> build/bdist.linux-x86_64/egg
creating stub loader for pygicp.cpython-38-x86_64-linux-gnu.so
byte-compiling build/bdist.linux-x86_64/egg/pygicp.py to pygicp.cpython-38.pyc
creating build/bdist.linux-x86_64/egg/EGG-INFO
copying pygicp.egg-info/PKG-INFO -> build/bdist.linux-x86_64/egg/EGG-INFO
copying pygicp.egg-info/SOURCES.txt -> build/bdist.linux-x86_64/egg/EGG-INFO
copying pygicp.egg-info/dependency_links.txt -> build/bdist.linux-x86_64/egg/EGG-INFO
copying pygicp.egg-info/not-zip-safe -> build/bdist.linux-x86_64/egg/EGG-INFO
copying pygicp.egg-info/top_level.txt -> build/bdist.linux-x86_64/egg/EGG-INFO
writing build/bdist.linux-x86_64/egg/EGG-INFO/native_libs.txt
creating 'dist/pygicp-0.0.1-py3.8-linux-x86_64.egg' and adding 'build/bdist.linux-x86_64/egg' to it
removing 'build/bdist.linux-x86_64/egg' (and everything under it)
Processing pygicp-0.0.1-py3.8-linux-x86_64.egg
creating /home/hyx/miniconda3/envs/slam/lib/python3.8/site-packages/pygicp-0.0.1-py3.8-linux-x86_64.egg
Extracting pygicp-0.0.1-py3.8-linux-x86_64.egg to /home/hyx/miniconda3/envs/slam/lib/python3.8/site-packages
Adding pygicp 0.0.1 to easy-install.pth file

Installed /home/hyx/miniconda3/envs/slam/lib/python3.8/site-packages/pygicp-0.0.1-py3.8-linux-x86_64.egg
Processing dependencies for pygicp==0.0.1
Finished processing dependencies for pygicp==0.0.1

But when i import pygicp in Python3, error occurs.

import pygicp
Traceback (most recent call last):
File "", line 1, in
ImportError: dynamic module does not define module export function (PyInit_pygicp)

But when i install it using Py2, everything is ok.
So,
How can I fix this Python3 import error?
Thanks a lot!

how to see the output?

Hi @koide3
We were able to build & Run the tool on existent pcd files
However, the execution won't create any output files, will it?

 rosrun fast_gicp gicp_align points-2.pcd  points-3.pcd
target:79[pts] source:79[pts]
--- pcl_gicp ---
single:2.38397[msec] 100times:219.636[msec] fitness_score:0.019277
--- pcl_ndt ---
single:10.1059[msec] 100times:991.808[msec] fitness_score:0.0203081
--- fgicp_st ---
single:9.13514[msec] 100times:886.917[msec] 100times_reuse:713.189[msec] fitness_score:0.0318018
--- fgicp_mt ---
single:17.7897[msec] 100times:285.958[msec] 100times_reuse:228.059[msec] fitness_score:0.0674881
--- vgicp_st ---
single:1.37252[msec] 100times:108.937[msec] 100times_reuse:92.5806[msec] fitness_score:0.0752508
--- vgicp_mt ---
single:4.60711[msec] 100times:62.8587[msec] 100times_reuse:81.8901[msec] fitness_score:0.0710234
--- vgicp_cuda (parallel_kdtree) ---
single:4.59491[msec] 100times:378.733[msec] 100times_reuse:652.478[msec] fitness_score:0.0831286
--- vgicp_cuda (gpu_bruteforce) ---
single:4.03216[msec] 100times:373.5[msec] 100times_reuse:573.646[msec] fitness_score:0.083081

R is not orthogonal

another bug,
matching.Sophus ensure failed in function 'Sophus::SO3<Scalar_, Options>::SO3(const Transformation&) [with Scalar_ = float; int Options = 0; Sophus::SO3<Scalar_, Options>::Transformation = Eigen::Matrix<float, 3, 3>]', file '/..../src/fast_gicp/thirdparty/Sophus/sophus/so3.hpp', line 473. R is not orthogonal: 1.00001 -1.83471e-07 -1.13947e-06 -1.83471e-07 1.00001 4.81601e-07 -1.13947e-06 4.81601e-07

Originally posted by @liuyf5231 in https://github.com/SMRT-AIST/fast_gicp/issue_comments/620425420

I am using FastVGICP and I faced with the same problem. Could you help me? matching. Sophus ensure failed in function 'Sophus::SO3<Scalar_, Options>::SO3(const Transformation&) [with Scalar_ = float; int Options = 0; Sophus::SO3<Scalar_, Options>::Transformation = Eigen::Matrix<float, 3, 3>]', file '/..../src/fast_gicp/thirdparty/Sophus/sophus/so3.hpp', line 473. I got this message and the system is terminated.

Dependencies/Installation/Build Instructions?

Hi,

This looks like a neat repo that I'd like to try out. What is the target deployment environment you are expecting? Do you have any dependency/build/installation instructions you'd like to share?

I'm on Ubuntu 18.04 / Cuda 10.1.

Non-ROS library

Hi,

I would like to use your code and would happily cite your work if I could use it. The only thing is that I am not familiar with ROS packages. Would it be possible for you to make a branch that creates a normal library without needing to work with ROS?

Thanks

Cannot find sophus/so3.hpp

Hello, this problem occurred when I was compiling the feature pack.
In file included from /home/bbm/catkin_ws/src/fast_gicp/src/fast_gicp/gicp/fast_vgicp.cpp:1:0:
/home/bbm/catkin_ws/src/fast_gicp/include/fast_gicp/gicp/fast_vgicp.hpp:14:26: fatal error: sophus/so3.hpp: No such file or directory
I checked it under the path in the function package. The so3.hpp file is under /home/bbm/catkin_ws/src/fast_gicp/include/fast_gicp/so3.
Why can’t I compile and find the header file?

How to install pygicp in python

When I follow the instruction to install pygicp, No errors occur in the installation. But I cannot import pygicp in python.
$ python setup.py install
running install
running bdist_egg
running egg_info
writing pygicp.egg-info/PKG-INFO
writing dependency_links to pygicp.egg-info/dependency_links.txt
writing top-level names to pygicp.egg-info/top_level.txt
reading manifest file 'pygicp.egg-info/SOURCES.txt'
writing manifest file 'pygicp.egg-info/SOURCES.txt'
installing library code to build/bdist.linux-x86_64/egg
running install_lib
running build_ext
-- Using CATKIN_DEVEL_PREFIX: /home/hyx/Code/fast_gicp/build/temp.linux-x86_64-3.7/devel
-- Using CMAKE_PREFIX_PATH: /opt/ros/melodic
-- This workspace overlays: /opt/ros/melodic
-- Found PythonInterp: /home/hyx/miniconda3/envs/rospy3/bin/python (found suitable version "3.7.10", minimum required is "3.7.10")
-- Using PYTHON_EXECUTABLE: /home/hyx/miniconda3/envs/rospy3/bin/python
-- Using Debian Python package layout
-- Using empy: /usr/bin/empy
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/hyx/Code/fast_gicp/build/temp.linux-x86_64-3.7/test_results
-- Forcing gtest/gmock from source, though one was otherwise available.
-- Found gtest sources under '/usr/src/googletest': gtests will be built
-- Found gmock sources under '/usr/src/googletest': gmock will be built
-- Found PythonInterp: /home/hyx/miniconda3/envs/rospy3/bin/python (found version "3.7.10")
-- Using Python nosetests: /usr/bin/nosetests
-- catkin 0.7.29
-- BUILD_SHARED_LIBS is on
-- Could NOT find ensenso (missing: ENSENSO_LIBRARY ENSENSO_INCLUDE_DIR)
** WARNING ** io features related to ensenso will be disabled
-- Could NOT find DAVIDSDK (missing: DAVIDSDK_LIBRARY DAVIDSDK_INCLUDE_DIR)
** WARNING ** io features related to davidSDK will be disabled
-- Could NOT find DSSDK (missing: _DSSDK_LIBRARIES)
** WARNING ** io features related to dssdk will be disabled
** WARNING ** io features related to pcap will be disabled
** WARNING ** io features related to png will be disabled
-- The imported target "vtkRenderingPythonTkWidgets" references the file
"/usr/lib/x86_64-linux-gnu/libvtkRenderingPythonTkWidgets.so"
but this file does not exist. Possible reasons include:

• The file was deleted, renamed, or moved to another location.
• An install or uninstall procedure did not complete successfully.
• The installation package was faulty and contained
  "/usr/lib/cmake/vtk-6.3/VTKTargets.cmake"
  but not all the files it references.

-- The imported target "vtk" references the file
"/usr/bin/vtk"
but this file does not exist. Possible reasons include:

• The file was deleted, renamed, or moved to another location.
• An install or uninstall procedure did not complete successfully.
• The installation package was faulty and contained
  "/usr/lib/cmake/vtk-6.3/VTKTargets.cmake"
  but not all the files it references.

** WARNING ** io features related to libusb-1.0 will be disabled
-- Could NOT find ensenso (missing: ENSENSO_LIBRARY ENSENSO_INCLUDE_DIR)
** WARNING ** visualization features related to ensenso will be disabled
-- Could NOT find DAVIDSDK (missing: DAVIDSDK_LIBRARY DAVIDSDK_INCLUDE_DIR)
** WARNING ** visualization features related to davidSDK will be disabled
-- Could NOT find DSSDK (missing: _DSSDK_LIBRARIES)
** WARNING ** visualization features related to dssdk will be disabled
-- Could NOT find RSSDK (missing: _RSSDK_LIBRARIES)
** WARNING ** visualization features related to rssdk will be disabled
-- looking for PCL_COMMON
-- looking for PCL_OCTREE
-- looking for PCL_IO
-- looking for PCL_KDTREE
-- looking for PCL_SEARCH
-- looking for PCL_SAMPLE_CONSENSUS
-- looking for PCL_FILTERS
-- looking for PCL_2D
-- looking for PCL_GEOMETRY
-- looking for PCL_FEATURES
-- looking for PCL_ML
-- looking for PCL_SEGMENTATION
-- looking for PCL_VISUALIZATION
-- looking for PCL_SURFACE
-- looking for PCL_REGISTRATION
-- looking for PCL_KEYPOINTS
-- looking for PCL_TRACKING
-- looking for PCL_RECOGNITION
-- looking for PCL_STEREO
-- looking for PCL_APPS
-- looking for PCL_IN_HAND_SCANNER
-- looking for PCL_MODELER
-- looking for PCL_POINT_CLOUD_EDITOR
-- looking for PCL_OUTOFCORE
-- looking for PCL_PEOPLE
-- Found OpenMP_C: -fopenmp
-- Found OpenMP_CXX: -fopenmp
-- Found OpenMP: TRUE
-- pybind11 v2.6.3 dev1
-- Found PythonInterp: /home/hyx/miniconda3/envs/rospy3/bin/python (found suitable version "3.7.10", minimum required is "2")
-- Configuring done
-- Generating done
-- Build files have been written to: /home/hyx/Code/fast_gicp/build/temp.linux-x86_64-3.7
[ 4%] Linking CXX shared library devel/lib/libfast_vgicp_cuda.so
[ 48%] Built target fast_vgicp_cuda
[ 52%] Linking CXX shared library devel/lib/libfast_gicp.so
[ 76%] Built target fast_gicp
[ 80%] Linking CXX shared module devel/lib/pygicp.cpython-37m-x86_64-linux-gnu.so
[ 84%] Built target pygicp
[ 88%] Linking CXX executable devel/lib/fast_gicp/gicp_align
[ 92%] Built target gicp_align
[ 96%] Linking CXX executable devel/lib/fast_gicp/gicp_kitti
[100%] Built target gicp_kitti
creating build/bdist.linux-x86_64/egg
creating build/bdist.linux-x86_64/egg/EGG-INFO
copying pygicp.egg-info/PKG-INFO -> build/bdist.linux-x86_64/egg/EGG-INFO
copying pygicp.egg-info/SOURCES.txt -> build/bdist.linux-x86_64/egg/EGG-INFO
copying pygicp.egg-info/dependency_links.txt -> build/bdist.linux-x86_64/egg/EGG-INFO
copying pygicp.egg-info/not-zip-safe -> build/bdist.linux-x86_64/egg/EGG-INFO
copying pygicp.egg-info/top_level.txt -> build/bdist.linux-x86_64/egg/EGG-INFO
creating 'dist/pygicp-0.0.1-py3.7-linux-x86_64.egg' and adding 'build/bdist.linux-x86_64/egg' to it
removing 'build/bdist.linux-x86_64/egg' (and everything under it)
Processing pygicp-0.0.1-py3.7-linux-x86_64.egg
removing '/home/hyx/miniconda3/envs/rospy3/lib/python3.7/site-packages/pygicp-0.0.1-py3.7-linux-x86_64.egg' (and everything under it)
creating /home/hyx/miniconda3/envs/rospy3/lib/python3.7/site-packages/pygicp-0.0.1-py3.7-linux-x86_64.egg
Extracting pygicp-0.0.1-py3.7-linux-x86_64.egg to /home/hyx/miniconda3/envs/rospy3/lib/python3.7/site-packages
pygicp 0.0.1 is already the active version in easy-install.pth

Installed /home/hyx/miniconda3/envs/rospy3/lib/python3.7/site-packages/pygicp-0.0.1-py3.7-linux-x86_64.egg
Processing dependencies for pygicp==0.0.1
Finished processing dependencies for pygicp==0.0.1

Then I find pygicp in pip-list, but still cannot import this package.
And there is not any python files in the egg file.
Please help and tell me how to fix it.
Thanks.

Inconsistent and wrong registration results in an indoor environment

Hi, I'm using your algorithm to align 2 lidars which have a 110 degree FOV each. There is an overlap region between the two lidars of around 20 degrees and I'm trying to place an object in this overlapping region to attempt to align the two point clouds.

I've placed a chessboard in the overlapping region and filtered the pointcloud to only use the overlapping region, however the transformation matrix is quite inconsistent and so the aligned pointcloud jumps around a lot.

Here are some examples of the transformation matrix.

  0.999199 -0.0311153  0.0251736  0.0150917
  0.031543   0.999362  -0.016778 0.00462453
-0.0246354  0.0175586   0.999542 -0.0450208
         0          0          0          1

   0.999689  -0.0243293 -0.00547935  -0.0202134
  0.0245881    0.998261   0.0535683   0.0226119
 0.00416655  -0.0536864    0.998549 -0.00405232
          0           0           0           1

My code snippet is below. I've also tried using FROBENIUS and DIRECT7 for regularization and neighbour search method, but it made no difference.

// downsampling
  pcl::PointCloud< pcl::PointXYZ >::Ptr dsource_pcl(new pcl::PointCloud< pcl::PointXYZ >());
  pcl::PointCloud< pcl::PointXYZ >::Ptr dtarget_pcl(new pcl::PointCloud< pcl::PointXYZ >());
  pcl::ApproximateVoxelGrid<pcl::PointXYZ> voxelgrid;
  voxelgrid.setLeafSize(0.1, 0.1, 0.1);

  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>());
  voxelgrid.setInputCloud(ftarget_pcl);
  voxelgrid.filter(*filtered);
  dtarget_pcl = filtered;

  filtered.reset(new pcl::PointCloud<pcl::PointXYZ>());
  voxelgrid.setInputCloud(fsource_pcl);
  voxelgrid.filter(*filtered);
  dsource_pcl = filtered;

  pcl::PointCloud< pcl::PointXYZ >::Ptr input_pcl(new pcl::PointCloud< pcl::PointXYZ >());
  pcl::fromROSMsg(*source_msg, *input_pcl);
  pcl::PointCloud< pcl::PointXYZ >::Ptr output_pcl(new pcl::PointCloud< pcl::PointXYZ >());

// aligning point cloud
  fast_gicp::FastVGICP<pcl::PointXYZ, pcl::PointXYZ> reg;
  reg.setResolution(0.5);
  reg.setMaxCorrespondenceDistance(1.0);
  reg.clearTarget();
  reg.clearSource();
  reg.setInputTarget(dtarget_pcl);
  reg.setInputSource(dsource_pcl);
  reg.align(*aligned_pcl);
  pcl::transformPointCloud(*fsource_pcl, *output_pcl, reg.getFinalTransformation());

// publish pcl 
  sensor_msgs::PointCloud2 pcl_1_msg, pcl_2_msg;
  pcl::toROSMsg(*output_pcl, pcl_1_msg);
  pcl::toROSMsg(*ftarget_pcl, pcl_2_msg);
  alignedpcl_1_pub_.publish(pcl_1_msg);
  alignedpcl_2_pub_.publish(pcl_2_msg);

Could I get some advice on this?

conflict with open3d

Ubuntu 20.04, Python 3.8.10, Open3D 0.13.0 (installed via pip), fast_gicp master (installed via setup.py)

If pygicp is imported after open3d, a segfault occurs. Otherwise, everything works as expected.

sample code

import numpy as np
import open3d as o3d
import pygicp

target = np.random.random((10,3))
source = np.random.random((10,3))
pygicp.align_points(target, source)

How to Change Neighbor Search Method in VGICP-Cuda

The variable search_method_ is set to NeighborSearchMethod::DIRECT1 in fast_vgicp_impl.hpp,it means each point only correspondences to one voxel. I haved tried to change the variable to DIRECT7, which contributes to better results with slower speed.
However,in fast_vgicp_cuda_impl.hpp,I do find the function setNeighborSearchMethod to set neighbor search method is never applied, and the variable offsets in fast_vgicp_cuda.cuh is initialized to Eigen::Vector3i::Zero().
I tried to apply setNeighborSearchMethod to change the search method, but the program crashed like this:
Segmentation fault (core dumped)
How can I change it?

The static assertion failed mistake!

Hi, when I compile the project, the static assertion mistake happened!

The gcc version in my computer is 8.3.0.
Could you please help me solve this problem?
Thanks!

residual format

Hi doctor @koide3,
I want to figure out the jacobians of residual in code.

• I see the residual is RCR_inv * d 209 line of fast_gicp_st_impl.hpp, which is 3 dimensional, not d.transpose() * RCR_inv * d, which is a scalar matching with the paper's cost function, why the residual is this?
• How to calculate the inverse matrix in $r$? Does its inverse exist?
  

Thanks for your help!
Jiao

Compiling project

While trying to compile this project there are lots of errors with dependencies and includes under Ubuntu 20.04.
I fixed a lot of them but still I am not able to compile. If I get a running version I could provide a pull request.
Currently the linker fails with:

[ 83%] Linking CXX executable devel/lib/fast_gicp/gicp_align
/usr/bin/ld: /lib/x86_64-linux-gnu/libpng16.so.16: undefined reference to `inflateReset2@ZLIB_1.2.3.4'
/usr/bin/ld: /lib/x86_64-linux-gnu/libpng16.so.16: undefined reference to `inflateValidate@ZLIB_1.2.9'
collect2: error: ld returned 1 exit status
make[2]: *** [CMakeFiles/gicp_align.dir/build.make:215: devel/lib/fast_gicp/gicp_align] Fehler 1
make[1]: *** [CMakeFiles/Makefile2:357: CMakeFiles/gicp_align.dir/all] Fehler 2
make: *** [Makefile:160: all] Fehler 2

zlib is installed by sudo apt install zlib1g-dev as well as libpng16 but I can not find a way to link it with CMake. Any ideas whats going wrong?

Bug in FastVGICPCudaCore::calculate_target_covariances

Hi, I noticed in the file fast_vgicp_cuda.cu, in the function:

void FastVGICPCudaCore::calculate_target_covariances(RegularizationMethod method) {
assert(source_points && source_neighbors);

the assertion is wrong. It should assert the target points and neighbors not the source. I think it is just a copy and paste error. However it makes it impossible to set the target without setting the source first.

compare with the ndt_omp

thanks for your excellent work,
i wonder the comparison of fast-gicp and ndt-omp, speed and accuracy.
will it replace the ndt method?

Convergence comparison between NDT_OMP, GICP and VGICP

Hi, @koide3 ,
Thanks for sharing your great work to community, I have used these methods in LiDAR odometry and LiDAR loop closure detection modules. These methods are pretty well in LiDAR odometry especially for Fast-VGICP, but when I used these methods in loop closure detection to calculate a initial transform form current LiDAR frame to candidate LiDAR map, GICP and VGICP output wrong results, and even not converged sometimes while NDT-omp can work normally. Have you compared the convergences between these methods, and do you have any ideas to improve convergences?

• Init source(red) and target(green) pointcloud
  
• gicp alignment results
  
• NDT alignment results
  
• FAST-VGICP is not converged.

Error occurs [Segmentation fault (core dumped)] when I run gicp_align

I use Ubuntu18.04, ROS melodic and PCL 1.8(build from source).
I use catkin_make to build this project following the README.md https://github.com/SMRT-AIST/fast_gicp#ros
cd ~/catkin_ws/src git clone https://github.com/SMRT-AIST/fast_gicp --recursive cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release
and I build it successfully.(Although there is no error, there are some cmake warnings, I don't know if it matters and how can I fix it). The output message is as follow:

`Base path: /home/dell/lyq/study/fast_gicp
Source space: /home/dell/lyq/study/fast_gicp/src
Build space: /home/dell/lyq/study/fast_gicp/build
Devel space: /home/dell/lyq/study/fast_gicp/devel
Install space: /home/dell/lyq/study/fast_gicp/install

Running command: "make cmake_check_build_system" in "/home/dell/lyq/study/fast_gicp/build"

-- Using CATKIN_DEVEL_PREFIX: /home/dell/lyq/study/fast_gicp/devel
-- Using CMAKE_PREFIX_PATH: /opt/ros/melodic
-- This workspace overlays: /opt/ros/melodic
-- Found PythonInterp: /usr/bin/python2 (found suitable version "2.7.17", minimum required is "2")
-- Using PYTHON_EXECUTABLE: /usr/bin/python2
-- Using Debian Python package layout
-- Using empy: /usr/bin/empy
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/dell/lyq/study/fast_gicp/build/test_results
-- Found gtest sources under '/usr/src/googletest': gtests will be built
-- Found gmock sources under '/usr/src/googletest': gmock will be built
-- Found PythonInterp: /usr/bin/python2 (found version "2.7.17")
-- Using Python nosetests: /usr/bin/nosetests-2.7
-- catkin 0.7.29
-- BUILD_SHARED_LIBS is on
-- BUILD_SHARED_LIBS is on
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- ~~ traversing 1 packages in topological order:
-- ~~ - fast_gicp
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- +++ processing catkin package: 'fast_gicp'
-- ==> add_subdirectory(fast_gicp)
-- Boost version: 1.65.1
-- Found the following Boost libraries:
-- system
-- filesystem
-- thread
-- date_time
-- iostreams
-- serialization
-- chrono
-- atomic
-- regex
-- Could NOT find ensenso (missing: ENSENSO_LIBRARY ENSENSO_INCLUDE_DIR)
** WARNING ** io features related to ensenso will be disabled
-- Could NOT find DAVIDSDK (missing: DAVIDSDK_LIBRARY DAVIDSDK_INCLUDE_DIR)
** WARNING ** io features related to davidSDK will be disabled
-- Could NOT find DSSDK (missing: _DSSDK_LIBRARIES)
** WARNING ** io features related to dssdk will be disabled
** WARNING ** io features related to pcap will be disabled
** WARNING ** io features related to png will be disabled
-- The imported target "vtkRenderingPythonTkWidgets" references the file
"/usr/lib/x86_64-linux-gnu/libvtkRenderingPythonTkWidgets.so"
but this file does not exist. Possible reasons include:

• The file was deleted, renamed, or moved to another location.
• An install or uninstall procedure did not complete successfully.
• The installation package was faulty and contained
  "/usr/lib/cmake/vtk-6.3/VTKTargets.cmake"
  but not all the files it references.

-- The imported target "vtk" references the file
"/usr/bin/vtk"
but this file does not exist. Possible reasons include:

• The file was deleted, renamed, or moved to another location.
• An install or uninstall procedure did not complete successfully.
• The installation package was faulty and contained
  "/usr/lib/cmake/vtk-6.3/VTKTargets.cmake"
  but not all the files it references.

** WARNING ** io features related to libusb-1.0 will be disabled
-- Could NOT find ensenso (missing: ENSENSO_LIBRARY ENSENSO_INCLUDE_DIR)
** WARNING ** visualization features related to ensenso will be disabled
-- Could NOT find DAVIDSDK (missing: DAVIDSDK_LIBRARY DAVIDSDK_INCLUDE_DIR)
** WARNING ** visualization features related to davidSDK will be disabled
-- Could NOT find DSSDK (missing: _DSSDK_LIBRARIES)
** WARNING ** visualization features related to dssdk will be disabled
-- Could NOT find RSSDK (missing: _RSSDK_LIBRARIES)
** WARNING ** visualization features related to rssdk will be disabled
-- looking for PCL_COMMON
-- looking for PCL_OCTREE
-- looking for PCL_IO
-- looking for PCL_KDTREE
-- looking for PCL_SEARCH
-- looking for PCL_SAMPLE_CONSENSUS
-- looking for PCL_FILTERS
-- looking for PCL_2D
-- looking for PCL_GEOMETRY
-- looking for PCL_FEATURES
-- looking for PCL_ML
-- looking for PCL_SEGMENTATION
-- looking for PCL_VISUALIZATION
-- looking for PCL_SURFACE
-- looking for PCL_REGISTRATION
-- looking for PCL_KEYPOINTS
-- looking for PCL_TRACKING
-- looking for PCL_RECOGNITION
-- looking for PCL_STEREO
-- looking for PCL_OUTOFCORE
-- looking for PCL_PEOPLE
-- Configuring done
CMake Warning at fast_gicp/CMakeLists.txt:51 (add_library):
Cannot generate a safe runtime search path for target fast_gicp because
files in some directories may conflict with libraries in implicit
directories:

runtime library [libz.so.1] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
  /usr/local/lib

Some of these libraries may not be found correctly.

CMake Warning at fast_gicp/CMakeLists.txt:68 (add_executable):
Cannot generate a safe runtime search path for target gicp_align because
files in some directories may conflict with libraries in implicit
directories:

runtime library [libz.so.1] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
  /usr/local/lib

Some of these libraries may not be found correctly.

CMake Warning at fast_gicp/CMakeLists.txt:75 (add_executable):
Cannot generate a safe runtime search path for target gicp_kitti because
files in some directories may conflict with libraries in implicit
directories:

runtime library [libz.so.1] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
  /usr/local/lib

Some of these libraries may not be found correctly.

-- Generating done
-- Build files have been written to: /home/dell/lyq/study/fast_gicp/build

Running command: "make -j16 -l16" in "/home/dell/lyq/study/fast_gicp/build"

[ 55%] Built target fast_gicp
Scanning dependencies of target gicp_align
[ 77%] Built target gicp_kitti
[ 88%] Building CXX object fast_gicp/CMakeFiles/gicp_align.dir/src/align.cpp.o
In file included from /usr/local/include/pcl-1.8/pcl/sample_consensus/sac_model.h:52:0,
from /usr/local/include/pcl-1.8/pcl/sample_consensus/sac.h:45,
from /usr/local/include/pcl-1.8/pcl/sample_consensus/ransac.h:44,
from /usr/local/include/pcl-1.8/pcl/registration/icp.h:45,
from /usr/local/include/pcl-1.8/pcl/registration/gicp.h:44,
from /home/dell/lyq/study/fast_gicp/src/fast_gicp/src/align.cpp:11:
/usr/local/include/pcl-1.8/pcl/sample_consensus/model_types.h: In function ‘void __static_initialization_and_destruction_0(int, int)’:
/usr/local/include/pcl-1.8/pcl/sample_consensus/model_types.h:99:3: warning: ‘pcl::SAC_SAMPLE_SIZE’ is deprecated: This map is deprecated and is kept only to prevent breaking existing user code. Starting from PCL 1.8.0 model sample size is a protected member of the SampleConsensusModel class [-Wdeprecated-declarations]
SAC_SAMPLE_SIZE (sample_size_pairs, sample_size_pairs + sizeof (sample_size_pairs) / sizeof (SampleSizeModel));
^~~~~~~~~~~~~~~
/usr/local/include/pcl-1.8/pcl/sample_consensus/model_types.h:99:3: note: declared here
[100%] Linking CXX executable /home/dell/lyq/study/fast_gicp/devel/lib/fast_gicp/gicp_align
[100%] Built target gicp_align`

When I try to test the benchmark, the error occurs.
I used : https://github.com/SMRT-AIST/fast_gicp#benchmark
roscd fast_gicp/data rosrun fast_gicp gicp_align 251370668.pcd 251371071.pcd
and I get:
Segmentation fault (core dumped)

I tried to debug, and I find the problem is occur in https://github.com/SMRT-AIST/fast_gicp/blob/master/src/align.cpp#L141
voxelgrid.filter(*filtered);

How can I fix this problem?
Thanks.

By the way, I want to ask another question.
If I want to use VGICP(use MT and without CUDA) in my own ROS project. Do I just need to put /src/fast_gicp/ficp https://github.com/SMRT-AIST/fast_gicp/tree/master/src/fast_gicp/gicp, /include/fast_gicp/gicp https://github.com/SMRT-AIST/fast_gicp/tree/master/include/fast_gicp/gicp and /include/fast_gicp/so3 https://github.com/SMRT-AIST/fast_gicp/tree/master/include/fast_gicp/so3 folders under my package folders and then modify my CMAKEList and make?
Thanks a lot!

Applicability to indoor scenes

Hi

I wanted to ask if you think the voxel based GICP proposed in this work can also work with indoor scenes where the task is to estimate object poses? I was trying it out for a couple of indoor scenes and while the GICP algorithm in the repo works fine, the VGICP is unable to converge to the same solution that GICP does in several cases.

KDTree creation takes much time for big clouds

Problem

I discovered that KDtree creation takes significantly more time when aligning with a big target cloud (with a map).

KDtree creation takes only 3ms for clouds with 17k points, but it takes 90ms for 370k points (my small 100x100 meters filtered map).

PCL creates kdtree inside "align" method, so it means that ndt_cuda (P2D) runs for 98ms instead of 8ms during first run.

Moreover, PCL spends 20-30ms to calculate fitness score, regardless of the size of the target map. It is also not very good 😁.

Detailed benchmark results

Aligning big target cloud with kdtree creation

target:364444[pts] source:18564[pts]
--- fgicp_st ---
single:2293.17[msec] 
--- fgicp_mt ---
single:460.209[msec] 
--- vgicp_st ---
single:2132.44[msec] 
--- vgicp_mt ---
single:381.7[msec] 
--- ndt_cuda (P2D) ---
single:98.3882[msec] 
--- ndt_cuda (D2D) ---
single:93.0577[msec] 
--- vgicp_cuda (parallel_kdtree) ---
single:340.319[msec] 
--- vgicp_cuda (gpu_bruteforce) ---
single:9404.37[msec] 
--- vgicp_cuda (gpu_rbf_kernel) ---
terminate called after throwing an instance of 'thrust::system::detail::bad_alloc'
  what():  std::bad_alloc: cudaErrorMemoryAllocation: out of memory
Aborted (core dumped)

Aligning big target cloud without kdtree creation

target:364444[pts] source:18564[pts]
--- fgicp_st ---
single:2224.49[msec] 
--- fgicp_mt ---
single:312.474[msec] 
--- vgicp_st ---
single:2044.39[msec] 
--- vgicp_mt ---
single:293.93[msec] 
--- ndt_cuda (P2D) ---
single:8.7521[msec] 
--- ndt_cuda (D2D) ---
single:7.81053[msec] 
--- vgicp_cuda (parallel_kdtree) ---
single:246.274[msec] 
--- vgicp_cuda (gpu_bruteforce) ---
single:9536.63[msec] 
--- vgicp_cuda (gpu_rbf_kernel) ---
terminate called after throwing an instance of 'thrust::system::detail::bad_alloc'
  what():  std::bad_alloc: cudaErrorMemoryAllocation: out of memory
Aborted (core dumped)

Aligning default clouds with kdtree creation

--- fgicp_st ---
single:263.107[msec] 100times:26208.3[msec] 100times_reuse:17901.3[msec] fitness_score:0.204374
--- fgicp_mt ---
single:53.8672[msec] 100times:3846.4[msec] 100times_reuse:2469.65[msec] fitness_score:0.204384
--- vgicp_st ---
single:185.224[msec] 100times:18208.6[msec] 100times_reuse:9911.83[msec] fitness_score:0.205022
--- vgicp_mt ---
single:54.0197[msec] 100times:2933.43[msec] 100times_reuse:1687.04[msec] fitness_score:0.205022
--- ndt_cuda (P2D) ---
single:8.74302[msec] 100times:861.372[msec] 100times_reuse:499.426[msec] fitness_score:0.197207
--- ndt_cuda (D2D) ---
single:6.07843[msec] 100times:587.673[msec] 100times_reuse:315.464[msec] fitness_score:0.198976
--- vgicp_cuda (parallel_kdtree) ---
single:31.5053[msec] 100times:2363.01[msec] 100times_reuse:992.915[msec] fitness_score:0.204697
--- vgicp_cuda (gpu_bruteforce) ---
single:45.3529[msec] 100times:3512.59[msec] 100times_reuse:1685.24[msec] fitness_score:0.204693
--- vgicp_cuda (gpu_rbf_kernel) ---
single:7.57619[msec] 100times:750.764[msec] 100times_reuse:309.824[msec] fitness_score:0.204584

Aligning default clouds without kdtree creation

--- fgicp_st ---
single:260.423[msec] 100times:25678.1[msec] 100times_reuse:17731.4[msec]
--- fgicp_mt ---
single:82.7624[msec] 100times:3546.36[msec] 100times_reuse:2466.41[msec]
--- vgicp_st ---
single:182.533[msec] 100times:17876.4[msec] 100times_reuse:9981.22[msec]
--- vgicp_mt ---
single:36.4721[msec] 100times:2744.13[msec] 100times_reuse:1607.6[msec]
--- ndt_cuda (P2D) ---
single:6.20965[msec] 100times:558.324[msec] 100times_reuse:472.443[msec]
--- ndt_cuda (D2D) ---
single:3.04992[msec] 100times:306.246[msec] 100times_reuse:303.85[msec]
--- vgicp_cuda (parallel_kdtree) ---
single:37.2568[msec] 100times:1809.56[msec] 100times_reuse:1083.63[msec]
--- vgicp_cuda (gpu_bruteforce) ---
single:42.7751[msec] 100times:3270.32[msec] 100times_reuse:1685.56[msec]
--- vgicp_cuda (gpu_rbf_kernel) ---
single:4.78256[msec] 100times:472.186[msec] 100times_reuse:306.657[msec]

Simple solution

My solution is to set empty kdtree as search method and enable force_no_recompute flag before calling "align".
Like this:

pcl::search::KdTree<pcl::PointXYZ>::Ptr empty_kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
reg.setSearchMethodTarget(empty_kdtree, true);

You can easily repeat my experiment in fork, branch test/disable-kdtree. Command:

build/gicp_align data/saved_map_cloud.pcd data/saved_scan.pcd

Better solution

Better solution would be to replace PCL default kdtree with something more efficient 😊

scan matching has not converged!!

hi,
I change the registration method of hdl graph slam to fast gicp , but the registration method may be not converged!

Can it read the topic of rosbag in LEGO-LOAM or LIO-SAM？

I found that this code reading point cloud reads every. bin, so can it read the topic of rosbag? Can it be used in LEGO-LOAM or LIO-SAM? Can I ask you if it's convenient?

registration failure for large translation

Hi. I'm currently trying to implement simple odometry using fast-gicp.
But for large translation, sometimes it fails.

Example picture is like this:

Point cloud colored red is registration result from fast-icp.
Point cloud colored green is the target point cloud of the registration.
Point cloud colored blue is source cloud transformed by initial guess transformation matrix.

As you can see point cloud colored green and blue are aligned sufficiently close.
But after running fast-gicp, it diverges.
One thing i'm in doubt is that aliasing occurs in the angular domain.

Here is the code and data that may help you reproduce the results.
sample.zip

I hope this report useful.
Thanks.

How can I get the author's base-libraries version?

Such as PCL and eigen version and so on?

How to use this code in visual studio?

This work is good, I use it in visual studio, I add path of fast_gicp at include, it return error LNK2019, it means lack the lib ,but i am not find some libs in your code ,what should i do? thank you

Support for more pcl point types

Hi,

I was wondering if you could add support for more point types? For example, I would like to use pcl::PointXYZRGBNormal but it is not possible.

Cheers

the processing timing too large in some frame

Dear officer,

I integrated the fast_gicp into hdl_slam by adding the methods into registration.cpp, very impressive compare to the original gicp in terms of processing time.
But I fount in some frame it appears larger processing time(more than 1s ) which might reduce the accuracy of odometry if I try to run the method in real-time

Record processing timing of matching function in the below function
Eigen::Matrix4f pose = matching(cloud_msg->header.stamp, cloud);
https://github.com/koide3/hdl_graph_slam/blob/0228e86b7f4f9649515b0217440694bc1fdf7f20/apps/scan_matching_odometry_nodelet.cpp#L113

Running with ubuntu18.04 with ROS melodic, CUDA 10.2 with Nvidia GeForce GTX 1660Ti.
FastGICP(max: 1465.45 ms), Fast VGICP(max 1156.91ms) and FastVGICPCuda(max 1626.14ms) have similar results after a few repeat trying with our bag files.
The bag file:
https://www.dropbox.com/s/pfjvauvletdpnh2/2019-04-28-20-58-02.bag?dl=0
https://www.polyu-ipn-lab.com/download
(playback speed: FastGICP r=0.5, Fast VGICP r =1, FastVGICPCuda r =1)

Take FastVGICPCuda's processing time (in ms) as an example:

25.238248
26.579396
46.928215
27.55004
32.815564
1255.325887
31.012012
30.705918
19.767951
24.523302
28.338081
30.295409
33.668795

max:1626.14/ min16.03/ mean 38.08/ median 31.28

My question is:
Is it an expected result in terms of processing time? or some mismatch of the bag files with the fast_gicp (I ran the bag file with GICP with max 520.10 ms, playback speed 0.1 )

cuda driver version?

I use ubuntu18.04 and cuda10.0.

When I run the code ,I get this bug:

terminate called after throwing an instance of 'thrust::system::detail::bad_alloc'
what(): std::bad_alloc: cudaErrorSystemDriverMismatch: system has unsupported display driver / cuda driver combination

I think maybe the cuda driver version is wrong. what is your version of cuda driver?

support point-to-point icp

Does fast_gicp support point-to-point icp?

ndt_cuda.cu doesn't contain the getTransformationProbability() function

hi koide, I have tested the autoware/ndt_gpu and want to integrate some autoware localization ideas into the hdl_graph_slam. I need the getTransformationProbability() but in fast_gicp, I cannot find something related. And suggestions to implement it ? @koide3 Thank a lot.

Does cuda version of LM work?

Hi, I try the master branch. The LM version of the CPU version works fine for me. However, the lm-Cuda branch cannot run at all. ERROR occurs and traces back to setinput cloud.
Would it be impertinent to ask when the Cuda version of lm will be ready?