0
$\begingroup$

Rosanswers logo

Hello all,

I am trying to convert a PointCloud2 message into an xyzrgb text file but am having problems with the rgb.

I am using and Intel RealSense camera and can extract RGB values just fine using the following code.

    def store_cloud(self, pc2_msg):
        self.data = []
        gen = pc2.read_points(pc2_msg, skip_nans=True, field_names=("x", "y", "z", "rgb"))
        for data in gen:            
            test = data[3]
            s = struct.pack('>f' ,test)
            i = struct.unpack('>l',s)[0]
            pack = ctypes.c_uint32(i).value
            r = int((pack & 0x00FF0000)>> 16)
            g = int((pack & 0x0000FF00)>> 8)
            b = int((pack & 0x000000FF))
            self.data.append([data[0], data[1], data[2], r, g, b])    

However upon viewing the point cloud in CloudCompare the colors behave strangely.

Some of the colors such as my skin and a good portion of the cieling and walls are colored correctly, but the rest of the image is colored wrong. Blue botches bloom where there is high reflection and my pink shirt turns blue. The flourescent lights are also a bright shade of blue. I do not understand how there can be such a discrepancy between the RVIZ display and the colored point cloud i extracted.

These images show the problem I am having.

If anyone has any ideas of how to fix this i would love to hear them.

RVIZ_pointcloud image description

My_pointcloud image description


Originally posted by Enmar on ROS Answers with karma: 33 on 2019-04-24

Post score: 0


Original comments

Comment by gvdhoorn on 2019-04-25:
Please attach your images directly to your question. I've given you sufficient karma.

Comment by Enmar on 2019-04-25:
@gvdhoorn Thanks for the karma!

Comment by Anuj07 on 2020-01-09:
I am also facing the same problem? Is there a solution/ reason to it?

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

While i don't have a fix to show i was able to determine what was causing my coloring errors from the pointcloud.

###SUMMARY:### Each field of the PointCloud2 message has a datatype property that tells you how to unpack the data into its proper values. I was attempting to unpack one datatype as another which caused the mismatch.

###DESCRIPTION:### Each point cloud message contains a property msg.fields which describes the type of data stored in the point cloud message.

For the RealSense D435i they are set up as follows

  • msg.fields[0]/name = "x"
  • msg.fields[1]/name = "y"
  • msg.fields[2]/name = "z"
  • msg.fields[3]/name = "rgb"

The RealSense camera publishes the xyz fields with a datatype 7 (float32) and the rgb field with a datatype of 6 (uint32). See the ROS PointCloud2 message description for more details.

You must first be careful when unpacking the data to make sure you unpack it assuming the correct datatype as that may change between your differing colored point cloud sources. Then you must ensure to cast/store the individual unpacked r, g, b using a datatype that displays their proper value (uint8). Only then will you get the correct 0-255 range for the data.

I know this explanation is lacking but i hope it helps some of you!


Originally posted by Enmar with karma: 33 on 2020-03-20

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$
0
$\begingroup$

Rosanswers logo

Probably way too late for op, but i willl just put it here for anyone stumble upon here like myself. Instead of unpacking the color yourself, you can simply use ros_numpy to save yourself from the headache

 pc=ros_numpy.numpify(data)
 pc=ros_numpy.point_cloud2.split_rgb_field(pc)
 points=np.zeros((pc.shape[0],3))
 points[:,0]=pc['x']
 points[:,1]=pc['y']
 points[:,2]=pc['z']
 rgb=np.zeros((pc.shape[0],3))
 rgb[:,0]=pc['r']
 rgb[:,1]=pc['g']
 rgb[:,2]=pc['b']

I formatted it this way to use it in open3d, but obviously you can reformat it to anything you like with numpy


Originally posted by BlueRingedOctopus with karma: 11 on 2022-02-04

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by kankanzheli1997 on 2022-07-04:
When I use it, it will return: ValueError: could not broadcast input array from shape (720,1280) into shape (720,)

Comment by BlueRingedOctopus on 2022-07-04:
looks like you have an image matrix instead of a pointcloud?

Comment by bluegiraffe-sc on 2022-09-16:
The ValueError experienced by @kankanzheli1997 is due to the fact that @BlueRingedOctopus code snippet assumes to be working with an unordered cloud (from PointCloud2 docs: If the cloud is unordered, height is 1 and width is the length of the point cloud.). My guess is that the numpify operation discard the extra "dummy" dimension.

The following code allowed me to work with an ordered cloud (not tested with an unordered one yet):

pc = ros_numpy.numpify(msg)
pc = ros_numpy.point_cloud2.split_rgb_field(pc)

shape = pc.shape + (3, )

points = np.zeros(shape) 
points[..., 0] = pc['x']
points[..., 1] = pc['y']
points[..., 2] = pc['z']
rgb = np.zeros(shape)
rgb[..., 0] = pc['r']
rgb[..., 1] = pc['g']
rgb[..., 2] = pc['b']
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.