/open3d-ros-helper

Helper for jointly using open3d and numpy in ROS

Primary LanguagePythonMIT LicenseMIT

open3d-ros-helper

Maintenance PyPI version PyPI license Hits Python 2.7 contributions welcome

  • 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

License

This project is licensed under the MIT License

References

Some codes are rewritten from

Documentation

Table of contents

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:

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:

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:

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:

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:

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:

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:

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:

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:

Returns:

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