Hi Koide,

thank you so much for your great work! However, I just found that the gradient test method you use for gicp's optimization progress is not compatible with that of the official PCL since this commit 3854 They just provided a virtual function, which has to be overridden when implementing, for checking the gradient when using bfgs algorithm.

the corresponding code in your project is here

By writing this issue, I am just wondering if you had plan to modify this project to be compatible with the latest PCL.

Looking forward to your reply and best regards,
Jiancong

hello,When I use rosrun ndt_ omp align 251370668.pcd 251371071. pcd
The error is as follows: My system is 18.04, my PCL is 1.8.1, and my VTK is 7.1.1. May I know the reason?

Generic Warning: In /build/vtk6-VHOYAG/vtk6-6.3.0+dfsg1/Rendering/OpenGL/vtkOpenGLPolyDataMapper.cxx, line 69
vtkOpenGLPolyDataMapper::vtkOpenGLPolyDataMapper was deprecated for VTK 6.2 and will be removed in a future version.

段错误 (核心已转储)

When two similar point clouds are registered with a local map, the registration time of the previous frame is 300 ms and that of the next frame is 50 ms. The number of iterations is the same. What is the possible cause? Parameter configuration: Select DIRECT7, the number of CPU cores to 6, number of threads to 12, point cloud downsampling to 0.8 m, and NDT grid resolution to 2.4 m.

Looking forward to a reply! Thank you!

I made an attempt to use the Hessian matrix to detect degradation, But I find the ndt Hessian matrix cann`t point out the Direction of degradation. but I I see the parper LiTAMIN2: Ultra Light LiDAR-based SLAM using Geometric Approximation applied with KL-Divergence, it say the PCA can point the Direction of degradation, So, I want to know is there anything I
misunderstanding?

Hello, your repo is really impressive. Pcl ndt algorithm uses align() function to set approximate transformation between source cloud and target cloud,but align() function in your ndt_omp repo differs from pcl . How can I set approximate transformation between source cloud and target cloud?Thank you.

@kkoide3 Hello,when I use ndt_omp a running error occurs,but it doesn't show up with original ndt ,the issue I proposed in PCL is here:PointCloudLibrary/pcl#5714, thanks a lot for your help!

Hello,

i see in the cmakelist file that the package needs ROS (roscpp) to be built. Is it planned to have a version compatible with ROS2 also?

And could ndt_omp be easilty detached from ros, do you think?

thanks

After catkin_make, it can run the example.

rosrun ndt_omp align 251370668.pcd 251371071.pcd

But how to use ndt_omp on my code?

I add ndt_omp on my CMaktLists.txt

find_package(catkin REQUIRED COMPONENTS
  tf
  tf2
  tf2_ros
  roscpp
  rospy
  cv_bridge
  # pcl library
  pcl_conversions
  # msgs
  std_msgs
  sensor_msgs
  geometry_msgs
  nav_msgs
  message_generation
  visualization_msgs
  std_srvs
  ndt_omp
)

catkin_package(
  INCLUDE_DIRS include
  DEPENDS PCL GTSAM

  CATKIN_DEPENDS
  tf2
  tf2_ros
  roscpp
  std_msgs
  nav_msgs
  geometry_msgs
  sensor_msgs
  message_runtime
  message_generation
  visualization_msgs
  ndt_omp
)

and link to executable by

target_link_libraries(${PROJECT_NAME}_mapLocalization Boost::timer ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam yaml-cpp ${ROS_LIB})

It can catkin_make, But when I run my node, it received signal SIGSEGV, Segmentation fault.
The GDB Info is below:

#0  __memcmp_avx2_movbe ()
    at ../sysdeps/x86_64/multiarch/memcmp-avx2-movbe.S:218
#1  0x00007ffff5dfd333 in __gnu_cxx::__normal_iterator<pcl::PCLPointField const*, std::vector<pcl::PCLPointField, std::allocator<pcl::PCLPointField> > > std::__find_if<__gnu_cxx::__normal_iterator<pcl::PCLPointField const*, std::vector<pcl::PCLPointField, std::allocator<pcl::PCLPointField> > >, __gnu_cxx::__ops::_Iter_pred<pcl::getFieldIndex<pcl::PointXYZI>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<pcl::PCLPointField, std::allocator<pcl::PCLPointField> > const&)::{lambda(auto:1 const&)#1}> >(__gnu_cxx::__normal_iterator<pcl::PCLPointField const*, std::vector<pcl::PCLPointField, std::allocator<pcl::PCLPointField> > >, __gnu_cxx::__normal_iterator<pcl::PCLPointField const*, std::vector<pcl::PCLPointField, std::allocator<pcl::PCLPointField> > >, __gnu_cxx::__ops::_Iter_pred<pcl::getFieldIndex<pcl::PointXYZI>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<pcl::PCLPointField, std::allocator<pcl::PCLPointField> > const&)::{lambda(auto:1 const&)#1}>, std::random_access_iterator_tag) ()
   from /lib/x86_64-linux-gnu/libpcl_filters.so.1.10
#2  0x00007ffff5f7a522 in void pcl::getMinMax3D<pcl::PointXYZI>(pcl::PointCloud<pcl::PointXYZI>::ConstPtr const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, float, float, Eigen::Matrix<float, 4, 1, 0, 4, 1>&, Eigen::Matrix<float, 4, 1, 0, 4, 1>&, bool) ()
   from /lib/x86_64-linux-gnu/libpcl_filters.so.1.10
#3  0x00007ffff7522b70 in pclomp::VoxelGridCovariance<pcl::PointXYZI>::applyFilt--Type <RET> for more, q to quit, c to continue without paging--c
er(pcl::PointCloud<pcl::PointXYZI>&) () from /home/oem/Projects/lio_ws/devel/lib/libndt_omp.so
#4  0x00005555555c0f71 in pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>::init() ()
#5  0x00005555555c9a47 in mapLocalization::initializeSystem() ()

How can I fix this problem? Thanks!

Hi,

your readme mentions that it is 10x faster than PCL, any consideration of upstreaming it to PCL so that everyone can share that improvement?

CC @kunaltyagi

When trying to build, the following error is occurring:

error: ‘shared_ptr’ in namespace ‘pcl’ does not name a template type
typedef pcl::shared_ptr< pcl::VoxelGrid > Ptr;

error: ‘shared_ptr’ in namespace ‘pcl’ does not name a template type
typedef pcl::shared_ptr< const pcl::VoxelGrid > ConstPtr;

My pcl version is the 1.8.1. I temporarily fixed it by replacing pcl::shared_ptr to boost::shared_ptr. I saw there was a recent pull request on this regard.

Hi, and thank you for making this code available. I have a question regarding the gicp implementation.

In a one-shot test project, it gives a huge speed increase over the pcl::gicp. However, when i actually integrate it into my application (with streaming data), the speed jumps around, 21ms some frames, then 255 the next. Could you think of any reason for this?

Do I need to set the num_threads variable for gicp, as you do for NDT?

I am running it like this:

 pclomp::GeneralizedIterativeClosestPoint<POINT_TYPE, POINT_TYPE> icp;

	  icp.setTransformationEpsilon(params_.tf_epsilon);
	  icp.setMaxCorrespondenceDistance(params_.corr_dist);
	  icp.setMaximumIterations(params_.iterations);

	  icp.setInputSource(query);
	  icp.setInputTarget(reference);

	  PointCloud unused;
	  icp.align(unused);

	  // Retrieve transformation and estimate and update.
	 Eigen::Matrix4f   T = icp.getFinalTransformation();

hello,

when I update inputTargetCloud, it occurs, why?
Can someone help me? Thanks a lot.

Thank you for creating this great project.

I've a question regarding to the last function in the source code calculateScore.

Is this a new way to evaluate the ndt matching result ?

After compare the fitness score value in the benchmark, can I get the conclusion, the bigger this value, the better the algorithm are?

Such as

And there are lots of errors like that when catkin_make .Is it the pcl version incorrect?

I made an attempt to use the Hessian matrix to detect degradation, But I find the ndt Hessian matrix cann`t point out the Direction of degradation. but I I see the parper LiTAMIN2: Ultra Light LiDAR-based SLAM using Geometric Approximation applied with KL-Divergence, it say the PCA can point the Direction of degradation, So, I want to know is there anything I
misunderstanding?

Hi koide,

thank you for your great work. I am using you hdl_localization together with this library and wanted to get a bit more information from the result about „how good“ the alignment is.

Therefore I stumbled across your calculateScore function and the TransformationProbability Function (in newer versions it is renamed to likelihood) of PCL.
If I understood the comments in the code of PCL and yours correctly both should implement Magnusson 2009 Eq 6.9.
In your case you use negative log likelihood instead of likelihood in the pcl case right?

Now I used the calculateScore Function together with the aligned Pointcloud which you feed into https://github.com/koide3/hdl_localization/blob/1cdef711d66fdb4ec8969623dd2568eb3f45ce25/apps/hdl_localization_nodelet.cpp#L441 , which I hope was the correct usage.
But I got in trouble with interpreting the result.

Your comment to the function suggests lower equals better but I am not sure if this really applies. For visually good alignment I got scores of 0.2-0.3 but if I moved the robot a bit it was also possible that it dropped close to zero.
For no alignment at all (out of map) the result is 0 which is no problem but I was wondering what negative values may mean.
Because I was only familiar with neg log likely hood and positive results.
I got negative results first only if I completely miss placed the robot in the map. So i thought this may make sense but then I also got negative values while driving with a correct localization results.

Maybe you can help me a bit to interpret the results of the Scoring function and especially why it gets negative at the first place.

thank you in advance,
Sven

I am very interested in your algorithm. I would like to propose to incorporate it even at Autoware, but what about licensing? Autoware is Apache 2 in order to be used by various companies, so we are happy if it is Apache 2.
https://github.com/CPFL/Autoware

Hello, thx for your implimentation on ndt. And here's an error of point types.
When I use PointXYZINormal to set pointcloud, the error appears:

CMakeFiles/centric_map_node.dir/src/centric_map.cpp.o: In function CentricMap::findtransform()': /home/a/driver_ws/src/centric_map_v.02_ranc/src/centric_map.cpp:147: undefined reference to pclomp::NormalDistributionsTransform<pcl::PointXYZINormal, pcl::PointXYZINormal>::NormalDistributionsTransform()'
CMakeFiles/centric_map_node.dir/src/centric_map.cpp.o: In function pclomp::NormalDistributionsTransform<pcl::PointXYZINormal, pcl::PointXYZINormal>::init()': /home/a/driver_ws/src/ndt_omp/include/pclomp/voxel_grid_covariance_omp.h:296: undefined reference to pclomp::VoxelGridCovariancepcl::PointXYZINormal::applyFilter(pcl::PointCloudpcl::PointXYZINormal&)'
CMakeFiles/centric_map_node.dir/src/centric_map.cpp.o: In function pclomp::NormalDistributionsTransform<pcl::PointXYZINormal, pcl::PointXYZINormal>::setInputTarget(boost::shared_ptr<pcl::PointCloud<pcl::PointXYZINormal> const> const&)': /home/a/driver_ws/src/ndt_omp/include/pclomp/voxel_grid_covariance_omp.h:296: undefined reference to pclomp::VoxelGridCovariancepcl::PointXYZINormal::applyFilter(pcl::PointCloudpcl::PointXYZINormal&)'
CMakeFiles/centric_map_node.dir/src/centric_map.cpp.o:(.data.rel.ro._ZTVN6pclomp19VoxelGridCovarianceIN3pcl15PointXYZINormalEEE[_ZTVN6pclomp19VoxelGridCovarianceIN3pcl15PointXYZINormalEEE]+0x48): undefined reference to pclomp::VoxelGridCovariance<pcl::PointXYZINormal>::applyFilter(pcl::PointCloud<pcl::PointXYZINormal>&)' CMakeFiles/centric_map_node.dir/src/centric_map.cpp.o:(.data.rel.ro._ZTVN6pclomp28NormalDistributionsTransformIN3pcl15PointXYZINormalES2_EE[_ZTVN6pclomp28NormalDistributionsTransformIN3pcl15PointXYZINormalES2_EE]+0x58): undefined reference to pclomp::NormalDistributionsTransform<pcl::PointXYZINormal, pcl::PointXYZINormal>::computeTransformation(pcl::PointCloudpcl::PointXYZINormal&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&)'

Could you please give me a method to bug that?
Thx for your help!

hey there, i'm compiling this tool with PCL version 1.8, Segfault really annoys me so much, no matter release or debug, when setinputtarget() function called, it crashes with segfault. and pcl 1.7 can make it work well (release or debug), do you have any ideas about that?

environment:
ubuntu: 18.04
pcl: 1.11.1
ros : melodic

I do not konw why aderived class cannot be rolled over to a base class?

src/ndt_omp/apps/align.cpp:122:55: error: no matching function for call to ‘align(pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr&, pcl::PointCloudpcl::PointXYZ::Ptr&, pcl::PointCloudpcl::PointXYZ::Ptr&)’
aligned = align(gicp_omp, target_cloud, source_cloud);
^
~/project/testhdl/src/ndt_omp/apps/align.cpp:15:37: note: candidate: pcl::PointCloudpcl::PointXYZ::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr, pcl::PointCloudpcl::PointXYZ::Ptr&, pcl::PointCloudpcl::PointXYZ::Ptr&)
pcl::PointCloudpcl::PointXYZ::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr registration, pcl::PointCloudpcl::PointXYZ::Ptr& target_cloud, pcl::PointCloudpcl::PointXYZ::Ptr& source_cloud ) {
^~~~~
~/project/testhdl/src/ndt_omp/apps/align.cpp:15:37: note: no known conversion for argument 1 from ‘pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr {aka boost::shared_ptr<pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> >}’ to ‘pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr {aka std::shared_ptr<pcl::Registration<pcl::PointXYZ, pcl::PointXYZ> >}’
In file included from /usr/include/boost/config/no_tr1/memory.hpp:21:0,
from /usr/include/boost/smart_ptr/shared_ptr.hpp:23,
from /usr/include/boost/shared_ptr.hpp:17,
from /opt/ros/melodic/include/ros/forwards.h:37,
from /opt/ros/melodic/include/ros/common.h:37,
from /opt/ros/melodic/include/ros/ros.h:43,
from ~/project/testhdl/src/ndt_omp/apps/align.cpp:2:
/usr/include/c++/7/memory:114:1: note: candidate: void* std::align(std::size_t, std::size_t, void*&, std::size_t&)
align(size_t __align, size_t __size, void*& __ptr, size_t& __space) noexcept
^~~~~
/usr/include/c++/7/memory:114:1: note: candidate expects 4 arguments, 3 provided
~/project/testhdl/src/ndt_omp/apps/align.cpp:145:58: error: no matching function for call to ‘align(pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr&, pcl::PointCloudpcl::PointXYZ::Ptr&, pcl::PointCloudpcl::PointXYZ::Ptr&)’
aligned = align(ndt_omp, target_cloud, source_cloud);
^
~/project/testhdl/src/ndt_omp/apps/align.cpp:15:37: note: candidate: pcl::PointCloudpcl::PointXYZ::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr, pcl::PointCloudpcl::PointXYZ::Ptr&, pcl::PointCloudpcl::PointXYZ::Ptr&)
pcl::PointCloudpcl::PointXYZ::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr registration, pcl::PointCloudpcl::PointXYZ::Ptr& target_cloud, pcl::PointCloudpcl::PointXYZ::Ptr& source_cloud ) {
^~~~~
~/project/testhdl/src/ndt_omp/apps/align.cpp:15:37: note: no known conversion for argument 1 from ‘pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr {aka boost::shared_ptr<pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> >}’ to ‘pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr {aka std::shared_ptr<pcl::Registration<pcl::PointXYZ, pcl::PointXYZ> >}’
In file included from /usr/include/boost/config/no_tr1/memory.hpp:21:0,
from /usr/include/boost/smart_ptr/shared_ptr.hpp:23,
from /usr/include/boost/shared_ptr.hpp:17,
from /opt/ros/melodic/include/ros/forwards.h:37,
from /opt/ros/melodic/include/ros/common.h:37,
from /opt/ros/melodic/include/ros/ros.h:43,
from ~/project/testhdl/src/ndt_omp/apps/align.cpp:2:
/usr/include/c++/7/memory:114:1: note: candidate: void* std::align(std::size_t, std::size_t, void*&, std::size_t&)
align(size_t __align, size_t __size, void*& __ptr, size_t& __space) noexcept
^~~~~
/usr/include/c++/7/memory:114:1: note: candidate expects 4 arguments, 3 provided
ndt_omp/CMakeFiles/align.dir/build.make:62: recipe for target 'ndt_omp/CMakeFiles/align.dir/apps/align.cpp.o' failed

thank you for your contribution, i don't know what is difference between DIRECT7 、DIRECT1 and KDTREE ？

Hello @koide3, thank you so much for your great work!

I've read #29 and it seems like DIRECT7 was come by you to amend the PCL implementation.
I would like to confirm if the term DIRECT7 was first mentioned here (ndt_omp), or was there any other reference I could look up?
Thank you in advance!

@koide3
I ported ndt_omp to ROS2 and set up a workflow for CI. tier4#5

I would like to create a pull request to this repository, but before that I would like to discuss how to manage branches.
In our setup, we support both ROS1 and ROS2 builds. Therefore, we can support both in the master branch.
However, some repositories (e.g. https://github.com/ros/diagnostics) have separate branches like master and ros2-devel. This simplifies the release process. Which pattern is better for you to manage?

hello,friends. today, i compile this on Xavier which is NVIDIA's product, it is the ARM architecture different from the intel‘s x86_64. when i build it on Xavier, some errors occur!! like these:
c++: error: unrecognized command line option ‘-msse’; did you mean ‘-fdse’?
c++: error: unrecognized command line option ‘-msse2’
c++: error: unrecognized command line option ‘-msse3’
c++: error: unrecognized command line option ‘-msse4’
c++: error: unrecognized command line option ‘-msse4.1’
c++: error: unrecognized command line option ‘-msse4.2’
c++: error: unrecognized command line option ‘-msse’; did you mean ‘-fdse’?
c++: error: unrecognized command line option ‘-msse2’
c++: error: unrecognized command line option ‘-msse3’
c++: error: unrecognized command line option ‘-msse4’
c++: error: unrecognized command line option ‘-msse4.1’
c++: error: unrecognized command line option ‘-msse4.2’

In the CMakeLists.txt is "add_definitions(-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")".
which is OK on the intel‘s x86_64 architecture. Can you give me some help ? THANK YOU FIRST!!

Hi @kkoide3
I am working on with ndt-omp. But when i used scan_matching to align 2 pointclouds, it worked fine on ubuntu 20 but it failed on ubuntu 22
PCL version on ubuntu 22 is 1.10 and eigen is 3.4
Could you have a check ?

Hi @koide3 , thanks for you sharing the work. When calculating the step in computeStepLengthMT() func, bool interval_converged = (step_max - step_min) > 0, step_max = step_size_, step_min = transformation_epsilon_ / 2, step_size_ = 0.1, transformation_epsilon_ = 0.1, so the interval_converged = true, Do we need initialize interval_converged = false before we start calculate the optimal step of delta_p_norm?

Best
jxl

hello kode3, thanks for your work! i have a question.when i use your ndt omp library for localization, i do not know how to get the covariance matrix which can evaluate my result?

c++: error: unrecognized command line option ‘-msse’
c++: error: unrecognized command line option ‘-msse2’
c++: error: unrecognized command line option ‘-msse3’
c++: error: unrecognized command line option ‘-msse4’
c++: error: unrecognized command line option ‘-msse4.1’
c++: error: unrecognized command line option ‘-msse4.2’
c++: error: unrecognized command line option ‘-msse’
c++: error: unrecognized command line option ‘-msse2’
c++: error: unrecognized command line option ‘-msse3’
c++: error: unrecognized command line option ‘-msse4’
c++: error: unrecognized command line option ‘-msse4.1’
c++: error: unrecognized command line option ‘-msse4.2’
ndt_omp/CMakeFiles/ndt_omp.dir/build.make:62: recipe for target 'slam/ndt_omp/CMakeFiles/ndt_omp.dir/src/pclomp/voxel_grid_covariance_omp.cpp.o' failed
make[2]: *** [slam/ndt_omp/CMakeFiles/ndt_omp.dir/src/pclomp/voxel_grid_covariance_omp.cpp.o] Error 1

I modify CMakeLists.txt with:
add_definitions(-std=c++11)
set(CMAKE_CXX_FLAGS "-std=c++11")
still have error:
catkin_ws/src/slam/ndt_omp/include/pclomp/gicp_omp_impl.hpp:78:61: error: ‘omp_get_thread_num’ was not declared in this scope
auto& nn_indecies = nn_indecies_array[omp_get_thread_num()];

These four lines in .clang-format seems to be invalid.
I think the correct way of writing would be

RawStringFormats:
  - Language: TextProto
    Delimiters:
      - pb
    BasedOnStyle: google

Please also refer to this PR: #56
After applying this PR, we can run clang-format e.g. by clang-format -i $FILE

[ 83%] Building CXX object CMakeFiles/align.dir/apps/align.cpp.o
In file included from /usr/include/boost/smart_ptr/detail/sp_thread_sleep.hpp:22,
                 from /usr/include/boost/smart_ptr/detail/yield_k.hpp:23,
                 from /usr/include/boost/smart_ptr/detail/spinlock_gcc_atomic.hpp:14,
                 from /usr/include/boost/smart_ptr/detail/spinlock.hpp:42,
                 from /usr/include/boost/smart_ptr/detail/spinlock_pool.hpp:25,
                 from /usr/include/boost/smart_ptr/shared_ptr.hpp:29,
                 from /usr/include/boost/shared_ptr.hpp:17,
                 from /opt/ros/noetic/include/ros/forwards.h:37,
                 from /opt/ros/noetic/include/ros/common.h:37,
                 from /opt/ros/noetic/include/ros/ros.h:43,
                 from /home/toni/.cache/yay/ros-noetic-ndt-omp-git/src/ros-noetic-ndt-omp-git/apps/align.cpp:2:
/usr/include/boost/bind.hpp:36:1: note: ‘#pragma message: The practice of declaring the Bind placeholders (_1, _2, ...) in the global namespace is deprecated. Please use <boost/bind/bind.hpp> + using namespace boost::placeholders, or define BOOST_BIND_GLOBAL_PLACEHOLDERS to retain the current behavior.’
   36 | BOOST_PRAGMA_MESSAGE(
      | ^~~~~~~~~~~~~~~~~~~~
/home/toni/.cache/yay/ros-noetic-ndt-omp-git/src/ros-noetic-ndt-omp-git/apps/align.cpp: In function ‘int main(int, char**)’:
/home/toni/.cache/yay/ros-noetic-ndt-omp-git/src/ros-noetic-ndt-omp-git/apps/align.cpp:80:55: error: no matching function for call to ‘align(pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr&, pcl::PointCloud<pcl::PointXYZ>::Ptr&, pcl::PointCloud<pcl::PointXYZ>::Ptr&)’
   80 |   aligned = align(gicp_omp, target_cloud, source_cloud);
      |                                                       ^
/home/toni/.cache/yay/ros-noetic-ndt-omp-git/src/ros-noetic-ndt-omp-git/apps/align.cpp:15:37: note: candidate: ‘pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr, const Ptr&, const Ptr&)’
   15 | pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr registration, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud ) {
      |                                     ^~~~~
/home/toni/.cache/yay/ros-noetic-ndt-omp-git/src/ros-noetic-ndt-omp-git/apps/align.cpp:15:96: note:   no known conversion for argument 1 from ‘pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr’ {aka ‘boost::shared_ptr<pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> >’} to ‘pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr’ {aka ‘std::shared_ptr<pcl::Registration<pcl::PointXYZ, pcl::PointXYZ> >’}
   15 | pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr registration, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud ) {
      |                                           ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~
In file included from /usr/include/boost/smart_ptr/detail/sp_counted_impl.hpp:35,
                 from /usr/include/boost/smart_ptr/detail/shared_count.hpp:27,
                 from /usr/include/boost/smart_ptr/shared_ptr.hpp:17,
                 from /usr/include/boost/shared_ptr.hpp:17,
                 from /opt/ros/noetic/include/ros/forwards.h:37,
                 from /opt/ros/noetic/include/ros/common.h:37,
                 from /opt/ros/noetic/include/ros/ros.h:43,
                 from /home/toni/.cache/yay/ros-noetic-ndt-omp-git/src/ros-noetic-ndt-omp-git/apps/align.cpp:2:
/usr/include/c++/10.2.0/memory:123:1: note: candidate: ‘void* std::align(std::size_t, std::size_t, void*&, std::size_t&)’
  123 | align(size_t __align, size_t __size, void*& __ptr, size_t& __space) noexcept
      | ^~~~~
/usr/include/c++/10.2.0/memory:123:1: note:   candidate expects 4 arguments, 3 provided
/home/toni/.cache/yay/ros-noetic-ndt-omp-git/src/ros-noetic-ndt-omp-git/apps/align.cpp:103:58: error: no matching function for call to ‘align(pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr&, pcl::PointCloud<pcl::PointXYZ>::Ptr&, pcl::PointCloud<pcl::PointXYZ>::Ptr&)’
  103 |       aligned = align(ndt_omp, target_cloud, source_cloud);
      |                                                          ^
/home/toni/.cache/yay/ros-noetic-ndt-omp-git/src/ros-noetic-ndt-omp-git/apps/align.cpp:15:37: note: candidate: ‘pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr, const Ptr&, const Ptr&)’
   15 | pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr registration, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud ) {
      |                                     ^~~~~
/home/toni/.cache/yay/ros-noetic-ndt-omp-git/src/ros-noetic-ndt-omp-git/apps/align.cpp:15:96: note:   no known conversion for argument 1 from ‘pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr’ {aka ‘boost::shared_ptr<pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> >’} to ‘pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr’ {aka ‘std::shared_ptr<pcl::Registration<pcl::PointXYZ, pcl::PointXYZ> >’}
   15 | pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr registration, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud ) {
      |                                           ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~
In file included from /usr/include/boost/smart_ptr/detail/sp_counted_impl.hpp:35,
                 from /usr/include/boost/smart_ptr/detail/shared_count.hpp:27,
                 from /usr/include/boost/smart_ptr/shared_ptr.hpp:17,
                 from /usr/include/boost/shared_ptr.hpp:17,
                 from /opt/ros/noetic/include/ros/forwards.h:37,
                 from /opt/ros/noetic/include/ros/common.h:37,
                 from /opt/ros/noetic/include/ros/ros.h:43,
                 from /home/toni/.cache/yay/ros-noetic-ndt-omp-git/src/ros-noetic-ndt-omp-git/apps/align.cpp:2:
/usr/include/c++/10.2.0/memory:123:1: note: candidate: ‘void* std::align(std::size_t, std::size_t, void*&, std::size_t&)’
  123 | align(size_t __align, size_t __size, void*& __ptr, size_t& __space) noexcept
      | ^~~~~
/usr/include/c++/10.2.0/memory:123:1: note:   candidate expects 4 arguments, 3 provided
make[2]: *** [CMakeFiles/align.dir/build.make:76: CMakeFiles/align.dir/apps/align.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:607: CMakeFiles/align.dir/all] Error 2
make: *** [Makefile:136: all] Error 2

#26 should fix this. This project use c++14, but why use boost::shared_ptr when std::shared_str exist since c++11?

Last year, a per-point time stamp was added to velodyne ros drivers.

ros-drivers velodyne
Issue Add a per-point timestamping of data
ros-drivers/velodyne#281
PR Add per-point time field
ros-drivers/velodyne#290

I'd like to use this in ndt_omp as well, as it helps with distortion correction in combination with IMU.

I added a new point time PointXYZIT in reference to the issue below, but it doesn't work.

fast_gicp
Support for more pcl point types
SMRT-AIST/fast_gicp#10

henry@ubuntu:~/catkin_ws$ catkin_make -j4

~~~
[100%] Linking CXX executable /home/henry/catkin_ws/devel/lib/ndt_omp/align
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::KdTreeFLANN<PointXYZIT, flann::L2_Simple<float> >::nearestKSearch(PointXYZIT const&, int, std::vector<int, std::allocator<int> >&, std::vector<float, std::allocator<float> >&) const'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::KdTreeFLANN<PointXYZIT, flann::L2_Simple<float> >::setInputCloud(boost::shared_ptr<pcl::PointCloud<PointXYZIT> const> const&, boost::shared_ptr<std::vector<int, std::allocator<int> > const> const&)'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::KdTreeFLANN<PointXYZIT, flann::L2_Simple<float> >::radiusSearch(PointXYZIT const&, double, std::vector<int, std::allocator<int> >&, std::vector<float, std::allocator<float> >&, unsigned int) const'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::VoxelGrid<PointXYZIT>::applyFilter(pcl::PointCloud<PointXYZIT>&)'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::PCLBase<PointXYZIT>::setIndices(boost::shared_ptr<std::vector<int, std::allocator<int> > const> const&)'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `void pcl::getMinMax3D<PointXYZIT>(pcl::PointCloud<PointXYZIT>::ConstPtr const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, float, float, Eigen::Matrix<float, 4, 1, 0, 4, 1>&, Eigen::Matrix<float, 4, 1, 0, 4, 1>&, bool)'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::KdTreeFLANN<PointXYZIT, flann::L2_Simple<float> >::KdTreeFLANN(bool)'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::KdTreeFLANN<PointXYZIT, flann::L2_Simple<float> >::setEpsilon(float)'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::PCLBase<PointXYZIT>::setInputCloud(boost::shared_ptr<pcl::PointCloud<PointXYZIT> const> const&)'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::PCLBase<PointXYZIT>::setIndices(boost::shared_ptr<pcl::PointIndices const> const&)'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::PCLBase<PointXYZIT>::setIndices(unsigned long, unsigned long, unsigned long, unsigned long)'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::KdTreeFLANN<PointXYZIT, flann::L2_Simple<float> >::cleanup()'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::PCLBase<PointXYZIT>::PCLBase()'
/usr/bin/ld: /home/henry/catkin_ws/devel/lib/libndt_omp.so: undefined reference to `pcl::PCLBase<PointXYZIT>::setIndices(boost::shared_ptr<std::vector<int, std::allocator<int> > > const&)'
collect2: error: ld returned 1 exit status
make[2]: *** [ndt_omp/CMakeFiles/align.dir/build.make:390: /home/henry/catkin_ws/devel/lib/ndt_omp/align] エラー 1
make[1]: *** [CMakeFiles/Makefile2:4845: ndt_omp/CMakeFiles/align.dir/all] エラー 2
make: *** [Makefile:141: all] エラー 2
Invoking "make -j4" failed

Can you give me some advices on these result?
Thank you!

Customized ndt_omp
henry0572@de4d98b

Hello, Koide. Thanks very much for your awesome work and contribution to the society.
I've tested ndt_omp, gicp_omp and also fast vgicp on KITTI Odometry dataset with only the scan-to-scan matching and uniform motion based initial guess after tuning the parameters. I'd like to share the result here.

Method ATE (%) ARE(0.01deg/m) TPF(ms)
gicp_omp 1.12 0.17 273
voxel_gicp 1.09 0.32 128
ndt_omp 3.75 0.86 172

The rotation accuracy of gicp_omp is quite well (top 10 on KITTI leaderboard).

I'd like to involve these algorithms in my project as baseline front-end methods, thank you a lot.

when I catkin_make , it errors :
/src/ndt_omp/include/pclomp/gicp_omp_impl.hpp:64:70: error: there are no arguments to ‘omp_get_max_threads’ that depend on a template parameter, so a declaration of ‘omp_get_max_threads’ must be available [-fpermissive]
std::vector<std::vector> nn_indices_array(omp_get_max_threads());
^

Can someone help me? Thanks a lot.

when I compier this bag, I get a

error: ‘pcl_isfinite’ was not declared in this scope, and no declarations were found by argument-dependent lookup at the point of instantiation [-fpermissive]
ts[cp].x) || !pcl_isfinite (input_->points[cp].y) || !pcl_isfinite (input_->po

So can anyone give me some advice on how to solver this error?
Thanks!

I am trying to develop an odometry with ndt_omp. I import this package as the build depend and exec depend like other catkin packages.
To enable the cuda feature of PCL, I used the self-building version of PCL and modified the configuration of pcl_ros. It works well in building ndt_omp itself. However, when building my package, the error occurs:

It looks like that ndt_omp doesn't link to PCL. Therefore, I added the code below in CMakeLists.txt of ndt_omp and it works.

target_link_libraries(ndt_omp
  ${catkin_LIBRARIES}
  ${PCL_LIBRARIES}
)

However, it is strange that in the default ROS environment, all packages could be built without modifying ndt_omp. I do not understand why it occurs at all.

Hi,

I am quite curious about the implementation in neighborhood search. As stated in this comment, radius search by kd-tree is experimentally faster than direct search by voxel hashing. This is quite different from the intuition that complexity for searching with kd-tree and voxel hashing are O(log(n)) and O(1), respectively. Any hints for this?

Thanks for the great work!!

I'm slamming on kitti with gicp_omp but it doesn't work well.
How do I tune the parameters?
The program is run in ros and ubuntu 20.04 and the data is played back at 0.2 times the speed.
And 3.5m voxel grid is used for source and target clouds.
I also use a minmax filter of 1-150m.
(By the way, I can't use gpu.)

gicp params
MaxCorrespondenceDistance : 3.0
TransformationEpsilon : 1e-8

The following uses data from kitti00.

scan2scan

Green :ground truth, yellow : gicp_omp

scan2accumatedscan(n=30)(Using the last n scans for target)

Thank you for reading.

The code in ndt_omp_impl.hpp 134～139； How does it work ?

    if (delta_p_norm == 0 || delta_p_norm != delta_p_norm)
    {
      trans_probability_ = score / static_cast<double> (input_->points.size ());
      converged_ = delta_p_norm == delta_p_norm;
      return;
    }

Thanks a lot for your work!
I have a question.
I see std::unordered_map was included, but std::map is used in the implement.
Is this a consideration of memory usage?

hi, @koide3
In hdl_graph_slam project, I find follow code:
if(num_threads>0){ndt->setNumThreads(num_threads)}
I note that the value of "ndt_num_threads" is always set to 0, so when I wanted to use NDT_OMP in my 8 core CPU, I should set NumThreads = 0?

hey there, i'm compiling this tool with PCL version 1.8, Segfault really annoys me so much, no matter release or debug, when setinputtarget() function called, it crashes with segfault. and pcl 1.7 can make it work well (release or debug), do you have any ideas about that?

Hi @koide3 , thanks for sharing your work to the community. I have test the accuracy og the ndt-omp and the conventional ndt in pcl. the ndt_omp seems to drift in z axis dramatically compared with the conventioanl NDT in pcl. have you find this ?

is this caused by the searching method in the ndt_omp?

Best,
Welson,

Hi, @koide3 , thanks for your sharing work.
In computeAngleDerivatives() func, these lines maybe seems miss a minus.

// h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0;
h_ang_c2_ << (-sx * cz - cx * sy * sz), (-sx * sz - cx * sy * cz), 0;

// h_ang_d1_ << (-cy * cz), (cy * sz), (sy);
h_ang_d1_ << (-cy * cz), (cy * sz), (-sy);

// h_ang.row(4).noalias() = Eigen::Vector4f((-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0, 0.0f);
h_ang.row(4).noalias() = Eigen::Vector4f((-sx * cz - cx * sy * sz), (-sx * sz - cx * sy * cz), 0, 0.0f);

// h_ang.row(6).noalias() = Eigen::Vector4f((-cy * cz), (cy * sz), (sy), 0.0f);
h_ang.row(6).noalias() = Eigen::Vector4f((-cy * cz), (cy * sz), (-sy), 0.0f);

Best jxl

Hi,

Sorry to bother you. Thanks to your great package, I have ran a version of robot localization successfully with NDT in ndt_omp. During my tests, I found something different in performance between KDTREE and DIRECT7, but I found it difficult to explain.

I ran a diff-driven robot with a 2d laser in a static environment on the same path with same 2d map. I ran my robot with DIRECT7, the laser scan did not match the map well while I ran with KDTREE, the laser scan match better.

Here are two pictures of matching with KDTREE and DIRECT7.

KDTREE match below.

DIRECT7 match below.

I really can not think about a reason to explain this phenomenon. I would be so grateful if you could offer some help on it.

Best regards.