Package: open3d-ros-helper
open3d-ros-helperHelper for jointly using open3d and ROSEasy conversion between ROS and open3d point cloud (supports both XYZ & XYZRGB point cloud)Easy conversion between ROS pose and transformDependenciespython 2.7ros-numpyopen3d == 0.9Installation$ 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_helperUsageImportopen3d-ros-helperfrom open3d_ros_helper import open3d_ros_helper as orhConvert4x4 SE(3)togeometry_msgs/Transformimport numpy as np
se3 = np.eye(4)
ros_transform = orh.se3_to_transform(se3)Convertsensor.msg.PointCloud2toopen3d.geometry.PointCloudo3dpc = orh.rospc_to_o3dpc(some_ros_pointcloud)Convertopen3d.geometry.PointCloudtosensor.msg.PointCloud2rospc = orh.rospc_to_o3dpc(o3dpc)AuthorsSeunghyeok BackseungbackLicenseThis project is licensed under the MIT LicenseReferencesSome codes are rewritten frompcl_herlperconversion b/w ros transformsaveraging-quaternionDocumentationTable of contentspose_to_pqpose_stamped_to_pqtransform_to_pqtransform_stamped_to_pqmsg_to_se3pq_to_transformpq_to_transform_stampedse3_to_transformse3_to_transform_stampedaverage_qaverage_pqrospc_to_o3dpco3dpc_to_rospcdo_transform_pointapply_pass_through_filtercrop_with_2dmaskp2p_icp_registrationppf_icp_registrationpose_to_pqpose_to_pq(pose)convert a ROS PoseS message into position/quaternion np arraysArguments:posegeometry_msgs/Pose- ROS geometric message to be convertedReturns:pnp.array- position array of [x, y, z]qnp.array- quaternion array of [x, y, z, w]
source codes fromhttps://answers.ros.org/question/332407/transformstamped-to-transformation-matrix-python/pose_stamped_to_pqpose_stamped_to_pq(pose_stamped)convert a ROS PoseStamped message into position/quaternion np arraysArguments:pose_stampedgeometry_msgs/PoseStamped- ROS geometric message to be convertedReturns:pnp.array- position array of [x, y, z]qnp.array- quaternion array of [x, y, z, w]
source codes fromhttps://answers.ros.org/question/332407/transformstamped-to-transformation-matrix-python/transform_to_pqtransform_to_pq(transform)convert a ROS Transform message into position/quaternion np arraysArguments:transformgeometry_msgs/Transform- ROS geometric message to be convertedReturns:pnp.array- position array of [x, y, z]qnp.array- quaternion array of [x, y, z, w]
source codes fromhttps://answers.ros.org/question/332407/transformstamped-to-transformation-matrix-python/transform_stamped_to_pqtransform_stamped_to_pq(transform_stamped)convert a ROS TransformStamped message into position/quaternion np arraysArguments:transform_stampedgeometry_msgs/TransformStamped- ROS geometric message to be convertedReturns:pnp.array- position array of [x, y, z]qnp.array- quaternion array of [x, y, z, w]
source codes fromhttps://answers.ros.org/question/332407/transformstamped-to-transformation-matrix-python/msg_to_se3msg_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 convertedReturns:se3np.array- a 4x4 SE(3) matrix as a numpy array
source codes fromhttps://answers.ros.org/question/332407/transformstamped-to-transformation-matrix-python/pq_to_transformpq_to_transform(p,q)convert position, quaternion to geometry_msgs/TransformArguments:pnp.array- position array of [x, y, z]qnp.array- quaternion array of [x, y, z, w]Returns:transformgeometry_msgs/Transform- ROS transform of given p and qpq_to_transform_stampedpq_to_transform_stamped(p,q,source_frame,target_frame,stamp=None)convert position, quaternion to geometry_msgs/TransformStampedArguments:pnp.array- position array of [x, y, z]qnp.array- quaternion array of [x, y, z, w]source_framestring- name of tf source frametarget_framestring- name of tf target frameReturns:transform_stampedgeometry_msgs/TransformStamped- ROS transform_stamped of given p and qse3_to_transformse3_to_transform(transform_nparray)convert 4x4 SE(3) to geometry_msgs/TransformArguments:transform_nparraynp.array- 4x4 SE(3)Returns:transformgeometry_msgs/Transform- ROS transform of given SE(3)se3_to_transform_stampedse3_to_transform_stamped(transform_nparray,source_frame,target_frame,stamp=None)convert 4x4 SE(3) to geometry_msgs/TransformStampedArguments:transform_nparraynp.array- 4x4 SE(3)source_framestring- name of tf source frametarget_framestring- name of tf target frameReturns:transform_stampedgeometry_msgs/TransformStamped- ROS transform_stamped of given SE(3)average_qaverage_q(qs)calculate the average of quaternionsArguments:qsnp.array- multiple quaternion array of shape Nx4Returns:q_averagenp.array- averaged quaternion array
source codes fromhttps://github.com/christophhagen/averaging-quaternionsaverage_pqaverage_pq(ps,qs)average the multiple position and quaternion arrayArguments:psnp.array- multiple position array of shape Nx3qsnp.array- multiple quaternion array of shape Nx4Returns:p_meannp.array- averaged position arrayq_meannp.array- averaged quaternion arrayrospc_to_o3dpcrospc_to_o3dpc(rospc,remove_nans=False)covert ros point cloud to open3d point cloudArguments:rospcsensor.msg.PointCloud2- ros point cloud messageremove_nansbool- if true, ignore the NaN pointsReturns:o3dpcopen3d.geometry.PointCloud- open3d point cloudo3dpc_to_rospco3dpc_to_rospc(o3dpc,frame_id=None,stamp=None)convert open3d point cloud to ros point cloudArguments:o3dpcopen3d.geometry.PointCloud- open3d point cloudframe_idstring- frame id of ros point cloud headerstamprospy.Time- time stamp of ros point cloud headerReturns:rospcsensor.msg.PointCloud2- ros point cloud messagedo_transform_pointdo_transform_point(o3dpc,transform_stamped)transform a input cloud with respect to the specific frame
open3d version of tf2_geometry_msgs.do_transform_pointArguments:o3dpcopen3d.geometry.PointCloud- open3d point cloudtransform_stampedgeometry_msgs.msgs.TransformStamped- transform to be appliedReturns:o3dpcopen3d.geometry.PointCloud- transformed open3d point cloudapply_pass_through_filterapply_pass_through_filter(o3dpc,x_range,y_range,z_range)apply 3D pass through filter to the open3d point cloudArguments:o3dpcopen3d.geometry.PointCloud- open3d point cloudx_rangelist- list of [x_min, x_maz]y_rangelist- list of [y_min, y_maz]z_rangelist- list of [z_min, z_max]Returns:o3dpcopen3d.geometry.PointCloud- filtered open3d point cloud
some codes fromhttps://github.com/powersimmani/example_3d_pass_through-filter_guidecrop_with_2dmaskcrop_with_2dmask(o3dpc,mask)crop open3d point cloud with given 2d binary maskArguments:o3dpcopen3d.geometry.PointCloud- open3d point cloudmasknp.array- binary mask aligned with the point cloud frameReturns:o3dpcopen3d.geometry.PointCloud- filtered open3d point cloudp2p_icp_registrationp2p_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 algorithmArguments:source_cloudopen3d.geometry.PointCloud- source open3d point cloudtarget_cloudopen3d.geometry.PointCloud- target open3d point cloud
for other parameter, go tohttp://www.open3d.org/docs/0.9.0/python_api/open3d.registration.registration_icp.htmlReturns:icp_resultopen3d.registration.RegistrationResult- registration resultppf_icp_registrationppf_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) matchArguments:source_cloudopen3d.geometry.PointCloud- source open3d point cloudtarget_cloudopen3d.geometry.PointCloud- target open3d point cloud
for other parameter, go tohttps://docs.opencv.org/master/dc/d9b/classcv_1_1ppf__match__3d_1_1ICP.htmlReturns:posenp.array- 4x4 transformation between source and targe cloudresidualfloat- the output resistration error