Carla: How to convert pointcloud from depth image to world frame
I am using carla 0.9.8
and using the following code -
#!/usr/bin/env python
# Copyright (c) 2019 Aptiv
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
"""
An example of client-side with basic car controls.
Controls:
W : throttle
S : brake
AD : steer
Space : hand-brake
ESC : quit
"""
# ==============================================================================
# -- find carla module ---------------------------------------------------------
# ==============================================================================
import time
start = time.time()
import glob
import os
import sys
try:
sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
sys.version_info.major,
sys.version_info.minor,
'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
pass
# ==============================================================================
# -- imports -------------------------------------------------------------------
# ==============================================================================
import carla
import weakref
import random
from math import pi, radians, sin, cos, tan
import open3d as o3d
import pygame
from pygame.locals import K_ESCAPE
from pygame.locals import K_SPACE
from pygame.locals import K_a
from pygame.locals import K_d
from pygame.locals import K_s
from pygame.locals import K_w
from pygame.locals import K_m
import numpy as np
from numpy.matlib import repmat
VIEW_WIDTH = 1920//3
VIEW_HEIGHT = 1080//3
VIEW_FOV = 90
# ==============================================================================
# -- BasicSynchronousClient ----------------------------------------------------
# ==============================================================================
class BasicSynchronousClient(object):
"""
Basic implementation of a synchronous client.
"""
def __init__(self):
self.client = None
self.world = None
self.camera = None
self.depth_camera = None
self.car = None
self.display = None
self.depth_display = None
self.image = None
self.depth_image = None
self.capture = True
self.depth_capture = True
self.point_cloud = o3d.geometry.PointCloud()
self.pcd = o3d.geometry.PointCloud()
self.all = o3d.geometry.PointCloud()
self.origin = None#o3d.geometry.TriangleMesh.create_coordinate_frame(size=10.0, origin=[0,0,0])
self.pose = None#o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0,0,0])
self.last_position = None
self.loop_number = 0
def camera_blueprint(self):
"""
Returns camera blueprint.
"""
camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
camera_bp.set_attribute('image_size_x', str(VIEW_WIDTH))
camera_bp.set_attribute('image_size_y', str(VIEW_HEIGHT))
camera_bp.set_attribute('fov', str(VIEW_FOV))
return camera_bp
def depth_camera_blueprint(self):
"""
Returns camera blueprint.
"""
depth_camera_bp = self.world.get_blueprint_library().find('sensor.camera.depth')
depth_camera_bp.set_attribute('image_size_x', str(VIEW_WIDTH))
depth_camera_bp.set_attribute('image_size_y', str(VIEW_HEIGHT))
depth_camera_bp.set_attribute('fov', str(VIEW_FOV))
return depth_camera_bp
def set_synchronous_mode(self, synchronous_mode):
"""
Sets synchronous mode.
"""
settings = self.world.get_settings()
settings.synchronous_mode = synchronous_mode
self.world.apply_settings(settings)
def setup_car(self):
"""
Spawns actor-vehicle to be controled.
"""
car_bp = self.world.get_blueprint_library().filter('model3')[0]
location = random.choice(self.world.get_map().get_spawn_points())
self.car = self.world.spawn_actor(car_bp, location)
position = self.car.get_transform()
x, y, z, roll, pitch, yaw = position.location.x, - position.location.y, position.location.z, position.rotation.roll, position.rotation.pitch, -position.rotation.yaw
self.last_position = x, y, z, roll, pitch, yaw
self.origin = o3d.geometry.TriangleMesh.create_coordinate_frame(size=10.0, origin=[0,0,0])
self.pose = o3d.geometry.TriangleMesh.create_coordinate_frame(size=10.0, origin=[position.location.x, -position.location.y, position.location.z])
def setup_camera(self):
"""
Spawns actor-camera to be used to render view.
Sets calibration for client-side boxes rendering.
"""
camera_transform = carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15))
self.camera = self.world.spawn_actor(self.camera_blueprint(), camera_transform, attach_to=self.car)
weak_self = weakref.ref(self)
self.camera.listen(lambda image: weak_self().set_image(weak_self, image))
calibration = np.identity(3)
calibration[0, 2] = VIEW_WIDTH / 2.0
calibration[1, 2] = VIEW_HEIGHT / 2.0
calibration[0, 0] = calibration[1, 1] = VIEW_WIDTH / (2.0 * np.tan(VIEW_FOV * np.pi / 360.0))
self.camera.calibration = calibration
def setup_depth_camera(self):
"""
Spawns actor-camera to be used to render view.
Sets calibration for client-side boxes rendering.
"""
depth_camera_transform = carla.Transform(carla.Location(z=3.0), carla.Rotation())
self.depth_camera = self.world.spawn_actor(self.depth_camera_blueprint(), depth_camera_transform, attach_to=self.car)
weak_depth_self = weakref.ref(self)
self.depth_camera.listen(lambda depth_image: weak_depth_self().set_depth_image(weak_depth_self, depth_image))
calibration = np.identity(3)
calibration[0, 2] = VIEW_WIDTH / 2.0
calibration[1, 2] = VIEW_HEIGHT / 2.0
calibration[0, 0] = calibration[1, 1] = VIEW_WIDTH / (2.0 * np.tan(VIEW_FOV * np.pi / 360.0))
self.depth_camera.calibration = calibration
def control(self, car):
"""
Applies control to main car based on pygame pressed keys.
Will return True If ESCAPE is hit, otherwise False to end main loop.
"""
keys = pygame.key.get_pressed()
if keys[K_ESCAPE]:
return True
control = car.get_control()
control.throttle = 0
if keys[K_w]:
control.throttle = 1
control.reverse = False
elif keys[K_s]:
control.throttle = 1
control.reverse = True
if keys[K_a]:
control.steer = max(-1., min(control.steer - 0.05, 0))
elif keys[K_d]:
control.steer = min(1., max(control.steer + 0.05, 0))
else:
control.steer = 0
control.hand_brake = keys[K_SPACE]
car.apply_control(control)
return False
@staticmethod
def set_image(weak_self, img):
"""
Sets image coming from camera sensor.
The self.capture flag is a mean of synchronization - once the flag is
set, next coming image will be stored.
"""
self = weak_self()
if self.capture:
self.image = img
self.capture = False
@staticmethod
def set_depth_image(weak_depth_self, depth_img):
"""
Sets image coming from camera sensor.
The self.capture flag is a mean of synchronization - once the flag is
set, next coming image will be stored.
"""
self = weak_depth_self()
if self.depth_capture:
self.depth_image = depth_img
self.depth_capture = False
def render(self, display):
"""
Transforms image from camera sensor and blits it to main pygame display.
"""
if self.image is not None:
array = np.frombuffer(self.image.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (self.image.height, self.image.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
display.blit(surface, (0, 0))
def depth_render(self, depth_display):
def pitopi(angle):
angle *= -1# - angle
#while angle > 180.0:
#angle -= 360.0
#while angle < - 180.0:
#angle += 360.0
return angle
position = self.car.get_transform()
x, y, z, roll, pitch, yaw = position.location.x, - position.location.y, position.location.z, position.rotation.roll, position.rotation.pitch, -position.rotation.yaw
dx, dy, dz, droll, dpitch, dyaw = x - self.last_position[0] , y - self.last_position[1], z - self.last_position[2], roll - self.last_position[3], pitch - self.last_position[4], yaw - self.last_position[5]
gamma,beta,alpha = radians(roll), radians(pitch), radians(yaw)
dgamma,dbeta,dalpha = radians(droll), radians(dpitch), radians(dyaw)#optimize
transformation_matrix = np.array([
[(cos(alpha) * cos(beta)), (cos(alpha) * sin(beta) * sin(gamma) - sin(alpha) * cos(gamma)), (cos(alpha)*sin(beta)*cos(gamma) + sin(alpha)*sin(gamma)), x],
[(sin(alpha) * cos(beta)), (sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma)), (sin(alpha)*sin(beta)*cos(gamma) - cos(alpha)*sin(gamma)), y],
[(-sin(beta)) , (cos(beta) * sin(gamma)) , (cos(beta) * cos(gamma)) , z],
[0,0,0,1]
])
rotation_matrix = np.array([
[(cos(dalpha) * cos(dbeta)), (cos(dalpha) * sin(dbeta) * sin(dgamma) - sin(dalpha) * cos(dgamma)), (cos(dalpha)*sin(dbeta)*cos(dgamma) + sin(dalpha)*sin(dgamma))],
[(sin(dalpha) * cos(dbeta)), (sin(dalpha) * sin(dbeta) * sin(dgamma) + cos(dalpha) * cos(dgamma)), (sin(dalpha)*sin(dbeta)*cos(dgamma) - cos(dalpha)*sin(dgamma))],
[(-sin(dbeta)) , (cos(dbeta) * sin(dgamma)) , (cos(dbeta) * cos(dgamma))]
])
translation_matrix = np.array([[x, y, z]]).T
self.last_position = x, y, z, roll, pitch, yaw
if self.depth_image is not None:
i = np.array(self.depth_image.raw_data)
i2 = i.reshape((VIEW_HEIGHT, VIEW_WIDTH, 4))
i3 = i2[:, :, :3]
H = i3.shape[0]
W = i3.shape[1]
far = 100.0
max_depth = 2.0
array = np.asarray(i3)
#array = image[:, :, ::-1]
# Apply (R + G * 256 + B * 256 * 256) / (256 * 256 * 256 - 1).
normalized_depth = np.dot(array[:, :, :3], [65536.0, 256.0, 1.0])
normalized_depth /= 16777215.0 # (256.0 * 256.0 * 256.0 - 1.0)
# (Intrinsic) K Matrix
k = np.identity(3)
k[0, 2] = W / 2.0
k[1, 2] = H / 2.0
k[0, 0] = k[1, 1] = W /(2.0 * tan(90 * pi / 360.0))
# 2d pixel coordinates
pixel_length = W * H
u_coord = repmat(np.r_[W-1:-1:-1],
H, 1).reshape(pixel_length)
v_coord = repmat(np.c_[H-1:-1:-1],
1, W).reshape(pixel_length)
normalized_depth = np.reshape(normalized_depth, pixel_length)
# Search for pixels where the depth is greater than max_depth to
# delete them
max_depth_indexes = np.where(normalized_depth > max_depth)
normalized_depth = np.delete(normalized_depth, max_depth_indexes)
u_coord = np.delete(u_coord, max_depth_indexes)
v_coord = np.delete(v_coord, max_depth_indexes)
# pd2 = [u,v,1]
p2d = np.array([u_coord, v_coord, np.ones_like(u_coord)])
p3d = np.dot(np.linalg.inv(k), p2d)
p3d *= normalized_depth * far
p = np.transpose(p3d)
py,pz,px,po = p[:, 0], p[:, 1] + 3.0, p[:, 2],np.ones(len(p))
P = np.vstack((px,py,pz)).T
point_cloud = P
self.pcd.points = o3d.utility.Vector3dVector(point_cloud)
self.pcd = self.pcd.voxel_down_sample(voxel_size= 0.5)
flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]
#self.pcd = self.pcd.translate(translation_matrix,relative=False)
#self.pcd = self.pcd.rotate(rotation_matrix)
#self.pcd.transform(flip_transform)
self.pcd.transform(transformation_matrix)
#self.all += self.pcd
#self.all = self.all.voxel_down_sample(voxel_size= 0.5)
#self.point_cloud.points = self.all.points
self.point_cloud.points = self.pcd.points
self.pose.translate(translation_matrix,relative=False)
self.pose.rotate(rotation_matrix)
if self.loop_number ==0:
self.depth_display.add_geometry(self.origin)
self.depth_display.add_geometry(self.pose)
self.depth_display.add_geometry(self.point_cloud)
def log(self):
position = self.car.get_transform()
self.loop_number +=1
x, y, z, roll, pitch, yaw = position.location.x, - position.location.y, position.location.z, position.rotation.roll, position.rotation.pitch, -position.rotation.yaw
pose = x, y, z, roll, pitch, yaw
print(pose)
#sys.stdout.write("\r{}".format(pose))
#sys.stdout.flush()
def game_loop(self):
"""
Main program loop.
"""
try:
pygame.init()
self.client = carla.Client('127.0.0.1', 2000)
self.client.set_timeout(2.0)
self.world = self.client.get_world()
self.setup_car()
self.setup_camera()
self.setup_depth_camera()
self.display = pygame.display.set_mode((VIEW_WIDTH, VIEW_HEIGHT), pygame.HWSURFACE | pygame.DOUBLEBUF)
self.depth_display = o3d.visualization.Visualizer()
self.depth_display.create_window(window_name = 'pointcloud_depth',width = VIEW_WIDTH*2 , height = VIEW_HEIGHT*2 )
self.depth_display.get_render_option()#.mesh_color_option = o3d.MeshColorOption.Normal
pygame_clock = pygame.time.Clock()
self.set_synchronous_mode(True)
vehicles = self.world.get_actors().filter('vehicle.*')
while True:
self.world.tick()
self.capture = True
self.depth_capture = True
pygame_clock.tick_busy_loop(30)
self.render(self.display)
pygame.display.flip()
pygame.event.pump()
self.depth_render(self.depth_display)
self.depth_display.update_geometry(self.origin)
self.depth_display.update_geometry(self.pose)
self.depth_display.update_geometry(self.point_cloud)
self.depth_display.poll_events()
self.depth_display.update_renderer()
self.log()
if self.control(self.car):
return
except Exception as e: print(e)
finally:
self.set_synchronous_mode(False)
self.camera.destroy()
self.depth_camera.destroy()
self.car.destroy()
self.depth_display.destroy_window()
pygame.quit()
# ==============================================================================
# -- main() --------------------------------------------------------------------
# ==============================================================================
def main():
"""
Initializes the client-side bounding box demo.
"""
try:
client = BasicSynchronousClient()
client.game_loop()
finally:
print('\n','EXIT')
if __name__ == '__main__':
main()
but having problem to transform the local pointcloud in global frame.
How can i construct the transformation matrix?
question
Saminhasan5220
All 2 comments
@DSantosO could you assist with this?
germanros1987
on 26 Jun 2020
Hello Saminhasan5220,
Thanks for your comment.
We have noticed a problem in the coordinate system in which the point cloud is given. It was not in the correct sensor frame. We have fixed the issue and it is currently under review in a PR so it should be in master very soon. You can check it already in the dsantos/lidar-transform branch. Also, with a several other improvements for the LiDAR sensor, it will be available in the next release.
In that PR, we have also exposed the transformation 4-matrix and its inverse so it is easier to transform from the sensor's frame to the world's one. I think with that fix and that matrix transformation you should be able to achieve your goal.
We will add an script example but I this should be helpful to do that in the mentioned branch:
points = np.frombuffer(image.raw_data, dtype=np.dtype('f4'))
points = np.reshape(points, (int(points.shape[0] / 3), 3))
points = np.append(points, np.ones((points.shape[0], 1)), axis=1)
transform = np.array(sensor.get_transform().get_matrix())
points = np.dot(transform, points.T).T
points = points[:, :-1]
DSantosO
on 26 Jun 2020
❤2
Was this page helpful?
0 / 5 - 0 ratings
Related issues
syinari0123
路
3Comments
kartikye
路
3Comments
tgrel
路
3Comments
jinfagang
路
3Comments
NYJiaxing
路
3Comments
Most helpful comment
Hello Saminhasan5220,
Thanks for your comment.
We have noticed a problem in the coordinate system in which the point cloud is given. It was not in the correct sensor frame. We have fixed the issue and it is currently under review in a PR so it should be in master very soon. You can check it already in the dsantos/lidar-transform branch. Also, with a several other improvements for the LiDAR sensor, it will be available in the next release.
In that PR, we have also exposed the transformation 4-matrix and its inverse so it is easier to transform from the sensor's frame to the world's one. I think with that fix and that matrix transformation you should be able to achieve your goal.
We will add an script example but I this should be helpful to do that in the mentioned branch:
points = np.frombuffer(image.raw_data, dtype=np.dtype('f4'))
points = np.reshape(points, (int(points.shape[0] / 3), 3))
points = np.append(points, np.ones((points.shape[0], 1)), axis=1)
transform = np.array(sensor.get_transform().get_matrix())
points = np.dot(transform, points.T).T
points = points[:, :-1]