Demo of usage:
ROOT=$(rospack find ros_detect_planes_from_depth_img)
rosrun ros_detect_planes_from_depth_img run_server.py \
--config_file $ROOT/config/plane_detector_config.yaml \
--depth_topic test_data/depth \
--color_topic test_data/color \
--camera_info $ROOT/data/cam_params_realsense.json
The main scripts are: plane_detector.py, run_server.py, and config/plane_detector_config.yaml.
For a fast unit test, run: bash run_unit_test.sh
Method:
(1) Create point cloud from depth image
(2) while RANSAC hasn't failed:
(3) Use RANSAC to detect a plane from point cloud.
(4) Remove the plane's points from point cloud.
Contents:
$ rostopic list
/detect_plane/colored_mask
/detect_plane/image_viz
/detect_plane/results
The plane detection results are published to the topic detect_plane/results
, whose message type is msg/PlanesResults.msg:
int32 N # Number of detected planes.
# In the following arrays,
# the planes' parameters are concatinated one by one.
float32[] norms # Nx3. Plane normal (nx, ny, nz).
float32[] center_3d # Nx3. Plane 3D center (cx, cy, cz).
float32[] center_2d # Nx2. Plane 2D center on the image (px, py),
# which means {px}th column, and {py}th row.
uint16[] mask_color # Nx3. Plane mask color (blue, green, red).
# Each color's range is [0, 255]
python plane_detector.py
Please download another package for publishing color and depth images:
cd ~/catkin_ws/src
git clone https://github.com/felixchenfy/ros_pub_and_sub_rgbd_and_cloud
Then, we can publish the data:
ROOT=$(rospack find ros_detect_planes_from_depth_img)
rosrun ros_pub_and_sub_rgbd_and_cloud pub_rgbd_and_cloud.py \
--base_dir $ROOT \
--config_file $ROOT/config/pub_rgbd_config.yaml
The settings are in config/pub_rgbd_images_config.yaml
:
ROOT=$(rospack find ros_detect_planes_from_depth_img)
rosrun ros_detect_planes_from_depth_img run_server.py \
--config_file $ROOT/config/plane_detector_config.yaml \
--depth_topic test_data/depth \
--color_topic test_data/color \
--camera_info $ROOT/data/cam_params_realsense.json
ROOT=$(rospack find ros_detect_planes_from_depth_img)
rosrun rviz rviz -d $ROOT/config/rviz_for_test_data.rviz