SeungBack/open3d-ros-helper

Helper for jointly using open3d and numpy in ROS

open3d-ros-helper

• Helper for jointly using open3d and ROS
• Easy conversion between ROS and open3d point cloud (supports both XYZ & XYZRGB point cloud)
• Easy conversion between ROS pose and transform

Dependencies

• python 2.7
• ros-numpy
• open3d == 0.9

Installation

$ sudo apt install ros-melodic-ros-numpy
$ pip2 install numpy open3d==0.9.0 opencv-python==4.2.0.32 pyrsistent==0.13
$ pip2 install open3d_ros_helper

Usage

Import open3d-ros-helper

from open3d_ros_helper import open3d_ros_helper as orh

Convert 4x4 SE(3) to geometry_msgs/Transform

import numpy as np
se3 = np.eye(4)
ros_transform = orh.se3_to_transform(se3) 

Convert sensor.msg.PointCloud2 to open3d.geometry.PointCloud

o3dpc = orh.rospc_to_o3dpc(some_ros_pointcloud) 

Convert open3d.geometry.PointCloud to sensor.msg.PointCloud2

rospc = orh.rospc_to_o3dpc(o3dpc) 

Authors

• Seunghyeok Back seungback

License

This project is licensed under the MIT License

References

Some codes are rewritten from

• pcl_herlper
• conversion b/w ros transforms
• averaging-quaternion

Documentation

Table of contents

• pose_to_pq
• pose_stamped_to_pq
• transform_to_pq
• transform_stamped_to_pq
• msg_to_se3
• pq_to_transform
• pq_to_transform_stamped
• se3_to_transform
• se3_to_transform_stamped
• average_q
• average_pq
• rospc_to_o3dpc
• o3dpc_to_rospc
• do_transform_point
• apply_pass_through_filter
• crop_with_2dmask
• p2p_icp_registration
• ppf_icp_registration

pose_to_pq

pose_to_pq(pose)

convert a ROS PoseS message into position/quaternion np arrays

Arguments:

• pose geometry_msgs/Pose - ROS geometric message to be converted

Returns:

• p np.array - position array of [x, y, z]
• q np.array - quaternion array of [x, y, z, w] source codes from https://answers.ros.org/question/332407/transformstamped-to-transformation-matrix-python/

pose_stamped_to_pq

pose_stamped_to_pq(pose_stamped)

convert a ROS PoseStamped message into position/quaternion np arrays

Arguments:

• pose_stamped geometry_msgs/PoseStamped - ROS geometric message to be converted

Returns:

• p np.array - position array of [x, y, z]
• q np.array - quaternion array of [x, y, z, w] source codes from https://answers.ros.org/question/332407/transformstamped-to-transformation-matrix-python/

transform_to_pq

transform_to_pq(transform)

convert a ROS Transform message into position/quaternion np arrays

Arguments:

• transform geometry_msgs/Transform - ROS geometric message to be converted

Returns:

• p np.array - position array of [x, y, z]
• q np.array - quaternion array of [x, y, z, w] source codes from https://answers.ros.org/question/332407/transformstamped-to-transformation-matrix-python/

transform_stamped_to_pq

transform_stamped_to_pq(transform_stamped)

convert a ROS TransformStamped message into position/quaternion np arrays

Arguments:

• transform_stamped geometry_msgs/TransformStamped - ROS geometric message to be converted

Returns:

• p np.array - position array of [x, y, z]
• q np.array - quaternion array of [x, y, z, w] source codes from https://answers.ros.org/question/332407/transformstamped-to-transformation-matrix-python/

msg_to_se3

msg_to_se3(msg)

convert geometric ROS messages to SE(3)

Arguments:

msg (geometry_msgs/Pose, geometry_msgs/PoseStamped, geometry_msgs/Transform, geometry_msgs/TransformStamped): ROS geometric messages to be converted

Returns:

• se3 np.array - a 4x4 SE(3) matrix as a numpy array source codes from https://answers.ros.org/question/332407/transformstamped-to-transformation-matrix-python/

pq_to_transform

pq_to_transform(p, q)

convert position, quaternion to geometry_msgs/Transform

Arguments:

• p np.array - position array of [x, y, z]
• q np.array - quaternion array of [x, y, z, w]

Returns:

• transform geometry_msgs/Transform - ROS transform of given p and q

pq_to_transform_stamped

pq_to_transform_stamped(p, q, source_frame, target_frame, stamp=None)

convert position, quaternion to geometry_msgs/TransformStamped

Arguments:

• p np.array - position array of [x, y, z]
• q np.array - quaternion array of [x, y, z, w]
• source_frame string - name of tf source frame
• target_frame string - name of tf target frame

Returns:

• transform_stamped geometry_msgs/TransformStamped - ROS transform_stamped of given p and q

se3_to_transform

se3_to_transform(transform_nparray)

convert 4x4 SE(3) to geometry_msgs/Transform

Arguments:

• transform_nparray np.array - 4x4 SE(3)

Returns:

• transform geometry_msgs/Transform - ROS transform of given SE(3)

se3_to_transform_stamped

se3_to_transform_stamped(transform_nparray, source_frame, target_frame, stamp=None)

convert 4x4 SE(3) to geometry_msgs/TransformStamped

Arguments:

• transform_nparray np.array - 4x4 SE(3)
• source_frame string - name of tf source frame
• target_frame string - name of tf target frame

Returns:

• transform_stamped geometry_msgs/TransformStamped - ROS transform_stamped of given SE(3)

average_q

average_q(qs)

calculate the average of quaternions

Arguments:

• qs np.array - multiple quaternion array of shape Nx4

Returns:

• q_average np.array - averaged quaternion array source codes from https://github.com/christophhagen/averaging-quaternions

average_pq

average_pq(ps, qs)

average the multiple position and quaternion array

Arguments:

• ps np.array - multiple position array of shape Nx3
• qs np.array - multiple quaternion array of shape Nx4

Returns:

• p_mean np.array - averaged position array
• q_mean np.array - averaged quaternion array

rospc_to_o3dpc

rospc_to_o3dpc(rospc, remove_nans=False)

covert ros point cloud to open3d point cloud

Arguments:

• rospc sensor.msg.PointCloud2 - ros point cloud message
• remove_nans bool - if true, ignore the NaN points

Returns:

• o3dpc open3d.geometry.PointCloud - open3d point cloud

o3dpc_to_rospc

o3dpc_to_rospc(o3dpc, frame_id=None, stamp=None)

convert open3d point cloud to ros point cloud

Arguments:

• o3dpc open3d.geometry.PointCloud - open3d point cloud
• frame_id string - frame id of ros point cloud header
• stamp rospy.Time - time stamp of ros point cloud header

Returns:

• rospc sensor.msg.PointCloud2 - ros point cloud message

do_transform_point

do_transform_point(o3dpc, transform_stamped)

transform a input cloud with respect to the specific frame open3d version of tf2_geometry_msgs.do_transform_point

Arguments:

• o3dpc open3d.geometry.PointCloud - open3d point cloud
• transform_stamped geometry_msgs.msgs.TransformStamped - transform to be applied

Returns:

• o3dpc open3d.geometry.PointCloud - transformed open3d point cloud

apply_pass_through_filter

apply_pass_through_filter(o3dpc, x_range, y_range, z_range)

apply 3D pass through filter to the open3d point cloud

Arguments:

• o3dpc open3d.geometry.PointCloud - open3d point cloud
• x_range list - list of [x_min, x_maz]
• y_range list - list of [y_min, y_maz]
• z_range list - list of [z_min, z_max]

Returns:

• o3dpc open3d.geometry.PointCloud - filtered open3d point cloud some codes from https://github.com/powersimmani/example_3d_pass_through-filter_guide

crop_with_2dmask

crop_with_2dmask(o3dpc, mask)

crop open3d point cloud with given 2d binary mask

Arguments:

• o3dpc open3d.geometry.PointCloud - open3d point cloud
• mask np.array - binary mask aligned with the point cloud frame

Returns:

• o3dpc open3d.geometry.PointCloud - filtered open3d point cloud

p2p_icp_registration

p2p_icp_registration(source_cloud, target_cloud, n_points=100, threshold=0.02, relative_fitness=1e-10, relative_rmse=1e-8, max_iteration=500, max_correspondence_distance=500)

align the source cloud to the target cloud using point-to-point ICP registration algorithm

Arguments:

• source_cloud open3d.geometry.PointCloud - source open3d point cloud
• target_cloud open3d.geometry.PointCloud - target open3d point cloud for other parameter, go to http://www.open3d.org/docs/0.9.0/python_api/open3d.registration.registration_icp.html

Returns:

• icp_result open3d.registration.RegistrationResult - registration result

ppf_icp_registration

ppf_icp_registration(source_cloud, target_cloud, n_points=3000, n_iter=100, tolerance=0.05, num_levels=5)

align the source cloud to the target cloud using point pair feature (PPF) match

Arguments:

• source_cloud open3d.geometry.PointCloud - source open3d point cloud
• target_cloud open3d.geometry.PointCloud - target open3d point cloud for other parameter, go to https://docs.opencv.org/master/dc/d9b/classcv_1_1ppf__match__3d_1_1ICP.html

Returns:

• pose np.array - 4x4 transformation between source and targe cloud
• residual float - the output resistration error