The following listing shows how to project a list of 3D points from the point cloud generated by an Ouster sensor back into the 2D range frame:
import numpy as np
import copy
def binary_search(arr, x):
low, high = 0, len(arr) - 1
while low <= high:
mid = (low + high) // 2
if x < arr[mid]:
low = mid + 1
elif x > arr[mid]:
high = mid - 1
else: # x == arr[mid]
return mid
if high < 0: return 0
d1 = abs(arr[low] - x)
d2 = abs(arr[high] - x)
return low if d1 < d2 else high
class ReverseXYZLut:
def __init__(self, metadata, height=None, width=None) -> None:
self._sensor_info = metadata
# For DEBUGGING
self._register = {}
def _generate_metadata(self, height, width):
si = self._sensor_info
factor = si.format.pixels_per_column // height
meta = copy.deepcopy(si)
pi = si.get_product_info()
form_factor = pi.form_factor if not pi.form_factor[-1].isdigit() else \
F"{pi.form_factor[:-1]}-{pi.form_factor[-1]}"
si.prod_line = f"{form_factor}-{height}"
si.format.pixels_per_column = height
si.format.pixel_shift_by_row = si.format.pixel_shift_by_row[::factor]
si.beam_azimuth_angles = si.beam_azimuth_angles[::factor]
si.beam_altitude_angles = si.beam_altitude_angles[::factor]
return meta
@staticmethod
def phi_to_u(beams, phi_deg):
if phi_deg > beams[0]:
u = 0
elif phi_deg < beams[-1]:
u = len(beams) - 1
else:
u = binary_search(beams, phi_deg)
return u
@staticmethod
def xyz_2_u_v(x, y, z, r0, beams, azimuth_angles_deg, W):
"""
r0 is the distance from the beam origin to the lidar origin
"""
r = np.linalg.norm([x, y, z])
sin_beta = z / r
cos_beta = np.sqrt(1 - np.power(sin_beta, 2))
phi = np.arctan2(z, r * cos_beta - r0)
r_prime = np.rint(z / np.sin(phi) + r0)
# now azimuth
u = ReverseXYZLut.phi_to_u(beams, np.rad2deg(phi))
a = np.deg2rad(-azimuth_angles_deg[u])
A = r_prime * np.cos(phi)
K = A * np.cos(a) + r0
L = A * np.sin(a)
sin_e = K * y - L * x
cos_e = K * x + L * y
theta_e = np.arctan2(sin_e, cos_e)
e = 2.0 * np.pi - theta_e * W / (2 * np.pi)
v = np.int32(np.rint(e)) - 6 # TODO: still need to figure out why `v-6`
return r_prime, u, v
def transform_point(self, p, transform):
hp = np.append(p, 1)
tp = np.dot(transform, hp)
return tp[:-1]
def project_point(self, xyz):
si = self._sensor_info
x, y, z = xyz
return ReverseXYZLut.xyz_2_u_v(x, y, z,
r0=si.lidar_origin_to_beam_origin_mm,
beams=si.beam_altitude_angles,
azimuth_angles_deg=si.beam_azimuth_angles,
W=si.format.columns_per_frame)
def check_for_duplicates(self, u, v, p):
if (u, v) not in self._register:
self._register[(u, v)] = []
self._register[(u, v)].append(p)
else:
self._register[(u, v)].append(p)
def report_duplicates(self):
duplicates = 0
for k, v in self._register.items():
if len(v) > 1:
print(k, v)
duplicates += 1
print("total duplicates:", duplicates)
def project_frame(self, points, use_sensor_2_lidar=True):
si = self._sensor_info
sensor_2_lidar = np.linalg.inv(si.lidar_to_sensor_transform)
H = si.format.pixels_per_column
W = si.format.columns_per_frame
output = np.zeros((H, W), dtype=np.int32)
self._register = {}
for p in points:
R = np.linalg.norm(p)
if np.any(np.isnan(p)) or R == 0:
continue
try:
if use_sensor_2_lidar:
p = self.transform_point(p, sensor_2_lidar)
if np.linalg.norm(p) == 0:
continue
r, u, v = self.project_point(p)
output[u, v] = r
self.check_for_duplicates(u, v, p)
except Exception as e:
print(f"error: {e}")
import traceback
traceback.print_exc()
print(r, u, v)
exit(1)
self.report_duplicates() # this is mainly for debugging purposes
return output
It should be very straightforward to use:
import ouster.sdk.core as core
from reverse_xyz import ReverseXYZLut
with open(metadata_url) as json_file:
sensor_info = core.SensorInfo(json_file.read())
# Create the ReverseXYZLut object using the SensorInfo
rXYZLut = ReverseXYZLut(sensor_info)
# then use the method project_frame to create the 2d image
range_image = rXYZLut.project_frame(points) # where points is a list of 3d points
Created in response to the following asks:
- Projection from 3d point cloud to intensity image and intrics · Issue #476 · ouster-lidar/ouster-ros · GitHub
- How to handle self-obstruction ? Or how to reproject 3d points in the 2d scan to create a mask in the scan · Issue #577 · ouster-lidar/ouster-sdk · GitHub
- How to Map Bounding Boxes from Point Cloud Data to Corresponding Positions on RGB Images? . Issue #602 . ouster-ros . Github
Please note that this part of an unfinished work, which may or may not be integrated into the SDK later, so it is provided as is.