url-kaist / patchwork-plusplus-ros

ROS & ROS2 Implementation of Patchwork++

patchwork-plusplus-ros

This is ROS package of Patchwork++ (@ IROS'22), which is a fast and robust ground segmentation method.

If you are not familiar with ROS, please visit the original repository.

If you follow the repository, you can run Patchwork++ in Python and C++ easily.

📂 What's in this repository

• ROS based Patchwork source code (patchworkpp.hpp)
• Demo launch file (demo.launch) with sample rosbag file. You can execute Patchwork++ simply!

📦 Prerequisite packages

You may need to install ROS, PCL, Eigen, ...

⚙️ How to build Patchwork++

To build Patchwork++, you can follow below codes.

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws
$ catkin build # or catkin_make

🏃 To run the demo codes

There is a demo which executes Patchwork++ with sample rosbag file. You can download a sample file with the following command.

For the sample rosbag data, I utilizes semantickitti2bag package.

$ wget https://urserver.kaist.ac.kr/publicdata/patchwork++/kitti_00_sample.bag

If you have any trouble to download the file by the above command, please click here to download the file directly.

The rosbag file is based on the KITTI dataset. The bin files are merged into the rosbag file format.

The sample file contains LiDAR sensor data only.

Then, you can run demo as follows.

# Start Patchwork++
$ roslaunch patchworkpp demo.launch
# Start the bag file
$ rosbag play kitti_00_sample.bag

📌 TODO List

• Update additional demo codes processing data with .bin file format
• Generalize point type in the source code
• Add visualization result of demo codes in readme

Citation

If you use our codes, please cite our paper.

In addition, you can also check the paper of our baseline(Patchwork) here.

@inproceedings{lee2022patchworkpp,
    title={{Patchwork++: Fast and robust ground segmentation solving partial under-segmentation using 3D point cloud}},
    author={Lee, Seungjae and Lim, Hyungtae and Myung, Hyun},
    booktitle={Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst.},
    year={2022},
    note={{Submitted}} 
}

@article{lim2021patchwork,
    title={Patchwork: Concentric Zone-based Region-wise Ground Segmentation with Ground Likelihood Estimation Using a 3D LiDAR Sensor},
    author={Lim, Hyungtae and Minho, Oh and Myung, Hyun},
    journal={IEEE Robotics and Automation Letters},
    year={2021}
}

📮 Contact

If you have any question, don't be hesitate let us know!

• Seungjae Lee ✉️ (sj98lee at kaist.ac.kr)
• Hyungtae Lim ✉️ (shapelim at kaist.ac.kr)

patchwork-plusplus-ros's Issues

What is the version of Open3D

Hi, what is the version of Open3D, thanks ~

Is there any plan to update this repo?

As the Basic repo describes, there had commit Modify RNR to be available and R-VPF to be correctly operated, is there any play to update to this repo?

process has died [pid 12628, exit code -11 cmd ......]

what can we deal with this condition, we use our own lidar data, it can only run about 13 seconds and the process has died

segmentation fault during estimate_ground

segmentation fault during estimate_ground

I can only run for about 20 seconds on a bagged pointcloud. I ran with valgrind to help pinpoint any issues.

I've found it seems to happen right after flush_patches and before before calling pc2czm
https://github.com/url-kaist/patchwork-plusplus-ros/blob/master/include/patchworkpp/patchworkpp.hpp#L487

I am slightly forked but only whitespace and ros namespace changes. See #10

==388489== Thread 1:
==388489== Use of uninitialised value of size 8
==388489==    at 0x1637FA: PatchWorkpp<pcl::PointXYZI>::estimate_ground(pcl::PointCloud<pcl::PointXYZI>, pcl::PointCloud<pcl::PointXYZI>&, pcl::PointCloud<pcl::PointXYZI>&, double&) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x12B765: callbackCloud(boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > const&) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x14E891: boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > const&)>, void, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > >) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x16870C: ros::SubscriptionCallbackHelperT<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > const&, void>::call(ros::SubscriptionCallbackHelperCallParams&) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x4D43138: ros::SubscriptionQueue::call() (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4CF1171: ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4CF2882: ros::CallbackQueue::callAvailable(ros::WallDuration) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x124D64: main (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489== 
==388489== Invalid read of size 4
==388489==    at 0x1637FA: PatchWorkpp<pcl::PointXYZI>::estimate_ground(pcl::PointCloud<pcl::PointXYZI>, pcl::PointCloud<pcl::PointXYZI>&, pcl::PointCloud<pcl::PointXYZI>&, double&) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x12B765: callbackCloud(boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > const&) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x14E891: boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > const&)>, void, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > >) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x16870C: ros::SubscriptionCallbackHelperT<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > const&, void>::call(ros::SubscriptionCallbackHelperCallParams&) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x4D43138: ros::SubscriptionQueue::call() (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4CF1171: ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4CF2882: ros::CallbackQueue::callAvailable(ros::WallDuration) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x124D64: main (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==  Address 0x0 is not stack'd, malloc'd or (recently) free'd
==388489== 
==388489== 
==388489== Process terminating with default action of signal 11 (SIGSEGV)
==388489==  Access not within mapped region at address 0x0
==388489==    at 0x1637FA: PatchWorkpp<pcl::PointXYZI>::estimate_ground(pcl::PointCloud<pcl::PointXYZI>, pcl::PointCloud<pcl::PointXYZI>&, pcl::PointCloud<pcl::PointXYZI>&, double&) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x12B765: callbackCloud(boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > const&) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x14E891: boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > const&)>, void, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > >) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x16870C: ros::SubscriptionCallbackHelperT<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > > const&, void>::call(ros::SubscriptionCallbackHelperCallParams&) (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==    by 0x4D43138: ros::SubscriptionQueue::call() (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4CF1171: ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4CF2882: ros::CallbackQueue::callAvailable(ros::WallDuration) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x124D64: main (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489==  If you believe this happened as a result of a stack
==388489==  overflow in your program's main thread (unlikely but
==388489==  possible), you can try to increase the size of the
==388489==  main thread stack using the --main-stacksize= flag.
==388489==  The main thread stack size used in this run was 8388608.
pc2czm - 4392 / ==388489== 
==388489== HEAP SUMMARY:
==388489==     in use at exit: 14,689,441 bytes in 3,827 blocks
==388489==   total heap usage: 230,399 allocs, 226,572 frees, 410,519,582 bytes allocated
==388489== 
==388489== 140 bytes in 1 blocks are definitely lost in loss record 1,729 of 2,008
==388489==    at 0x483B7F3: malloc (in /usr/lib/x86_64-linux-gnu/valgrind/vgpreload_memcheck-amd64-linux.so)
==388489==    by 0x521DF54: __libc_alloc_buffer_allocate (alloc_buffer_allocate.c:26)
==388489==    by 0x52C1628: alloc_buffer_allocate (alloc_buffer.h:143)
==388489==    by 0x52C1628: __resolv_conf_allocate (resolv_conf.c:411)
==388489==    by 0x52BEE21: __resolv_conf_load (res_init.c:592)
==388489==    by 0x52C1232: __resolv_conf_get_current (resolv_conf.c:163)
==388489==    by 0x52BF3D4: __res_vinit (res_init.c:614)
==388489==    by 0x52C05CF: maybe_init (resolv_context.c:122)
==388489==    by 0x52C05CF: context_get (resolv_context.c:184)
==388489==    by 0x52C05CF: context_get (resolv_context.c:176)
==388489==    by 0x52C05CF: __resolv_context_get (resolv_context.c:195)
==388489==    by 0x5283880: gaih_inet.constprop.0 (getaddrinfo.c:747)
==388489==    by 0x5284F58: getaddrinfo (getaddrinfo.c:2256)
==388489==    by 0x647BE76: XmlRpc::XmlRpcSocket::connect(int, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int) (in /opt/ros/noetic/lib/libxmlrpcpp.so)
==388489==    by 0x647450E: XmlRpc::XmlRpcClient::doConnect() (in /opt/ros/noetic/lib/libxmlrpcpp.so)
==388489==    by 0x64743FB: XmlRpc::XmlRpcClient::setupConnection() (in /opt/ros/noetic/lib/libxmlrpcpp.so)
==388489== 
==388489== 368 bytes in 1 blocks are possibly lost in loss record 1,837 of 2,008
==388489==    at 0x483DD99: calloc (in /usr/lib/x86_64-linux-gnu/valgrind/vgpreload_memcheck-amd64-linux.so)
==388489==    by 0x40149DA: allocate_dtv (dl-tls.c:286)
==388489==    by 0x40149DA: _dl_allocate_tls (dl-tls.c:532)
==388489==    by 0x4896322: allocate_stack (allocatestack.c:622)
==388489==    by 0x4896322: pthread_create@@GLIBC_2.2.5 (pthread_create.c:660)
==388489==    by 0x64A9441: boost::thread::start_thread_noexcept() (in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.71.0)
==388489==    by 0x4CDC7B7: ros::PollManager::start() (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D2FE4C: ros::start() (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D0273E: ros::NodeHandle::construct(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D02A12: ros::NodeHandle::NodeHandle(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x124A51: main (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489== 
==388489== 368 bytes in 1 blocks are possibly lost in loss record 1,838 of 2,008
==388489==    at 0x483DD99: calloc (in /usr/lib/x86_64-linux-gnu/valgrind/vgpreload_memcheck-amd64-linux.so)
==388489==    by 0x40149DA: allocate_dtv (dl-tls.c:286)
==388489==    by 0x40149DA: _dl_allocate_tls (dl-tls.c:532)
==388489==    by 0x4896322: allocate_stack (allocatestack.c:622)
==388489==    by 0x4896322: pthread_create@@GLIBC_2.2.5 (pthread_create.c:660)
==388489==    by 0x64A9441: boost::thread::start_thread_noexcept() (in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.71.0)
==388489==    by 0x4CC55DB: ros::XMLRPCManager::start() (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D2FE59: ros::start() (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D0273E: ros::NodeHandle::construct(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D02A12: ros::NodeHandle::NodeHandle(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x124A51: main (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489== 
==388489== 368 bytes in 1 blocks are possibly lost in loss record 1,839 of 2,008
==388489==    at 0x483DD99: calloc (in /usr/lib/x86_64-linux-gnu/valgrind/vgpreload_memcheck-amd64-linux.so)
==388489==    by 0x40149DA: allocate_dtv (dl-tls.c:286)
==388489==    by 0x40149DA: _dl_allocate_tls (dl-tls.c:532)
==388489==    by 0x4896322: allocate_stack (allocatestack.c:622)
==388489==    by 0x4896322: pthread_create@@GLIBC_2.2.5 (pthread_create.c:660)
==388489==    by 0x64A9441: boost::thread::start_thread_noexcept() (in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.71.0)
==388489==    by 0x4D2BF9E: ros::ROSOutAppender::ROSOutAppender() (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D307D4: ros::start() (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D0273E: ros::NodeHandle::construct(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D02A12: ros::NodeHandle::NodeHandle(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x124A51: main (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489== 
==388489== 368 bytes in 1 blocks are possibly lost in loss record 1,840 of 2,008
==388489==    at 0x483DD99: calloc (in /usr/lib/x86_64-linux-gnu/valgrind/vgpreload_memcheck-amd64-linux.so)
==388489==    by 0x40149DA: allocate_dtv (dl-tls.c:286)
==388489==    by 0x40149DA: _dl_allocate_tls (dl-tls.c:532)
==388489==    by 0x4896322: allocate_stack (allocatestack.c:622)
==388489==    by 0x4896322: pthread_create@@GLIBC_2.2.5 (pthread_create.c:660)
==388489==    by 0x64A9441: boost::thread::start_thread_noexcept() (in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.71.0)
==388489==    by 0x4D33EAC: boost::thread::thread<void (&)()>(void (&)()) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D3056F: ros::start() (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D0273E: ros::NodeHandle::construct(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x4D02A12: ros::NodeHandle::NodeHandle(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&) (in /opt/ros/noetic/lib/libroscpp.so)
==388489==    by 0x124A51: main (in catkin_ws/devel/.private/patchworkpp/lib/patchworkpp/demo)
==388489== 
==388489== LEAK SUMMARY:
==388489==    definitely lost: 140 bytes in 1 blocks
==388489==    indirectly lost: 0 bytes in 0 blocks
==388489==      possibly lost: 1,472 bytes in 4 blocks
==388489==    still reachable: 14,687,829 bytes in 3,822 blocks
==388489==         suppressed: 0 bytes in 0 blocks
==388489== Reachable blocks (those to which a pointer was found) are not shown.
==388489== To see them, rerun with: --leak-check=full --show-leak-kinds=all
==388489== 
==388489== Use --track-origins=yes to see where uninitialised values come from
==388489== For lists of detected and suppressed errors, rerun with: -s
==388489== ERROR SUMMARY: 669212 errors from 11 contexts (suppressed: 0 from 0)

why

Why the sum of ground points and non-ground points obtained using patchwork-plusplus-rosis not equal to the original point cloud,but patchwork-plusplus has not this problem?

some suggestions: you need to modify the frame_id "/map" to "map", since it would cause some probs

When I run the 'demo.launch', rviz visualizer cannot output the cloud processed by 'patchworkpp'.
After I change the frame_ids, the system can run!

Could you please put rosbag on other platform such as google drive

Hi,l use the command wget but always mention Unable to establish SSL connection..Could you please put rosbag on other platform such as google drive?

High CPU usage when no data input

Hello,
Thank you for sharing such a wonderful job. I have a question now, it seems that the program also consumes a large amount of CPU resources without data input (with one CPU core consistently occupying 100%). Why is this? Is there a way to reduce CPU usage? (CPU: I7-7800k, no data input occupation: 12%, input point cloud scale 45312 pts: 14%)
Thank you and look forward to your answer!

Depth camera adaptation

thank you for your work

I would like to know:

1. Can this algorithm be adapted to depth cameras?
2. If so, what should you pay attention to?

Looking forward to your reply, thank you!

patchwork++ has more poor segmentation than patchwork

Thank you for contribution!
When I run this project, I found that patchwork ground segmentation is better than patchwork++(on kitti_0_sample. bag), which seems to be different from what is claimed in the paper. Why? Both use default parameters.

Similar to the poor segmentation of patchwork++ in the figure above, there are many cases.

Any plans to provide support for ROS2?

I would like to know if you have any plans of creating another branch to provide support for ROS2. It can be very helpful for people trying to use this with ROS2.

low accurancy in mine area

Hello, can I share my bag with you and help me take a look? Thank you very much, I am very confused about this for a long time.

low accurancy

Hello. Looks like this package is not correctly recognized ground in mountains

Apply to other multi-beam lidar sensors

Thanks a lot for sharing your excellent work! If I want to test this project with our own lidar sensors (such as robosense 128-beams rs-ruby lidar), what params need to be adjusted? I found the param settings in the config folder, and in the yaml file, sensor_height should be reset according to our platform, are there any other parameters that need to be adjusted? Looking forword to your reply.

About Detecting Negative Objects as Non Ground?

Hello, I've been following you since patchwork v1. Congratulations on your successful work.

I would like to use your algorithm to also detect negative objects such as high slopes, holes, and small cliffs as non-ground. Which parameters should I change?
My autonomous vehicle specifically perceives the road as 'ground' when it's on the sidewalk. There is a 45 cm difference between the sidewalk and the road.

ROS2

In the current ROS2 branch the param max_flatness_storage_ and max_elevation_treshold_ are never set. https://github.com/url-kaist/patchwork-plusplus-ros/blob/15a00dafab479fb86e25416f31ba5cccd559cfa1/include/patchworkpp/patchworkpp.hpp#L235C5-L235C55

This results in runtime errors. In think here is a line, where the default value for an undefined int causes trouble:

patchwork-plusplus-ros/include/patchworkpp/patchworkpp.hpp

Line 798 in 15a00da

int exceed_num = update_flatness_[i].size() - max_flatness_storage_;

Poor Results on Kitti Sequence 00

Hello,

I use the demo.launch without changing any parameters and test on the 00 sequence of kitti dataset. I get results like shown in fig below. These results do not look like the gif on the main page. Any ideas what I am doing wrong?

Best,
Haider