VL53L5CX Multi-Zone Sensor get x,y,z of points relative to origin?

[VBhat.1]

Hi, I was trying to create a small 8x8 / 4x4 point cloud using a VL53L5CX sensor, and I can get the depth data for each zone, but I was wondering if there was a way to get x,y,z from this information? Like if there was a way to know the vertical and horizontal angle for every zone on the VL53L5CX and then calculate the x,y,z location of the points using that and the depth data.

Any help would be appreciated!

[John E KVAM]

Yes there is.

You can assume (in 8x8 mode) that the origin is exactly perpendicular to the sensor.

The code below should do what you want.

Sorry we didn't do the 4x4 case, but you can work out below what we did and the 4x4 case should be clear.

• john

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include "vl53l5cx_api.h"
#include "math.h"
/* USER CODE END Includes */
 
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
 
/* USER CODE END PTD */
 
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
 
/* USER CODE END PD */
 
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
 
/* USER CODE END PM */
 
/* Private variables ---------------------------------------------------------*/
 
/* USER CODE BEGIN PV */
int status;
volatile int IntCount;
uint8_t p_data_ready;
VL53L5CX_Configuration 	Dev;
VL53L5CX_ResultsData 	Results;
uint8_t resolution, isAlive;
uint16_t idx;
 
const double VL53L5_Zone_Pitch8x8[64] = {
		59.00,64.00,67.50,70.00,70.00,67.50,64.00,59.00,
		64.00,70.00,72.90,74.90,74.90,72.90,70.00,64.00,
		67.50,72.90,77.40,80.50,80.50,77.40,72.90,67.50,
		70.00,74.90,80.50,85.75,85.75,80.50,74.90,70.00,
		70.00,74.90,80.50,85.75,85.75,80.50,74.90,70.00,
		67.50,72.90,77.40,80.50,80.50,77.40,72.90,67.50,
		64.00,70.00,72.90,74.90,74.90,72.90,70.00,64.00,
		59.00,64.00,67.50,70.00,70.00,67.50,64.00,59.00
};
 
const double VL53L5_Zone_Yaw8x8[64] = {
		135.00,125.40,113.20, 98.13, 81.87, 66.80, 54.60, 45.00,
		144.60,135.00,120.96,101.31, 78.69, 59.04, 45.00, 35.40,
		156.80,149.04,135.00,108.45, 71.55, 45.00, 30.96, 23.20,
		171.87,168.69,161.55,135.00, 45.00, 18.45, 11.31, 8.13,
		188.13,191.31,198.45,225.00,315.00,341.55,348.69,351.87,
		203.20,210.96,225.00,251.55,288.45,315.00,329.04,336.80,
		203.20,225.00,239.04,258.69,281.31,300.96,315.00,324.60,
		225.00,234.60,246.80,261.87,278.13,293.20,305.40,315.00
};
 
 
PlaneEquation_t PlaneEquation;
XYZ_ZoneCoordinates_t XYZ_ZoneCoordinates;
 
double SinOfPitch[64], CosOfPitch[64], SinOfYaw[64], CosOfYaw[64];
/* USER CODE END PV */
 
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
 
 
 
uint8_t ComputeSinCosTables(void)
{
	//This function will save the math processing time of the code. If the user wishes to not
	//perform this function, these tables can be generated and saved as a constant.
	uint8_t ZoneNum;
	for (ZoneNum = 0; ZoneNum < 64; ZoneNum++)
	{
		SinOfPitch[ZoneNum] = sin((VL53L5_Zone_Pitch8x8[ZoneNum])*Pi/180);
		CosOfPitch[ZoneNum] = cos((VL53L5_Zone_Pitch8x8[ZoneNum])*Pi/180);
		SinOfYaw[ZoneNum] = sin(VL53L5_Zone_Yaw8x8[ZoneNum]*Pi/180);
		CosOfYaw[ZoneNum] = cos(VL53L5_Zone_Yaw8x8[ZoneNum]*Pi/180);
	}
 
	return 0;
}
 
uint8_t ConvertDist2XYZCoords8x8(VL53L5CX_ResultsData *ResultsData, XYZ_ZoneCoordinates_t *XYZ_ZoneCoordinates)
{
	uint8_t ZoneNum;
	float Hyp;
	for (ZoneNum = 0; ZoneNum < 64; ZoneNum++)
	{
		if ((ResultsData->nb_target_detected[ZoneNum] > 0) && (ResultsData->distance_mm[ZoneNum] > 0) && ((ResultsData->target_status[ZoneNum] == 5) || (ResultsData->target_status[ZoneNum] == 6) || (ResultsData->target_status[ZoneNum] == 9)) )
		{
			Hyp = ResultsData->distance_mm[ZoneNum]/SinOfPitch[ZoneNum];
			XYZ_ZoneCoordinates->Xpos[ZoneNum] = CosOfYaw[ZoneNum]*CosOfPitch[ZoneNum]*Hyp;
			XYZ_ZoneCoordinates->Ypos[ZoneNum] = SinOfYaw[ZoneNum]*CosOfPitch[ZoneNum]*Hyp;
			XYZ_ZoneCoordinates->Zpos[ZoneNum] = ResultsData->distance_mm[ZoneNum];
		}
		else
		{
			XYZ_ZoneCoordinates->Xpos[ZoneNum] = 0;
			XYZ_ZoneCoordinates->Ypos[ZoneNum] = 0;
			XYZ_ZoneCoordinates->Zpos[ZoneNum] = 0;
		}
	}
	return 0;
}
 
uint8_t PrintXYZCoords(XYZ_ZoneCoordinates_t *XYZ_ZoneCoordinates)
{
	uint8_t i, j;
	printf("XYZ Coordinates for the target in each zone\n");
	for (i = 0; i < 8; i++) \
	{
		for (j = 0; j < 8; j++)
		{
			idx = (i * 8 + j);
			printf("%5.0f, %5.0f, %5.0f |",XYZ_ZoneCoordinates->Xpos[idx],XYZ_ZoneCoordinates->Ypos[idx],XYZ_ZoneCoordinates->Zpos[idx]);
		}
		printf("\n");
	}
	printf("\n");
 
	return 0;
}

[MMcAl.1]

Could you provide code examples for the model with larger FOV: VL53L7CX?

Thanks in advance!

[John E KVAM]

you alread have it!

The L7 is identical in every way - save for the optics that make the FoV.

So the L5 code you have will work.

Or you can download a new version and there is an #ifdef that selects between the L5 and the L7.

The calculation of the radial to perpendicular has come different coeffecents.

But that is the only difference.

Chip ID might be different as well.

(Look for the #define - if you don't see it, get an updated driver.)

• john

[MMcAl.1]

Thanks for your fast reply.

If I understand you correctly, the updated coefficients could be found somewhere in sample code. Can you give me a hint where I can find this specific code to convert the TOF data to a point cloud? I am not able to find it in the downloads available online: STSW-IMG036, STSW-IMG037, STSW-IMG038 (https://www.st.com/en/imaging-and-photonics-solutions/vl53l7cx.html#tools-software).

Thanks!

[John E KVAM]

I don't think the guy who wrote that L5 code translated it into the L7. Although I can ask him.

Not sure how quickly I can get that for you however.

This is NOT something that's in the STSW-IMG packages.

[John E KVAM]

In the code above insert the following conditional.

But the guy who did the work did minimal verification. It may not be perfect, but it's very close.

(At least it matches the data I took.)

So use this and if it's not perfect, let me know.

• john

#ifdef VL53L5
const double VL53L5_Zone_Pitch8x8[64] = {
		59.00,64.00,67.50,70.00,70.00,67.50,64.00,59.00,
		64.00,70.00,72.90,74.90,74.90,72.90,70.00,64.00,
		67.50,72.90,77.40,80.50,80.50,77.40,72.90,67.50,
		70.00,74.90,80.50,85.75,85.75,80.50,74.90,70.00,
		70.00,74.90,80.50,85.75,85.75,80.50,74.90,70.00,
		67.50,72.90,77.40,80.50,80.50,77.40,72.90,67.50,
		64.00,70.00,72.90,74.90,74.90,72.90,70.00,64.00,
		59.00,64.00,67.50,70.00,70.00,67.50,64.00,59.00
};
#else	//VL53L7
const double VL53L5_Zone_Pitch8x8[64] = {
		52.00,56.00,61.00,64.00,64.00,61.00,56.00,52.00,
		56.00,62.00,67.00,70.00,70.00,67.00,62.00,56.00,
		61.00,67.00,76.00,78.00,78.00,76.00,67.00,61.00,
		64.00,70.00,78.00,84.00,84.00,78.00,70.00,64.00,
		64.00,70.00,78.00,84.00,84.00,78.00,70.00,64.00,
		61.00,67.00,76.00,78.00,78.00,76.00,67.00,61.00,
		56.00,62.00,67.00,70.00,70.00,67.00,62.00,56.00,
		52.00,56.00,61.00,64.00,64.00,61.00,56.00,52.00
};
#endif

[MMcAl.1]

Hi john,

thank you very much for your fast response!

At first sight, the point cloud looks good!

[DSchi.4]

Hi John,

Thanks for sharing this code. If I try to compile the code, I can't find the declaration for

1. PlaneEquation_t PlaneEquation;
2. XYZ_ZoneCoordinates_t XYZ_ZoneCoordinates;

In which API is the type defined?

Best regards,

Daniel

[John E KVAM]

/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file : VL53L5_Plane_Algo.h
 * @brief : Header for main.c file.
 * This file contains the common defines of the application.
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
 * All rights reserved.</center></h2>
 *
 * This software component is licensed by ST under BSD 3-Clause license,
 * the "License"; You may not use this file except in compliance with the
 * License. You may obtain a copy of the License at:
 * opensource.org/licenses/BSD-3-Clause
 *
 ******************************************************************************
 */
/* USER CODE END Header */
 
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __VL53L5_Plane_Algo_H
#define __VL53L5_Plane_Algo_H
 
#ifdef __cplusplus
extern "C" {
#endif
 
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "vl53l5cx_api.h"
#include <stdio.h>
#include "math.h"
 
 
 
#define Pi 3.14156
 
typedef struct
{
	double Ax;
	double By;
	double Cz;
	double Offset;
	double XZ_Angle;
	double YZ_Angle;
}PlaneEquation_t;
 
typedef struct
{
	double Xpos[64];
	double Ypos[64];
	double Zpos[64];
}XYZ_ZoneCoordinates_t;
 
uint8_t Plane_Detection8x8(XYZ_ZoneCoordinates_t *XYZ, PlaneEquation_t *PlaneEq, uint16_t Accuracy_mm);
uint8_t ConvertDist2XYZCoords8x8(VL53L5CX_ResultsData *ResultsData, XYZ_ZoneCoordinates_t *XYZ_ZoneCoordinates);
uint8_t PrintResults(VL53L5CX_ResultsData *ResultsData);
uint8_t PrintXYZCoords(XYZ_ZoneCoordinates_t *XYZ_ZoneCoordinates);
uint8_t ComputeSinCosTables(void);
 
 
#endif /* __VL53L5_Plane_Algo_H */

I took this from some older code. Not sure if you should use the entire include or not.

But it does answer the question you asked.

[DSchi.4]

Thank you.
This email contains confidential information. If you have received it in error, you must not read, use, copy or pass on this e-mail or its attachments.
If you have received the e-mail in error, please inform me immediately by reply e-mail and then delete this e-mail from your system. Thank you
Diese E-Mail enthaelt vertrauliche Informationen. Sollten Sie sie irrtuemlich erhalten haben, duerfen Sie diese E-Mail oder ihre Anhaenge nicht lesen, verwenden, kopieren oder weitergeben.
Sollten Sie die Mail versehentlich erhalten haben, teilen Sie mir dies bitte umgehend per Antwort-E-Mail mit und loeschen Sie diese E-Mail dann aus Ihrem System. Vielen Dank

[John E KVAM]

Have a look at the two arrays. And let us consider pitch first. 

The exact center of the part is actually a dot in the center of the matrix. (Right between the zones of:

135,45

225, 315

we are using a cylindric coordinate system. So the 0 angle is to the west, 90 degrees North, 180 East and 270 is South. 

Notice the 45's are at the one up, one over, two up and two over etc.

But you need another angle. You need to know how far off straight ahead your target is. And that's the Yaw. 

If you are very near straight ahead, the angles are small (4.5) and they get larger as you get toward the 45. 

(The sensor is 43 degrees or so in field of video, but we are pointing to the center of the zone - not the edge. )

The result of the function call is an (X,Y,Z) point in the Cartesian coordinate system. 

So if you had an object straight ahead and 1M away, it's pitch would be anything, the yaw would be 0 and the distance from the sensor would be 1M. In the XYZ coordinate system it would be (0,0,1)

- john

[CPoisson]

Thank you.
I understand the idea. Just for the ones who would do it with the 4x4 configuration, there are the two matrix pitch and yaw :

const double Pitch_4x4[16] = {
 66.67, 73.25, 73.25, 66.67,
	73.25, 80.24, 80.24, 73.25,
	73.25, 80.24, 80.24, 73.25,
	66.67, 73.25, 73.25, 66.67
};

const double Yaw_4x4[16] = { 
 135.00, 108.40, 71.60, 45.00, 				
 161.60, 135.00, 45.00, 18.40, 
 198.40, 225.00, 315.00, 341.60,				
 225.00, 251.60, 288.40, 315.00				
};

 
This method doesn't work so well to calculate the normal vector of the plan seen by the sensor.
Thank you for your answer !

[John E KVAM]

there is no way I can write that for you. But someone else already did it on another thread. Have a look at:

https://community.st.com/t5/imaging-sensors/vl53l5cx-multi-zone-sensor-get-x-y-z-of-points-relative-to/td-p/172929

 

[Farris]

Ah no worries I got it working in the end.

Cheers