ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
3

Failed to find match for field 'intensity' with Ouster LIDAR

asked 2018-11-08 03:27:13 -0600

tuandl gravatar image

updated 2018-11-16 02:32:15 -0600

Hi, I tried to convert Ouster lidar packets to pcl::PointCloud<pcl::PointXYZI> using the standard pcl::fromROSMsg. I have the error complaining about the conversion can't find the field intensity. However, carefully inspect the sensor_msgs::PointCloud2 message, I see that it has the intensity field

rostopic echo /os1_node/points 
  header: 
  seq: 2
  stamp: 
    secs: 1528426469
    nsecs: 194866238
  frame_id: "os1"
height: 1
width: 65536
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
  - 
    name: "t"
    offset: 16
    datatype: 7
    count: 1
  - 
    name: "reflectivity"
    offset: 20
    datatype: 4
    count: 1
  - 
    name: "intensity"
    offset: 22
    datatype: 4
    count: 1
  - 
    name: "ring"
    offset: 24
    datatype: 2
    count: 1
is_bigendian: False
point_step: 32
row_step: 2097152

And I can print the intensity value of a point

pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
 ROS_INFO("Point intensity %f ",laserCloudIn->points[3].intensity);
Failed to find match for field 'intensity'.
[ INFO] [1541667829.480241159, 1528426469.011953052]: Point intensity 0.994560 
Failed to find match for field 'intensity'.
[ INFO] [1541667838.574432769, 1528426469.102849368]: Point intensity 0.025828

Here I do not understand why I still get the error Failed to find match for field intensity.

I greatly appreciate your help!

EDIT1: @PeteBlackerThe3rd, thank for your help. According to the User manual,

Lidar data packets consist of 16 azimuth blocks and are always 12608 Bytes in length. The packet rate is dependent on the output mode. ​Words are 32 bits in length.

Each azimuth block contains:

● Timestamp ​[64 bits] - Unique time in nanoseconds.

● Measurement ID ​ [32 bits] - a sequentially incrementing azimuth measurement counting up from 0 to 511, or 0 to 1023, or 0 to 2047 depending on lidar_mode.

● Encoder Count ​ [32 bits] - an azimuth angle as a raw encoder count, starting from 0 with a max value of 90111 - incrementing 44 ticks every azimuth angle in x2048 mode, 88 ticks in x1024 mode, and 176 ticks in x512 mode.

● Data Block ​ [96 bits] - 3 data words for each of the 16 or 64 pixels. See ​Table below for full definition. ○ Range ​[20 bits] - Range in millimeters, discretized to the nearest 12 millimeters. ○ Reflectivity ​[16 bits] - Sensor signal_photon measurements are scaled based on measured range and sensor sensitivity at that range, providing an indication of target reflectivity. Calibration of this measurement has not currently been rigorously implemented, but this will be updated in future a future firmware release. ○ Signal Photons ​[16 bits] - Signal photons in the signal return measurement are reported ○ Noise Photons ​[16 bits] - Noise photons in the noise return measurement are reported.

● Packet Status ​ - indicates whether the azimuth block is good or bad. Good = 0xFFFFFFFF, Bad = 0x0. If a packet is bad ​Measurement ID ​, ​Encoder Count ​, and ​Data Bock:Range ​ and ​Data Block:Reflectivity ​ will also be set to 0x0.

The driver indeed does provide pointcloud messages directly. The conversion is for my segmentation algorithm. I can't understand why pcl::fromROSMsg complains that there is no intensity field to match while I can ... (more)

edit retag flag offensive close merge delete

Comments

What message format are the Ouster lidar packets in? I've found most 3D LiDAR drivers produce PointCloud messages directly, which don't need conversion to pcl. At times though I've found that conversions have failed due to case sensitivity.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-11-08 05:40:11 -0600 )edit

@PeteBlackerThe3rd please see my EDIT1. I use the official driver provided by the company. It's true that in some case the intensity field is named as intensities but not with this lidar.

tuandl gravatar image tuandl  ( 2018-11-08 05:57:51 -0600 )edit

My guess would be that the ros converter is expecting the fields x, y, z & intensity. The additional fields may be confusing it. I have some cpp code which I'll post in a few minutes which enables you to manually extract fields into a pcl::PointCloud object. I'll just dig it out.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-11-08 08:19:59 -0600 )edit

2 Answers

Sort by » oldest newest most voted
3

answered 2018-11-08 11:04:14 -0600

Here are a few things that hopefully will help you get this working properly.

The point cloud message you posted the rostopic echo of has a custom point type with values (x,y,z,t,reflectivity,intensity,ring) to be able to use this type of point cloud with the C++ PCL library there will need to be a definition of this non-standard point type. This should look something like this code snippet from some of my work with ROS LiDAR hardware drivers:

#include <pcl/point_types.h>

/// Custom PCL point type with additional point data 
struct OcularPointType
{
  PCL_ADD_POINT4D;
  float amplitude;
  float reflectance;
  int pulseShapeDeviation;
  unsigned int timestamp;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (CustomPointType,
                                   (float, x, x)
                                   (float, y, y)
                                   (float, z, z)
                                   (float, amplitude, amplitude)
                                   (float, reflectance, reflectance)
                                   (float, pulseShapeDeviation, pulseShapeDeviation)
                                   (unsigned int, timestamp, timestamp)
)

This then allows you to use it with all the standard PCL functions. The ROS driver for your LiDAR should include a definition of this in a in include file somewhere, in which case you will be able to work with these point clouds natively.

If this doesn't get it working here is a last resort solution. The following template function demonstrates how to parse the ROS PointCloud2 message into a pcl::PointCloud yourself, this is doing the job of pcl::fromROSMsg but you can see exactly what's going on and fix it if you need to.

void yourPointCloud2Callback(const sensor_msgs::PointCloud2 & cloud_msg)
{
  pcl::PointCloud<pcl::PointXYZI> cloud;

  // Get the field structure of this point cloud
  int pointBytes = cloud_msg.point_step;
  int offset_x;
  int offset_y;
  int offset_z;
  int offset_int;
  for (int f=0; f<cloud_msg.fields.size(); ++f)
  {
    if (cloud_msg.fields[f].name == "x")
      offset_x = cloud_msg.fields[f].offset;
    if (cloud_msg.fields[f].name == "y")
      offset_y = cloud_msg.fields[f].offset;
    if (cloud_msg.fields[f].name == "z")
      offset_z = cloud_msg.fields[f].offset;
    if (cloud_msg.fields[f].name == "intensity")
      offset_int = cloud_msg.fields[f].offset;
  }

  // populate point cloud object
  for (int p=0; p<cloud_msg.width; ++p)
  {
      pcl::PointXYZI newPoint;

      newPoint.x = *(float*)(&cloud_msg.data[0] + (pointBytes*p) + offset_x);
      newPoint.y = *(float*)(&cloud_msg.data[0] + (pointBytes*p) + offset_y);
      newPoint.z = *(float*)(&cloud_msg.data[0] + (pointBytes*p) + offset_z);
      newPoint.intensity = *(unsigned char*)(&cloud_msg.data[0] + (pointBytes*p) + offset_int);

      cloud.points.push_back(newPoint);
  }

// The rest of your function here....

}

Hope this helps you get this working.

edit flag offensive delete link more

Comments

@PeteBlackerThe3rd I greatly appreciate your help. Indeed, the struct to define the point is provided by the driver, that's why I can get sensor_msgs::PointCloud2 messages from ouster_lidar_packets. I will use your method to manually populate the pointcloud object and see if it correctly...

tuandl gravatar image tuandl  ( 2018-11-08 11:08:54 -0600 )edit

parses the intensity. Thanks again! I will get back asap.

tuandl gravatar image tuandl  ( 2018-11-08 11:09:21 -0600 )edit

Okay, note my code example is for a 8 bit unsigned intensity whereas your Lidar message have 16 bit unsigned intensity values (type #4)

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-11-08 11:22:10 -0600 )edit

Did you have any luck getting this working?

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-11-09 07:21:38 -0600 )edit

@PeteBlackerThe3rd , thanks for your help. I change unsigned char to uint16_t to fit my data. Though we should find a way to avoid C-style casting :)

tuandl gravatar image tuandl  ( 2018-11-09 09:15:17 -0600 )edit

I agree it's rather low level. I may have a look at the source of fromROSMsg when I get a chance and find out why it isn't working in your case.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-11-09 09:26:31 -0600 )edit

I did try that too but no success :(. I think the problem is here #L169 where the function makes memcpy of the data field. I suspect something is wrong when it try to read the data type of my lidar intensity.

tuandl gravatar image tuandl  ( 2018-11-09 09:36:39 -0600 )edit

And using for loop is definitely slower. I try your function with the normal Velodyne data and the conversion time is 2-3 times slower than using pcl::fromROSMsg.

tuandl gravatar image tuandl  ( 2018-11-09 09:38:42 -0600 )edit
0

answered 2019-07-05 15:07:28 -0600

Lorry gravatar image

Hi everybody,

even though I get the output "Failed to find match for field", the PC is actually filled via the pcl::fromROSMsg command.

In order to do so, I added the points to the Point type with that:

//define velocity point
#define PCL_ADD_UNION_POINTV \
   union EIGEN_ALIGN16 { \
     struct { \
       float v_x; \
       float v_y; \
       float v_z; \
         }; \
     };


struct PointOdometry
{
    PCL_ADD_POINT4D;                  // preferred way of adding a XYZ+padding
    PCL_ADD_UNION_POINTV;
...
    PCL_ADD_INTENSITY;

Then I set up the pointcloud message by always adding a padding around those upper blocks and the intensity block.

Strangely, for the intensity and xyz I do not get the warning msg but only for my own added data. But with that solution any workarounds become obsolete.

edit flag offensive delete link more

Question Tools

4 followers

Stats

Asked: 2018-11-08 03:27:13 -0600

Seen: 9,523 times

Last updated: Jul 05 '19