[jsk_pcl_ros] ICP Registration on 2D plane

[furushchev]

Sorry for delay.
I implemented from scratch but there is totally same algorithm in pcl to achieve 2d transform estimation.
Cleaned up codes, updated docs with images.

plane is constrained from header frame coordinates of point cloud.
e.g. to fitting to objects on a table, you will need:

0. assumed that point cloud is published from sensor tf frame.
1. extract plane of table and broadcast a normal point of table surface as tf. (let it be table_frame)
2. use TfPointCloud to change frame id from sensor frame to table_frame
3. apply icp with setting param transform_3dof to true.

[k-okada]

can we add launch file for this? and if we can add test code, that more better.

[k-okada]

@furushchev : can we add launch file for this? and if we can add test code, that more better.

[mmurooka]

Source code diff LGTM.

Please do:

@furushchev : can we add launch file for this? and if we can add test code, that more better.

[YoheiKakiuchi]

ping @furushchev

[furushchev]

@YoheiKakiuchi Sorry for delay. I'm working on this now. Please wait a moment...

[furushchev]

added sample launch file for ICP 2D

roslaunch openni2_launch openni2.launch
roseus cylinder_cloud_publisher.l # publish cylinder cloud to topic /reference_cloud
roslaunch jsk_pcl_ros sample_icp_registration_2d.launch

To execute this launch file, #2038, #2039 will be needed.

To publish cylinder reference as pointcloud, the node will be like below code:
(note that to execute this lisp code, euslisp/jskeus#425 is necessary)

;; cylinder_cloud_publisher.l
;; Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>


(ros::roseus "cylinder_cloud_publisher")

(ros::advertise "/reference_cloud" sensor_msgs::PointCloud2 1 t) ;; latched
(unix:sleep 1)

(setq *cylinder* (make-cylinder 55 300))
(setq *cloud* (object->pointcloud *cylinder* :step 30 :with-normal t :with-color t))
(setq *msg* (make-ros-msg-from-eus-pointcloud *cloud* :frame "camera_link"))

(ros::rate 1)
(while (ros::ok)
  (send *msg* :header :stamp (ros::time-now))
  (ros::publish "/reference_cloud" *msg*)
  (ros::spin-once)
  (ros::sleep))

[furushchev]

I have a rosbag of picture attached above. If anyone give an advice to upload, I will write a test code.

[k-okada]

I have a rosbag of picture attached above. If anyone give an advice to upload,

@wkentaro wiill give you an advice to upload

[wkentaro]

$ tar zcvf sample_icp_registration_2d.bag.tgz sample_icp_registration_2d.bag
$ jsk_data put --public sample_icp_registration_2d.bag.tgz
$ jsk_data pubinfo sample_icp_registration_2d.bag.tgz
# it shows download url
$ md5sum sample_icp_registration_2d.bag.tgz
# copy this to md5 in download_data

# edit scripts/install_sample_data.py
...
    download_data(
        pkg_name=PKG,
        path='sample/data/sample_icp_registration_2d.bag.tgz',
        url='https://drive.google.com/uc?id=XXXXXXXXXXXXXXXx',
        md5='xxxxxxxxxxxxxxxxxxxxxxxxxxxxx',
        extract=True,
    )

[furushchev]

@k-okada I'm now back.

@furushchev : can we add launch file for this? and if we can add test code, that more better.

Added sample launch file and test code.