koide3 / ndt_omp Goto Github PK
View Code? Open in Web Editor NEWMulti-threaded and SSE friendly NDT algorithm
License: BSD 2-Clause "Simplified" License
Multi-threaded and SSE friendly NDT algorithm
License: BSD 2-Clause "Simplified" License
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.
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.
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?
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?
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!
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?
When I run a non ROS version of the program on UGV, I occasionally encounter the issue of "terminate called after throwing an instance of 'std:: length_error'", which causes the program to interrupt. The error log obtained from using the GDB debugger is as follows. Among all the programs, only ndtomp uses the "aligned-allocator" function, but I still cannot find the problem. I don't know if you have any suggestions.
terminate called after throwing an instance of 'std::length_error'
what(): vector::reserve
--Type for more, q to quit, c to continue without paging--where
Thread 1 "UGV" received signal SIGABRT, Aborted.
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
50 ../sysdeps/unix/sysv/linux/raise.c: 没有那个文件或目录.
(gdb) where
#0 __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1 0x0000fffff2dc1aac in __GI_abort () at abort.c:79
#2 0x0000fffff30d28bc in __gnu_cxx::__verbose_terminate_handler() ()
at /usr/lib/aarch64-linux-gnu/libstdc++.so.6
#3 0x0000fffff30d020c in () at /usr/lib/aarch64-linux-gnu/libstdc++.so.6
#4 0x0000fffff30d0270 in () at /usr/lib/aarch64-linux-gnu/libstdc++.so.6
#5 0x0000fffff30d0564 in () at /usr/lib/aarch64-linux-gnu/libstdc++.so.6
#6 0x0000fffff30cbb7c in std::__throw_length_error(char const*) ()
at /usr/lib/aarch64-linux-gnu/libstdc++.so.6
#7 0x0000fffff5cfde88 in std::vector<pcl::PointXYZ, Eigen::aligned_allocatorpcl::PointXYZ >::reserve(unsigned long) ()
at /usr/lib/aarch64-linux-gnu/libpcl_io.so.1.10
#8 0x0000fffff3db41d0 in vtkOpenGLIndexBufferObject::AppendPointIndexBuffer(std::vector<unsigned int, std::allocator >&, vtkCellArray*, long long) () at /usr/lib/aarch64-linux-gnu/libvtkRenderingOpenGL2-7.1.so.7.1p
#9 0x0000fffff3db454c in vtkOpenGLIndexBufferObject::CreatePointIndexBuffer(vtkCellArray*) () at /usr/lib/aarch64-linux-gnu/libvtkRenderingOpenGL2-7.1.so.7.1p
#10 0x0000fffff3e12400 in vtkOpenGLPolyDataMapper::BuildIBO(vtkRenderer*, vtkActor*, vtkPolyData*) ()
at /usr/lib/aarch64-linux-gnu/libvtkRenderingOpenGL2-7.1.so.7.1p
#11 0x0000fffff3e15780 in vtkOpenGLPolyDataMapper::BuildBufferObjects(vtkRenderer*, vtkActor*) ()
at /usr/lib/aarch64-linux-gnu/libvtkRenderingOpenGL2-7.1.so.7.1p
--Type for more, q to quit, c to continue without paging--
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
After compare the fitness score value in the benchmark, can I get the conclusion, the bigger this value, the better the algorithm are?
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.
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 functionpclomp::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 functionpclomp::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 topclomp::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!
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
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.
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.
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?
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 @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,
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!
[ 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
koide3/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
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
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?
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?
thank you for your contribution, i don't know what is difference between DIRECT7 、DIRECT1 and KDTREE ?
@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?
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?
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!
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!!
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
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.
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();
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
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.
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.
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 ?
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.
段错误 (核心已转储)
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
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;
}
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()];
hello,
when I update inputTargetCloud, it occurs, why?
Can someone help me? Thanks a lot.
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
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?
@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!
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
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.