angle range reduced Cartesian cloud point parsing

[sisusch]

Hello everyone,

I have a problem interpreting the data coming from the picoscan 150 sensor, below I report my problem.
The sensor is configured with 0.05° 15hz, compact data format (see cli for the sensor setting in initialization). Now I have two problems that I can't explain, and I think they both derive from the misinterpretation of the data that I do.

I would like to install the sensor at a certain height with the x-axis perpendicular to the ground. With respect to the x-axis I have to consider the points that are from beta =-64° to +64°. So i'm interested only at the middle sector, the orange marker sketched.

Below is what sopas and lidar seen

Below is my Cartesian callback to process the data:

void customizedPointCloudMsgCb(SickScanApiHandle apiHandle, const SickScanPointCloudMsg* msg)
{
	//segment_idx = -1 is a full frame
	if (msg->segment_idx == -1)
	{
		// Calcolare la dimensione totale del buffer da ricostruire
		size_t total_size = msg->row_step * msg->height;  // Dimensione totale del buffer (in byte)

		// Calculate the total number of points
		size_t num_points = (total_size) / (4 * sizeof(float));

		std::cout << "Segment number:" << msg->segment_idx << " Pack size:" << total_size <<
			"  Num points:" << num_points << std::endl;
		// Get offsets for x, y, z, intensity values
		SickScanPointFieldMsg* msg_fields_buffer = msg->fields.buffer;
		int field_offset_x = -1, field_offset_y = -1, field_offset_z = -1, field_offset_intensity = -1;
		for (int n = 0; n < msg->fields.size; n++)
		{
			if (strcmp(msg_fields_buffer[n].name, "x") == 0 && msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32)
				field_offset_x = msg_fields_buffer[n].offset;
			else if (strcmp(msg_fields_buffer[n].name, "y") == 0 && msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32)
				field_offset_y = msg_fields_buffer[n].offset;
			else if (strcmp(msg_fields_buffer[n].name, "z") == 0 && msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32)
				field_offset_z = msg_fields_buffer[n].offset;
			else if (strcmp(msg_fields_buffer[n].name, "i") == 0 && msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32)
				field_offset_intensity = msg_fields_buffer[n].offset;
		}
		try {
			if (field_offset_x < 0 || field_offset_y < 0 || field_offset_z < 0) {
				throw std::invalid_argument("Offsets cannot be negative");
			}
			// Proceed with the code if the offsets are valid
		}
		catch (const std::invalid_argument& e) {
			std::cerr << "Error: " << e.what() << std::endl;
			// Handle the error (e.g., log it, try to recover, etc.)
			// Optionally, terminate the program or take corrective actions
		}

		//CLEAR THE DATA STRUCT BEFORE PARSE NEW VALUE
		// Array of references to the vectors inside CloudPoints
		std::vector<float>* fields[] = { &CloudPoints.X, &CloudPoints.Y, &CloudPoints.Z, &CloudPoints.I };
		// Loop through each vector and perform clear and resize
		for (auto& field : fields) {
			field->clear();
			field->resize(num_points);
		}
		//Is a 2D lidar, so signle row with a col number = num points
		for (int col_idx = 0; col_idx < num_points; col_idx++)
		{
			// Get cartesian point coordinates
			int polar_point_offset = col_idx * msg->point_step;
			CloudPoints.X[col_idx] = *((float*)(msg->data.buffer + polar_point_offset + field_offset_x));
			CloudPoints.Y[col_idx] = *((float*)(msg->data.buffer + polar_point_offset + field_offset_y));
			CloudPoints.Z[col_idx] = *((float*)(msg->data.buffer + polar_point_offset + field_offset_z));
			if (field_offset_intensity >= 0)
				CloudPoints.I[col_idx] = *((float*)(msg->data.buffer + polar_point_offset + field_offset_intensity));
		}
		saveToCSV("cloud_points.csv", CloudPoints);
	}
}

Question

1. Now being interested in an angular range of 128° (64*2) and with a spatial resolution of 0.05° I should have a number of points equal to 128/0.05= 2560 per scan. So when the full frame trigger my function i have in reality many values, as can be seen below 4200 points.

CLI_SETTING.txt

2. Comparing what I get with the csv and my code and what is instead represented graphically in the sopas (see first screen), my image has a clockwise rotation of 90°, can anyone explain to me why? I would like the data saved in the csv and then displayed through matlab, to be as shown in the sopas interface.

cloud_points.csv

3.Perhaps by solving the first question, this one is also answered. Why is the positive limit of +64° respected and data above this angle is discarded, while on the negative angle side values ​​below -64° are not discarded? It looks like my frame starts at -138° and stops a bit above +64°. As if I took the first 7 compact segments

Any suggestions on how to solve this problems?

Thanks for the support.

[weinmalSICKAG]

Hello sisusch,
thank you for your questions. Let me try to answer them.

1. Please use the option host_LFPangleRangeFilter = 1 -64.0 +64.0 -90.0 +90.0 1 to restrict the field of view. Beware that there might be more points in the result that one would expect. These unexpected points should have a distance of zero as explained in this issue (How to set maximum and minimum angles in sick_picoscan.launch driver #452 (comment)). If the rangeFilter parameter is set to rangeFilter=0.05,120,1 these unexpected points should not be in the point cloud.
2. This 90° clockwise rotation comes from the fact that the x-axis is pointing to the top of the the screen in SOPAS and the x-axis is pointing to the right of the screen in matlab.
3. Maybe this solves itself by solving question one.

Please do not hesitate to ask further questions if something is unclear or is not working.

[sisusch]

Thanks weinmalSICKAG for the answer,
but now I have even more questions

1. what should these parameters do in the launch file?

<arg name="all_segments_min_deg" default="-64.0" />      <!-- angle range covering all segments: all segments pointcloud on topic publish_topic_all_segments is published,  -->
<arg name="all_segments_max_deg" default="+64.0" />      <!-- if received segments cover angle range from all_segments_min_deg to all_segments_max_deg. -180...+180 for multiScan136 (360 deg fullscan), -138...+138 for picoScan (fullscan) -->

OK for the rotation it's clear the -90° rotation that i have to introduce to align with the sopas rappresentation

CloudPoints.X[col_idx] = -Y;
CloudPoints.Y[col_idx] = X;

I would like a constant number of points, even if slightly greater than my measurement range of interest.
For example, I am interested from +64° to -64°, with a frequency of 15hz, I have 10 segments of 30° and 600 points for each segment, with a resolution of about 0.05°. According to the table I reported above, I would like the sensor to transmit only segments 2,3,4,5,6 (angular range -78° to +72°) the others not being of mine interest , the sensor will not have to transmit them in the udp. This will be to have 600 point for 5 segment 3000 point into a full frame.
I want that if there are outliers inside this range , these points are published anyway with 0 or -1 (if it is possible), so that I don't have to use variable structures after the callback.

So, here is how I would like to change my configuration parameters, please check if this will be correct based in how I would to use the lidar

<arg name="all_segments_min_deg" default="-78.0" />      <!-- angle range covering all segments: all segments pointcloud on topic publish_topic_all_segments is published,  -->
<arg name="all_segments_max_deg" default="+72.0" />      <!-- if received segments cover angle range from all_segments_min_deg to all_segments_max_deg. -180...+180 for multiScan136 (360 deg fullscan), -138...+138 for picoScan (fullscan) -->

<arg name="host_LFPangleRangeFilter" default="1 -64.0 +64.0 -90.0 +90.0 1" /> <!-- Optionally set LFPangleRangeFilter to "<enabled> <azimuth_start> <azimuth_stop> <elevation_start> <elevation_stop> <beam_increment>" with azimuth and elevation given in degree -->
<arg name="host_set_LFPangleRangeFilter" default="True" />                     <!-- If true, LFPangleRangeFilter is set at startup (default: false) -->

<!-- cloud_unstructured_segments: cartesian coordinates, segmented, all echos, all layers, range filter on, max. 2700 points, mean ca. 1000 points per cloud -->
<param name="cloud_unstructured_segments" type="string" value="coordinateNotation=0 updateMethod=1 echos=0 layers=1 reflectors=0,1 infringed=0,1 rangeFilter=0.05,20,1 topic=/Schnell_unstructured_segments frameid=world publish=1"/>
        
<!-- cloud_unstructured_fullframe: cartesian coordinates, fullframe, all echos, all layers, range filter on, max. 32400 points, mean ca. 10000 points per cloud -->
<param name="cloud_unstructured_fullframe" type="string" value="coordinateNotation=0 updateMethod=0 echos=0 layers=1 reflectors=0,1 infringed=0,1 rangeFilter=0.05,20,1 topic=/Schnell_fullframe frameid=world publish=1"/>

[weinmalSICKAG]

Hello siush,
the first thing that you can try to do is set the back to their default values of -138.0 and +138.0 respectively.

To reduce that data that is sent from the device to segments 2,3,4,5,6 the following is sufficient.

<arg name="host_LFPangleRangeFilter" default="1 -64.0 +64.0 -90.0 +90.0 1" /> <!-- Optionally set LFPangleRangeFilter to "<enabled> <azimuth_start> <azimuth_stop> <elevation_start> <elevation_stop> <beam_increment>" with azimuth and elevation given in degree -->
<arg name="host_set_LFPangleRangeFilter" default="True" />                     <!-- If true, LFPangleRangeFilter is set at startup (default: false) -->

The unstructured point cloud is not the right data structure to use when you expect the same number of points for each 360° rotation because the point cloud is not dense. A laser scan or multi echo laser scan is more suitable for that.

You can try your luck by disabling the range filter with rangeFilter=0.05,20,0. This way the points which have been filtered out by the device (because of host_LFPangleRangeFilter) and set to zero will be contained in the point cloud. However, the size of the point cloud might change in the future without notice when points are not added to the point cloud for some other reason.

[sisusch]

Thanks weinmalSICKAG for the answer,

this conversation is becoming more and more interactive in understanding how to best use this sensor.

Yes in my application I would need data defined and contant number at each scann, like a profilometer, but with a range of a lidar.

Before ask for a sample, we have read the datasheet of the sensor, I thought it was able to perform this task, acting as laser scan.

Also reading the launch file, do you think I could use the custom structured configuration? If so how can I make the callback intercept only this structure and not the unstructred ones?

<!-- cloud_structured_hires0: cartesian and polar coordinates, fullframe, all echos, range filter off, high resolution layer 6, fields=x,y,z,i,range,azimuth,elevation, number of points: 2880 x NumEchos x NumSegments -->
        <param name="cloud_structured_hires0" type="string" value="coordinateNotation=3 updateMethod=0 fields=x,y,z,i,range,azimuth,elevation echos=0,1,2 layers=6 reflectors=0,1 infringed=0,1 rangeFilter=0,999,0 topic=/cloud_structured_hires0 frameid=world publish=1"/>

        <!-- cloud_structured_hires0_segments: cartesian and polar coordinates, segments, all echos, range filter off, high resolution layer 6, fields=x,y,z,i,range,azimuth,elevation -->
        <param name="cloud_structured_hires0_segments" type="string" value="coordinateNotation=3 updateMethod=1 fields=x,y,z,i,range,azimuth,elevation echos=0,1,2 layers=6 reflectors=0,1 infringed=0,1 rangeFilter=0,999,0 topic=/cloud_structured_hires0_segments frameid=world publish=1"/>

        <!-- cloud_structured_hires1: cartesian and polar coordinates, fullframe, all echos, range filter off, high resolution layer 14, fields=x,y,z,i,range,azimuth,elevation, number of points: 2880 x NumEchos x NumSegments -->
        <param name="cloud_structured_hires1" type="string" value="coordinateNotation=3 updateMethod=0 fields=x,y,z,i,range,azimuth,elevation echos=0,1,2 layers=14 reflectors=0,1 infringed=0,1 rangeFilter=0,999,0 topic=/cloud_structured_hires1 frameid=world publish=1"/>

        <!-- cloud_structured_hires1_segments: cartesian and polar coordinates, segments, all echos, range filter off, high resolution layer 14, fields=x,y,z,i,range,azimuth,elevation -->
        <param name="cloud_structured_hires1_segments" type="string" value="coordinateNotation=3 updateMethod=1 fields=x,y,z,i,range,azimuth,elevation echos=0,1,2 layers=14 reflectors=0,1 infringed=0,1 rangeFilter=0,999,0 topic=/cloud_structured_hires1_segments frameid=world publish=1"/>

        <!-- cloud_structured: cartesian and polar coordinates, fullframe, all echos, range filter off, low resolution layers 1,2,3,4,5,7,8,9,10,11,12,13,15,16, fields=x,y,z,i,range,azimuth,elevation, 12*360*14*3=181440 points per cloud -->
        <param name="cloud_structured" type="string" value="coordinateNotation=3 updateMethod=0 fields=x,y,z,i,range,azimuth,elevation echos=0,1,2 layers=1,2,3,4,5,7,8,9,10,11,12,13,15,16 reflectors=0,1 infringed=0,1 rangeFilter=0,999,0 topic=/cloud_structured frameid=world publish=1"/>

        <!-- cloud_structured_segments: cartesian and polar coordinates, segments, all echos, range filter off, low resolution layers 1,2,3,4,5,7,8,9,10,11,12,13,15,16, fields=x,y,z,i,range,azimuth,elevation, 360*14*3=15120 points per cloud -->
        <param name="cloud_structured_segments" type="string" value="coordinateNotation=3 updateMethod=1 fields=x,y,z,i,range,azimuth,elevation echos=0,1,2 layers=1,2,3,4,5,7,8,9,10,11,12,13,15,16 reflectors=0,1 infringed=0,1 rangeFilter=0,999,0 topic=/cloud_structured_segments frameid=world publish=1"/>

        <!-- cloud_all_fields_segments: all fields (x,y,z,i,range,azimuth,elevation,layer,echo,reflector), segments, all echos, all layers, range filter off -->
        <param name="cloud_all_fields_segments" type="string" value="coordinateNotation=3 updateMethod=1 fields=x,y,z,i,range,azimuth,elevation,t,ts,lidar_sec,lidar_nsec,ring,layer,echo,reflector echos=0,1,2 layers=1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16 reflectors=0,1 infringed=0,1 rangeFilter=0,999,0 topic=/cloud_all_fields_segments frameid=world publish=1"/>

        <!-- cloud_all_fields_fullframe: all fields (x,y,z,i,range,azimuth,elevation,layer,echo,reflector), fullframe, all echos, all layers, range filter off -->
        <param name="cloud_all_fields_fullframe" type="string" value="coordinateNotation=3 updateMethod=0 fields=x,y,z,i,range,azimuth,elevation,t,ts,lidar_sec,lidar_nsec,ring,layer,echo,reflector echos=0,1,2 layers=1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16 reflectors=0,1 infringed=0,1 rangeFilter=0,999,0 topic=/cloud_all_fields_fullframe frameid=world publish=1"/>

thanks for the support.

[weinmalSICKAG]

To avoid confusion I will answer to the first part of your reply.
I was not precise with what I meant by "laser scan". Of course the picoScan is a laser scanner. The driver sick_scan_xd can provide the data in various data structures for ROS: PointCloud2 and LaserScan. After reading the thread again, I realized that you are not using ROS.

As described here https://github.com/SICKAG/sick_scan_xd/blob/develop/launch/sick_picoscan.launch#L179 the structured point clouds contain all points. To define structured point clouds equivalent to these unstructured point clouds

<!-- cloud_unstructured_segments: cartesian coordinates, segmented, all echos, all layers, range filter on, max. 2700 points, mean ca. 1000 points per cloud -->
<param name="cloud_unstructured_segments" type="string" value="coordinateNotation=0 updateMethod=1 echos=0 layers=1 reflectors=0,1 infringed=0,1 rangeFilter=0.05,20,1 topic=/Schnell_unstructured_segments frameid=world publish=1"/>
        
<!-- cloud_unstructured_fullframe: cartesian coordinates, fullframe, all echos, all layers, range filter on, max. 32400 points, mean ca. 10000 points per cloud -->
<param name="cloud_unstructured_fullframe" type="string" value="coordinateNotation=0 updateMethod=0 echos=0 layers=1 reflectors=0,1 infringed=0,1 rangeFilter=0.05,20,1 topic=/Schnell_fullframe frameid=world publish=1"/>

define the following parameters

<!-- cloud_unstructured_segments: cartesian coordinates, segmented, all echos, all layers, range filter on, max. 2700 points, mean ca. 1000 points per cloud -->
<param name="sisusch_cloud_structured_segments" type="string" value="coordinateNotation=0 updateMethod=1 echos=0 layers=1 reflectors=0,1 infringed=0,1 rangeFilter=0,999,0 topic=/Schnell_structured_segments frameid=world publish=1"/>
        
<!-- cloud_unstructured_fullframe: cartesian coordinates, fullframe, all echos, all layers, range filter on, max. 32400 points, mean ca. 10000 points per cloud -->
<param name="sisusch_cloud_structured_fullframe" type="string" value="coordinateNotation=0 updateMethod=0 echos=0 layers=1 reflectors=0,1 infringed=0,1 rangeFilter=0,999,0 topic=/Schnell_fullframe frameid=world publish=1"/>

The only difference is the rangeFilter.
Then add these two new point clouds to custom_pointclouds.

I am sorry for the confusion.

[sisusch]

Thanks for the clarification, now I have everything clearer, it actually seemed strange to me that a sensor of this type had such a limited compartment.

later.....
I tried what you suggested (literally copying the lines of code you wrote me and replace the old one), but when i did it the call back is never triggered. So I simply updated the ones I already had by eliminating the effect of rangeFilter=0.05,999,0 .

    <param name="custom_pointclouds" type="string" value="$(arg custom_pointclouds)"/> <!-- Default pointclouds: segmented pointcloud and fullframe pointcloud with all layers and echos in cartesian coordinates -->
    
    <!-- A list predefined pointclouds is configured below. Use all of them or just a subset, according to your needs. Further customized pointclouds can be added in the following configuration -->
    <!-- param name="custom_pointclouds" type="string" value="cloud_unstructured_segments cloud_polar_unstructured_segments cloud_unstructured_fullframe cloud_unstructured_echo1 cloud_unstructured_echo1_segments cloud_unstructured_echo2 cloud_unstructured_echo2_segments cloud_unstructured_echo3 cloud_unstructured_echo3_segments cloud_unstructured_reflector cloud_unstructured_reflector_segments cloud_structured_hires0 cloud_structured_hires0_segments cloud_structured_hires1 cloud_structured_hires1_segments cloud_structured cloud_structured_segments cloud_all_fields_segments cloud_all_fields_fullframe"/ -->

 <!-- cloud_unstructured_segments: cartesian coordinates, segmented, all echos, all layers, range filter on, max. 2700 points, mean ca. 1000 points per cloud -->
<param name="cloud_unstructured_segments" type="string" value="coordinateNotation=0 updateMethod=1 echos=0 layers=1 reflectors=0,1 infringed=0,1 rangeFilter=0.05,999,0 topic=/Schnell_structured_segments frameid=world publish=1"/>
<!-- cloud_unstructured_fullframe: cartesian coordinates, fullframe, all echos, all layers, range filter on, max. 32400 points, mean ca. 10000 points per cloud -->
<param name="cloud_unstructured_fullframe" type="string" value="coordinateNotation=0 updateMethod=0 echos=0 layers=1 reflectors=0,1 infringed=0,1 rangeFilter=0.05,999,0 topic=/Schnell_structured_fullframe frameid=world publish=1"/>
<!-- On ROS-1 and ROS-2, sick_scan_xd publishes TF messsages to map a given base frame (i.e. base coordinates system) to the lidar frame (i.e. lidar coordinates system) and vice versa.
        The default base frame id is "map" (which is the default frame in rviz). 
        The default 6D pose is (x,y,z,roll,pitch,yaw) = (0,0,0,0,0,0) defined by position (x,y,z) in meter and (roll,pitch,yaw) in radians.
        This 6D pose (x,y,z,roll,pitch,yaw) is the transform T[base,lidar] with parent "base" and child "lidar".
        For lidars mounted on a carrier, the lidar pose T[base,lidar] and base frame can be configured in this launchfile using the following parameter.
        The lidar frame id given by parameter "frame_id" resp. "publish_frame_id".
        Note that the transform is specified using (x,y,z,roll,pitch,yaw). In contrast, the ROS static_transform_publisher uses commandline arguments in order (x,y,z,yaw,pitch,roll).

The above is what work for me.