def test_online_operation(self):
# Simulate an online operation during 1000 frames
# And cut-off every 100 frames into a new VCD
vcds = []
uid = -1
for frame_num in range(0, 1000):
if frame_num % 100 == 0:
# Create new VCD
vcds.append(core.VCD())
vcd_current = vcds[-1]
# Optionally we could here dump into JSON file
if frame_num == 0:
uid = vcd_current.add_object(
'CARLOTA', 'Car') # leave VCD to assign uid = 0
vcd_current.add_object_data(
uid, types.bbox('', (0, frame_num, 0, 0)), frame_num)
else:
uid = vcd_current.add_object(
'CARLOTA', 'Car', uid=uid) # tell VCD to use last_uid
vcd_current.add_object_data(
uid, types.bbox('', (0, frame_num, 0, 0)), frame_num)
else:
# Continue with current VCD
vcd_current = vcds[-1]
vcd_current.add_object_data(
uid, types.bbox('', (0, frame_num, 0, 0)), frame_num)
for vcd_this in vcds:
self.assertEqual(vcd_this.get_num_objects(), 1)
self.assertEqual(vcd_this.get_object(uid)['name'], 'CARLOTA')
self.assertEqual(vcd_this.get_object(uid)['type'], 'Car')
def test_create_search_simple(self):
# 1.- Create a VCD instance
vcd = core.VCD()
# 2.- Create the Object
uid_marcos = vcd.add_object(name='marcos', semantic_type="person")
self.assertEqual(uid_marcos, "0", "Should be 0")
# 3.- Add some data to the object
vcd.add_object_data(uid=uid_marcos, object_data=types.bbox(name='head', val=(10, 10, 30, 30)))
vcd.add_object_data(uid=uid_marcos, object_data=types.bbox(name='body', val=(0, 0, 60, 120)))
vcd.add_object_data(uid=uid_marcos, object_data=types.vec(name='speed', val=(0.0, 0.2)))
vcd.add_object_data(uid=uid_marcos, object_data=types.num(name='accel', val=0.1))
uid_peter = vcd.add_object(name='peter', semantic_type="person")
vcd.add_object_data(uid=uid_peter, object_data=types.num(name='age', val=38.0))
vcd.add_object_data(uid=uid_peter, object_data=types.vec(name='eyeL', val=(0, 0, 10, 10)))
vcd.add_object_data(uid=uid_peter, object_data=types.vec(name='eyeR', val=(0, 0, 10, 10)))
# 4.- Write into string
vcd_string_pretty = vcd.stringify()
vcd_string_nopretty = vcd.stringify(False)
# 5.- We can ask VCD
marcos_ref = vcd.get_element(element_type=core.ElementType.object, uid=uid_marcos)
# print('Found Object: uid = ', uid_marcos, ', name = ', marcosRef['name'])
self.assertEqual(uid_marcos, "0", "Should be 0")
self.assertEqual(marcos_ref['name'], 'marcos', "Should be marcos")
peter_ref = vcd.get_element(element_type=core.ElementType.object, uid=uid_peter)
# print('Found Object: uid = ', uid_peter, ', name = ', peterRef['name'])
self.assertEqual(uid_peter, "1", "Should be 1")
self.assertEqual(peter_ref['name'], 'peter', "Should be peter")
# print('VCD string no pretty:\n', vcd_string_nopretty)
# print('VCD string pretty:\n', vcd_string_pretty)
if not os.path.isfile('./etc/' + vcd_version_name + '_test_create_search_simple_nopretty.json'):
vcd.save('./etc/' + vcd_version_name + '_test_create_search_simple_nopretty.json')
vcd_file_nopretty = open('./etc/' + vcd_version_name + '_test_create_search_simple_nopretty.json', "r")
vcd_string_nopretty_read = vcd_file_nopretty.read()
self.assertEqual(vcd_string_nopretty_read, vcd_string_nopretty, "VCD no-pretty not equal to read file")
vcd_file_nopretty.close()
if not os.path.isfile('./etc/' + vcd_version_name + '_test_create_search_simple_pretty.json'):
vcd.save('./etc/' + vcd_version_name + '_test_create_search_simple_pretty.json', True)
vcd_file_pretty = open('./etc/' + vcd_version_name + '_test_create_search_simple_pretty.json', "r")
vcd_string_pretty_read = vcd_file_pretty.read()
self.assertEqual(vcd_string_pretty, vcd_string_pretty_read, "VCD pretty not equal to read file")
vcd_file_pretty.close()
def convert_town_center_to_VCD4():
if not os.path.isfile('./etc/TownCentreXVID_groundTruth.top'):
import urllib.request
url = 'https://www.robots.ox.ac.uk/ActiveVision/Research/Projects/2009bbenfold_headpose/Datasets/TownCentre-groundtruth.top'
urllib.request.urlretrieve(url, './etc/TownCentreXVID_groundTruth.top')
orig_file_name = "./etc/TownCentreXVID_groundTruth.top"
vcd = core.VCD()
with open(orig_file_name, newline='') as csvfile:
my_reader = csv.reader(csvfile, delimiter=',')
for row in my_reader:
personNumber = int(row[0])
frameNumber = int(row[1])
headValid = int(row[2])
bodyValid = int(row[3])
headLeft = float(row[4])
headTop = float(row[5])
headRight = float(row[6])
headBottom = float(row[7])
headWidth = float((int(1000*headRight) - int(1000*headLeft))/1000)
headHeight = float((int(1000*headBottom) - int(1000*headTop))/1000)
bodyLeft = float(row[8])
bodyTop = float(row[9])
bodyRight = float(row[10])
bodyBottom = float(row[11])
bodyWidth = float((int(1000*bodyRight) - int(1000*bodyLeft))/1000)
bodyHeight = float((int(1000*bodyBottom) - int(1000*bodyTop))/1000)
body = types.bbox(name="body",
val=(bodyLeft, bodyTop, bodyWidth, bodyHeight))
head = types.bbox("head", (headLeft, headTop, headWidth, headHeight))
if not vcd.has(core.ElementType.object, personNumber):
vcd.add_object(name="", semantic_type="Pedestrian",
uid=personNumber)
if bodyValid:
vcd.add_object_data(personNumber, body, frameNumber)
if headValid:
vcd.add_object_data(personNumber, head, frameNumber)
else:
if bodyValid:
vcd.add_object_data(personNumber, body, frameNumber)
if headValid:
vcd.add_object_data(personNumber, head, frameNumber)
vcd_json_file_name = "./etc/vcd420_towncenter.json"
vcd.save(vcd_json_file_name, False)
vcd_proto_file_name = "./etc/vcd420_proto_towncenter.txt"
serializer.json2proto_bin(vcd_json_file_name, vcd_proto_file_name)
def test_nested_object_data_attributes(self):
vcd = core.VCD()
uid_obj1 = vcd.add_object('someName1', '#Some')
box1 = types.bbox('head', (0, 0, 10, 10))
box1.add_attribute(types.boolean('visible', True))
self.assertEqual('attributes' in box1.data, True)
self.assertEqual('boolean' in box1.data['attributes'], True)
self.assertEqual(
type(box1.data['attributes']['boolean']) is list, True)
self.assertEqual(box1.data['attributes']['boolean'][0]['name'],
"visible")
self.assertEqual(box1.data['attributes']['boolean'][0]['val'], True)
vcd.add_object_data(uid_obj1, box1, 0)
if not os.path.isfile('./etc/vcd430_test_nested_object_data.json'):
vcd.save('./etc/vcd430_test_nested_object_data.json', True)
vcd_read = core.VCD('./etc/vcd430_test_nested_object_data.json',
validation=True)
vcd_read_stringified = vcd_read.stringify()
vcd_stringified = vcd.stringify()
# print(vcd_stringified)
self.assertEqual(vcd_read_stringified, vcd_stringified)
def vcd_add_object_debug():
time_0 = time.time()
vcd = core.VCD()
for frame_num in range(0, 10000):
if frame_num % 10 == 0:
uid = vcd.add_object('CARLOTA' + str(frame_num), '#Car')
vcd.add_object_data(uid, types.bbox("shape", (0, 0, 100, 200)), frame_value=frame_num)
time_1 = time.time()
elapsed_time_loop = time_1 - time_0
print("Loop: %s seconds. " % elapsed_time_loop)
def test_element_data_nested_same_name(self):
vcd = core.VCD()
uid1 = vcd.add_object('mike', '#Pedestrian')
body = types.bbox('body', (0, 0, 100, 150))
body.add_attribute(types.boolean('visible', True))
body.add_attribute(types.boolean('occluded', False))
body.add_attribute(types.boolean('visible', False)) # this is repeated, so it is substituted
vcd.add_object_data(uid1, body, (0, 5))
#self.assertEqual(vcd.stringify(False), '{"vcd":{"frames":{"0":{"objects":{"0":{"object_data":{"bbox":[{"name":"body","val":[0,0,100,150],"attributes":{"boolean":[{"name":"visible","val":false},{"name":"occluded","val":false}]}}]}}}},"1":{"objects":{"0":{"object_data":{"bbox":[{"name":"body","val":[0,0,100,150],"attributes":{"boolean":[{"name":"visible","val":false},{"name":"occluded","val":false}]}}]}}}},"2":{"objects":{"0":{"object_data":{"bbox":[{"name":"body","val":[0,0,100,150],"attributes":{"boolean":[{"name":"visible","val":false},{"name":"occluded","val":false}]}}]}}}},"3":{"objects":{"0":{"object_data":{"bbox":[{"name":"body","val":[0,0,100,150],"attributes":{"boolean":[{"name":"visible","val":false},{"name":"occluded","val":false}]}}]}}}},"4":{"objects":{"0":{"object_data":{"bbox":[{"name":"body","val":[0,0,100,150],"attributes":{"boolean":[{"name":"visible","val":false},{"name":"occluded","val":false}]}}]}}}},"5":{"objects":{"0":{"object_data":{"bbox":[{"name":"body","val":[0,0,100,150],"attributes":{"boolean":[{"name":"visible","val":false},{"name":"occluded","val":false}]}}]}}}}},"schema_version":"4.3.0","frame_intervals":[{"frame_start":0,"frame_end":5}],"objects":{"0":{"name":"mike","type":"#Pedestrian","frame_intervals":[{"frame_start":0,"frame_end":5}],"object_data_pointers":{"body":{"type":"bbox","frame_intervals":[{"frame_start":0,"frame_end":5}],"attributes":{"visible":"boolean","occluded":"boolean"}}}}}}}')
if not os.path.isfile('./etc/' + vcd_version_name + '_test_element_data_nested_same_name.json'):
vcd.save('./etc/' + vcd_version_name + '_test_element_data_nested_same_name.json')
def test_remove_simple(self):
# 1.- Create VCD
vcd = core.VCD()
# 2.- Create some objects
car1_uid = vcd.add_object(name='BMW', semantic_type='#Car')
car2_uid = vcd.add_object(name='Seat', semantic_type='#Car')
person1_uid = vcd.add_object(name='John', semantic_type='#Pedestrian')
trafficSign1UID = vcd.add_object(name='', semantic_type='#StopSign')
# 3.- Add some content
# Same FrameInterval (0, 5)
vcd.add_object_data(uid=person1_uid, object_data=types.bbox('face', (0, 0, 100, 100)), frame_value=(0, 5))
vcd.add_object_data(uid=person1_uid, object_data=types.bbox('mouth', (0, 0, 10, 10)), frame_value=(0, 5))
vcd.add_object_data(uid=person1_uid, object_data=types.bbox('hand', (0, 0, 30, 30)), frame_value=(0, 5))
vcd.add_object_data(uid=person1_uid, object_data=types.bbox('eyeL', (0, 0, 10, 10)), frame_value=(0, 5))
vcd.add_object_data(uid=person1_uid, object_data=types.bbox('eyeR', (0, 0, 10, 10)), frame_value=(0, 5))
# A different FrameInterval (0, 10)
vcd.add_object_data(uid=person1_uid, object_data=types.num('age', 35.0), frame_value=(0, 10))
# Data for the other objects
vcd.add_object_data(uid=car1_uid, object_data=types.bbox('position', (100, 100, 200, 400)), frame_value=(0, 10))
vcd.add_object_data(uid=car1_uid, object_data=types.text('color', 'red'), frame_value=(6, 10))
vcd.add_object_data(uid=car2_uid, object_data=types.bbox('position', (300, 1000, 200, 400)), frame_value=(0, 10))
vcd.add_object_data(uid=trafficSign1UID, object_data=types.boolean('visible', True), frame_value=(0, 4))
# print("Frame 5, dynamic only message: ", vcd.stringify_frame(5, dynamic_only=True))
# print("Frame 5, full message: ", vcd.stringify_frame(5, dynamic_only=False))
if not os.path.isfile('./etc/' + vcd_version_name + '_test_remove_simple.json'):
vcd.save('./etc/' + vcd_version_name + '_test_remove_simple.json')
self.assertEqual(vcd.get_num_objects(), 4, "Should be 4")
# 4.- Delete some content
vcd.rm_object(uid=car2_uid)
self.assertEqual(vcd.get_num_objects(), 3, "Should be 3")
vcd.rm_object_by_type(semantic_type='#StopSign')
self.assertEqual(vcd.get_num_objects(), 2, "Should be 2")
# 5.- Remove all content sequentially
vcd.rm_object(uid=person1_uid)
self.assertEqual(vcd.get_num_objects(), 1, "Should be 1")
vcd.rm_object(uid=car1_uid)
self.assertEqual(vcd.get_num_objects(), 0, "Should be 0")
self.assertEqual(vcd.get_frame_intervals().empty(), True)
def test_ontology_list(self):
vcd = core.VCD()
ont_uid_1 = vcd.add_ontology(
"http://www.vicomtech.org/viulib/ontology")
ont_uid_2 = vcd.add_ontology("http://www.alternativeURL.org/ontology")
# Let's create an object with a pointer to the ontology
uid_car = vcd.add_object('CARLOTA',
'#Car',
frame_value=None,
uid=None,
ont_uid=ont_uid_1)
vcd.add_object_data(uid_car, types.text('brand', 'Toyota'))
vcd.add_object_data(uid_car, types.text('model', 'Prius'))
uid_marcos = vcd.add_object('Marcos',
'#Person',
frame_value=None,
uid=None,
ont_uid=ont_uid_2)
vcd.add_object_data(uid_marcos, types.bbox('head', (10, 10, 30, 30)),
(2, 4))
self.assertEqual(vcd.get_object(uid_car)['ontology_uid'], ont_uid_1)
self.assertEqual(vcd.get_object(uid_marcos)['ontology_uid'], ont_uid_2)
self.assertEqual(vcd.get_ontology(ont_uid_1),
"http://www.vicomtech.org/viulib/ontology")
self.assertEqual(vcd.get_ontology(ont_uid_2),
"http://www.alternativeURL.org/ontology")
if not os.path.isfile('./etc/vcd430_test_ontology.json'):
vcd.save('./etc/vcd430_test_ontology.json', True)
vcd_read = core.VCD('./etc/vcd430_test_ontology.json', validation=True)
self.assertEqual(vcd_read.stringify(), vcd.stringify())
def __copy_elements(self, vcd_430, root, frame_num=None):
if 'objects' in root:
for object in root['objects']:
uid = str(object['uid']) # Let's convert to string here
name = object['name']
ontologyUID = None
if 'ontologyUID' in object:
ontologyUID = str(object['ontologyUID']) # Let's convert to string here
typeSemantic = object.get('type', '') # In VCD 4.3.0 type is required, but it VCD 3.3.0 seems to be not
if not vcd_430.has(core.ElementType.object, uid):
vcd_430.add_object(name, typeSemantic, frame_num, uid, ontologyUID)
if 'objectDataContainer' in object:
objectDataContainer = object['objectDataContainer']
for key, value in objectDataContainer.items():
for object_data in value:
inStream = None
if 'inStream' in object_data:
inStream = object_data['inStream']
if 'val' in object_data:
val = object_data['val']
currentObjectData = None
# Create main object_data body
# NOTE: in the following calls, I am using direct access to dictionary for required fields, e.g.
# object_data['name'], etc.
# For optional fields, I am using get() function, e.g. object_data.get('mode') which defaults to
# None
if key == 'num':
if len(val) == 1:
# Single value, this is a num
currentObjectData = types.num(object_data['name'], val[0], inStream)
else:
# Multiple values, this is a vec
currentObjectData = types.vec(object_data['name'], val, inStream)
elif key == 'bool':
currentObjectData = types.boolean(object_data['name'], val, inStream)
elif key == 'text':
currentObjectData = types.text(object_data['name'], val, inStream)
elif key == 'image':
currentObjectData = types.image(
object_data['name'], val,
object_data['mimeType'], object_data['encoding'],
inStream
)
elif key == 'binary':
currentObjectData = types.binary(
object_data['name'], val,
object_data['dataType'], object_data['encoding'],
inStream
)
elif key == 'vec':
currentObjectData = types.vec(object_data['name'], val, inStream)
elif key == 'bbox':
currentObjectData = types.bbox(object_data['name'], val, inStream)
elif key == 'cuboid':
currentObjectData = types.cuboid(object_data['name'], val, inStream)
elif key == 'mat':
currentObjectData = types.mat(
object_data['name'], val,
object_data['channels'], object_data['width'], object_data['height'],
object_data['dataType'],
inStream
)
elif key == 'point2D':
currentObjectData = types.point2d(object_data['name'], val, object_data.get('id'), inStream)
elif key == 'point3D':
currentObjectData = types.point3d(object_data['name'], val, object_data.get('id'), inStream)
elif key == "poly2D":
mode_int = object_data['mode']
currentObjectData = types.poly2d(
object_data['name'], val, types.Poly2DType(mode_int), object_data['closed'], inStream
)
elif key == "poly3D":
currentObjectData = types.poly3d(object_data['name'], val, object_data['closed'], inStream)
elif key == "mesh":
currentObjectData = types.mesh(object_data['name'])
if 'point3D' in object_data:
for p3d_330 in object_data['point3D']:
# Create a types.point3d object and add it to the mesh
id = p3d_330['id']
name = p3d_330['name']
val = p3d_330['val']
p3d_430 = types.point3d(name, val)
self.__add_attributes(p3d_330, p3d_430)
currentObjectData.add_vertex(p3d_430, id)
if 'lineReference' in object_data:
for lref_330 in object_data['lineReference']:
# Create a types.line_reference object and add it to the mesh
id = lref_330['id']
name = lref_330['name']
referenceType = lref_330['referenceType']
assert(referenceType == "point3D")
val = lref_330.get('val') # defaults to None, needed for the constructor
lref_430 = types.lineReference(name, val, types.ObjectDataType.point3d)
self.__add_attributes(lref_330, lref_430)
currentObjectData.add_edge(lref_430, id)
if 'areaReference' in object_data:
for aref_330 in object_data['areaReference']:
# Create a types.area_reference object and add it to the mesh
id = aref_330['id']
name = aref_330['name']
referenceType = aref_330['referenceType']
assert (referenceType == "point3D" or referenceType == "lineReference")
val = aref_330.get('val') # defaults to None, needed for the constructor
if referenceType == "point3D":
aref_430 = types.areaReference(name, val, types.ObjectDataType.point3d)
else:
aref_430 = types.areaReference(name, val, types.ObjectDataType.line_reference)
self.__add_attributes(aref_330, aref_430)
currentObjectData.add_area(aref_430, id)
# Add any attributes
self.__add_attributes(object_data, currentObjectData)
# Add the object_data to the object
vcd_430.add_object_data(uid, currentObjectData, frame_num)
if 'actions' in root:
for action in root['actions']:
uid = str(action['uid'])
name = action['name']
ontologyUID = None
if 'ontologyUID' in action:
ontologyUID = str(action['ontologyUID'])
typeSemantic = action.get('type', '') # required in VCD 4.0, not in VCD 3.3.0
vcd_430.add_action(name, typeSemantic, frame_num, uid, ontologyUID)
if 'events' in root:
for event in root['events']:
uid = str(event['uid'])
name = event['name']
ontologyUID = None
if 'ontologyUID' in event:
ontologyUID = str(event['ontologyUID'])
typeSemantic = event.get('type', '')
vcd_430.add_event(name, typeSemantic, frame_num, uid, ontologyUID)
if 'contexts' in root:
for context in root['contexts']:
uid = str(context['uid'])
name = context['name']
ontologyUID = None
if 'ontologyUID' in context:
ontologyUID = str(context['ontologyUID'])
typeSemantic = context.get('type', '')
vcd_430.add_context(name, typeSemantic, frame_num, uid, ontologyUID)
if 'relations' in root:
for relation in root['relations']:
uid = str(relation['uid'])
name = relation['name']
ontologyUID = None
if 'ontologyUID' in relation:
ontologyUID = str(relation['ontologyUID'])
predicate = relation.get('predicate', '')
rdf_objects = relation.get('rdf_objects', None)
rdf_subjects = relation.get('rdf_subjects', None)
vcd_430.add_relation(name, predicate, frame_value=frame_num, uid=uid, ont_uid=ontologyUID)
relation = vcd_430.get_element(core.ElementType.relation, uid)
if not 'rdf_objects' in relation or len(relation['rdf_objects']) == 0: # just add once
for rdf_object in rdf_objects:
element_type = None
rdf_object_type_str = rdf_object['type']
if rdf_object_type_str == "Object":
element_type = core.ElementType.object
elif rdf_object_type_str == "Action":
element_type = core.ElementType.action
elif rdf_object_type_str == "Event":
element_type = core.ElementType.event
elif rdf_object_type_str == "Context":
element_type = core.ElementType.context
else:
warnings.warn("ERROR: Unrecognized Element type. Must be Object, Action, Event or Context.")
vcd_430.add_rdf(uid, core.RDF.object, str(rdf_object['uid']), element_type)
if not 'rdf_subjects' in relation or len(relation['rdf_subjects']) == 0: # just add once
for rdf_subject in rdf_subjects:
element_type = None
rdf_object_type_str = rdf_subject['type']
if rdf_object_type_str == "Object":
element_type = core.ElementType.object
elif rdf_object_type_str == "Action":
element_type = core.ElementType.action
elif rdf_object_type_str == "Event":
element_type = core.ElementType.event
elif rdf_object_type_str == "Context":
element_type = core.ElementType.context
else:
warnings.warn("ERROR: Unrecognized Element type. Must be Object, Action, Event or Context.")
vcd_430.add_rdf(uid, core.RDF.subject, str(rdf_subject['uid']), element_type)
def parse_sequence(self, seq_number):
vcd = core.VCD()
#########################################
# OPEN files
#########################################
calib_file_name = os.path.join(self.kitti_tracking_calib_path,
str(seq_number).zfill(4) + ".txt")
oxts_file_name = os.path.join(self.kitti_tracking_oxts_path,
str(seq_number).zfill(4) + ".txt")
object_file_name = os.path.join(self.kitti_tracking_objects_path,
str(seq_number).zfill(4) + '.txt')
calib_file = open(calib_file_name, newline='')
oxts_file = open(oxts_file_name, newline='')
object_file = open(object_file_name, newline='')
calib_reader = csv.reader(calib_file, delimiter=' ')
oxts_reader = csv.reader(oxts_file, delimiter=' ')
object_reader = csv.reader(object_file, delimiter=' ')
#########################################
# READ calibration matrices
#########################################
img_width_px = 1236
img_height_px = 366 # these are rectified dimensions
calib_matrices = {}
for row in calib_reader:
calib_matrices[row[0]] = [
float(x) for x in row[1:] if len(x) > 0
] # filter out some spaces at the end of the row
left_camera_K3x4 = np.reshape(calib_matrices["P2:"], (3, 4))
right_camera_K3x4 = np.reshape(calib_matrices["P3:"], (3, 4))
camera_rectification_3x3 = np.reshape(calib_matrices["R_rect"], (3, 3))
transform_velo_to_camleft_3x4 = np.reshape(
calib_matrices["Tr_velo_cam"], (3, 4)) # WRT to LEFT CAMERA ONLY
#########################################
# LIDAR info
#########################################
# http://www.cvlibs.net/datasets/kitti/setup.php
location_velo_wrt_lcs_3x1 = np.array(
[[0.76], [0.0], [1.73]]) # according to the documentation
# Create pose (p=[[R|C],[0001]])
pose_velo_wrt_lcs_4x4 = utils.create_pose(
np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]),
location_velo_wrt_lcs_3x1)
transform_lcs_to_velo_4x4 = utils.inv(pose_velo_wrt_lcs_4x4)
vcd.add_stream(stream_name="VELO_TOP",
uri="",
description="Velodyne roof",
stream_type=core.StreamType.lidar)
vcd.add_stream_properties(
stream_name="VELO_TOP",
extrinsics=types.Extrinsics(
pose_scs_wrt_lcs_4x4=list(pose_velo_wrt_lcs_4x4.flatten())))
#########################################
# GPS/IMU info
#########################################
# Let's build also the pose of the imu
location_imu_wrt_lcs_4x4 = np.array([[-0.05], [0.32], [0.93]
]) # according to documentation
pose_imu_wrt_lcs_4x4 = utils.create_pose(
np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]),
location_imu_wrt_lcs_4x4)
vcd.add_stream(stream_name="IMU",
uri="",
description="GPS/IMU",
stream_type=core.StreamType.other)
vcd.add_stream_properties(
stream_name="IMU",
extrinsics=types.Extrinsics(
pose_scs_wrt_lcs_4x4=list(pose_imu_wrt_lcs_4x4.flatten())))
#########################################
# CAMERAS
#########################################
# From KITTI readme.txt:
# To project a point from Velodyne coordinates into the left color image,
# you can use this formula: x = P2 * R0_rect * Tr_velo_to_cam * y
# For the right color image: x = P3 * R0_rect * Tr_velo_to_cam * y
# Note: All matrices are stored row-major, i.e., the first values correspond
# to the first row. R0_rect contains a 3x3 matrix which you need to extend to
# a 4x4 matrix by adding a 1 as the bottom-right element and 0's elsewhere.
# Tr_xxx is a 3x4 matrix (R|t), which you need to extend to a 4x4 matrix
# in the same way!
# Virtually, cam_left and cam_right are defined as the same coordinate systems, so their scs are the same
# But their projection matrices (3x4) include a right-most non-zero column which shifts 3d points when projected
# into the images, that is why projecting from velodyne to left and right use the same "extrinsics", and just differ
# in the usage of the "intrinsic" matrices P2 and P3
# P2 and P3 might be decomposed so P2 = K2*T2 and P3=K3*T3, so T2 and T3 could host extrinsic information
# while K2 and K3 could host the intrinsic information. This way, the pose of cam_left would be T2*R_rect*Tr_velo
# However, such decomposition seems to be non-trivial.
# x = P2 * R0_rect * Tr_velo_to_cam * y
# x = P3 * R0_rect * Tr_velo_to_cam * y
camera_rectification_4x4 = np.vstack((np.hstack(
(camera_rectification_3x3, [[0], [0], [0]])), [0, 0, 0, 1]))
transform_velo_to_camleft_4x4 = np.vstack(
(transform_velo_to_camleft_3x4, [0, 0, 0, 1]))
transform_velo_to_camleft_4x4 = np.dot(
camera_rectification_4x4, transform_velo_to_camleft_4x4
) # such that X_cam = transform_velo_to_cam_4x4 * X_velo
# The pose of cameras can't be read from documentation, as these are virtual cameras created via a rectification
# process, therefore, we need to build them using the velo_to_cam calibration
# Pose_camLeft_wrt_ccs = RT_camLeft_to_ccs
transform_lcs_to_camleft_4x4 = np.dot(transform_velo_to_camleft_4x4,
transform_lcs_to_velo_4x4)
pose_camleft_wrt_lcs_4x4 = utils.inv(transform_lcs_to_camleft_4x4)
pose_camright_wrt_lcs_4x4 = pose_camleft_wrt_lcs_4x4
# Create cams and fill scene
vcd.add_stream(stream_name="CAM_LEFT",
uri="",
description="Virtual Left color camera",
stream_type=core.StreamType.camera)
vcd.add_stream_properties(
stream_name="CAM_LEFT",
intrinsics=types.IntrinsicsPinhole(width_px=img_width_px,
height_px=img_height_px,
camera_matrix_3x4=list(
left_camera_K3x4.flatten()),
distortion_coeffs_1xN=None),
extrinsics=types.Extrinsics(
pose_scs_wrt_lcs_4x4=list(pose_camleft_wrt_lcs_4x4.flatten())))
vcd.add_stream(stream_name="CAM_RIGHT",
uri="",
description="Virtual Right color camera",
stream_type=core.StreamType.camera)
vcd.add_stream_properties(
stream_name="CAM_RIGHT",
intrinsics=types.IntrinsicsPinhole(
width_px=img_width_px,
height_px=img_height_px,
camera_matrix_3x4=list(right_camera_K3x4.flatten()),
distortion_coeffs_1xN=None),
extrinsics=types.Extrinsics(pose_scs_wrt_lcs_4x4=list(
pose_camright_wrt_lcs_4x4.flatten())))
#########################################
# ODOMETRY
#########################################
oxts = []
for row in oxts_reader:
row = row[0:len(row) - 1]
floats = [float(i) for i in row]
oxts.append(floats)
'''lat_deg = row[0] # deg
lon_deg = row[1]
alt_deg = row[2]
roll_rad = row[3] # 0 = level, positive = left side up (-pi..pi)
pitch_rad = row[4] # 0 = level, positive = front down (-pi/2..pi/2)
yaw_rad = row[5] # 0 = east, positive = counter clockwise (-pi..pi)
vn = row[6] # velocity towards north(m / s)
ve = row[7] # velocity towards east(m / s)
vf = row[8] # forward velocity, i.e.parallel to earth - surface(m / s)
vl = row[9] # leftward velocity, i.e.parallel to earth - surface(m / s)
vu = row[10] # upward velocity, i.e.perpendicular to earth - surface(m / s)
ax = row[11] # acceleration in x, i.e. in direction of vehicle front(m / s ^ 2)
ay = row[12] # acceleration in y, i.e. in direction of vehicle left(m / s ^ 2)
az = row[13] # acceleration in z, i.e. in direction of vehicle top(m / s ^ 2)
af = row[14] # forward acceleration(m / s ^ 2)
al = row[15] # leftward acceleration(m / s ^ 2)
au = row[16] # upward acceleration(m / s ^ 2)
wx = row[17] # angular rate around x(rad / s)
wy = row[18] # angular rate around y(rad / s)
wz = row[19] # angular rate around z(rad / s)
wf = row[20] # angular rate around forward axis(rad / s)
wl = row[21] # angular rate around leftward axis(rad / s)
wu = row[22] # angular rate around upward axis(rad / s)
posacc = row[23] # velocity accuracy(north / east in m)
velacc = row[24] # velocity accuracy(north / east in m / s)
navstat = row[25] # navigation status
numsats = row[26] # number of satellites tracked by primary GPS receiver
posmode = row[27] # position mode of primary GPS receiver
velmode = row[28] # velocity mode of primary GPS receiver
orimode = row[29] # orientation mode of primary GPS receiver
'''
# Convert odometry (GPS) to poses
odometry_4x4xN = utils.convert_oxts_to_pose(oxts)
# An odometry entry is a 4x4 pose matrix of the lcs wrt wcs
# poses_4x4xN_lcs_wrt_wcs = odometry_4x4xN
frames_1xN = np.arange(0, odometry_4x4xN.shape[2], 1).reshape(
(1, odometry_4x4xN.shape[2]))
r, c = frames_1xN.shape
for i in range(0, c):
vcd.add_odometry(
int(frames_1xN[0, i]),
types.Odometry(
pose_lcs_wrt_wcs_4x4=list(odometry_4x4xN[:, :,
i].flatten())))
#########################################
# LABELS
#########################################
for row in object_reader:
frameNum = int(row[0])
trackID = int(row[1]) + 1 # VCD can't handle negative ids
semantic_class = row[2]
truncated = utils.float_2dec(float(row[3]))
occluded = int(row[4])
alpha = utils.float_2dec(float(row[5]))
left = utils.float_2dec(float(row[6]))
top = utils.float_2dec(float(row[7]))
width = utils.float_2dec(float(row[8]) - left)
height = utils.float_2dec(float(row[9]) - top)
bounding_box = types.bbox(name="",
val=(left, top, width, height),
stream='CAM_LEFT')
dimHeight = utils.float_2dec(float(row[10]))
dimWidth = utils.float_2dec(float(row[11]))
dimLength = utils.float_2dec(float(row[12]))
locX = utils.float_2dec(float(row[13]))
locY = utils.float_2dec(float(row[14]))
locZ = utils.float_2dec(float(row[15]))
rotY = utils.float_2dec(float(row[16]))
# Note KITTI uses (h, w, l, x, y, z, ry) for cuboids, in camera coordinates (X-to-right, Y-to-bottom, Z-to-front)
# while in VCD (x,y,z, rx, ry, rz, sx, sy, sz) is defined as a dextrogire system
# To express the cuboid in LCS (Local-Coordinate-System), we can add the pose of the camera
# Cameras are 1.65 m height wrt ground
# Cameras are 1.03 meters wrt to rear axle
cam_wrt_rear_axle_z = 1.03
cam_height = 1.65
cuboid = types.cuboid(
name="",
val=(utils.float_2dec(locZ + cam_wrt_rear_axle_z),
utils.float_2dec(-locX),
utils.float_2dec(-locY + cam_height), 0, 0,
utils.float_2dec(rotY), utils.float_2dec(dimWidth),
utils.float_2dec(dimLength), utils.float_2dec(dimHeight)))
# Note that if no "stream" parameter is given to this cuboid, LCS is assumed
if not vcd.has(core.ElementType.object, trackID):
vcd.add_object(name="",
semantic_type=semantic_class,
uid=trackID)
vcd.add_object_data(trackID, bounding_box, frameNum)
if semantic_class != "DontCare":
vcd.add_object_data(trackID, cuboid, frameNum)
vcd.add_object_data(trackID,
types.num(name="truncated", val=truncated),
frameNum)
vcd.add_object_data(trackID,
types.num(name="occluded", val=occluded),
frameNum)
vcd.add_object_data(trackID, types.num(name="alpha", val=alpha),
frameNum)
# Return
return vcd
def parse_sequence_direct(self, seq_number):
# This is a variant approach for creating a VCD 4.3.0 file reading the KITTI calibration files,
# trying to avoid additional computation at this level, and exploiting the ability of VCD 4.3.0 to
# express arbitrary transforms across coordinate systems
vcd = core.VCD()
#########################################
# OPEN files
#########################################
calib_file_name = os.path.join(self.kitti_tracking_calib_path,
str(seq_number).zfill(4) + ".txt")
oxts_file_name = os.path.join(self.kitti_tracking_oxts_path,
str(seq_number).zfill(4) + ".txt")
object_file_name = os.path.join(self.kitti_tracking_objects_path,
str(seq_number).zfill(4) + '.txt')
calib_file = open(calib_file_name, newline='')
oxts_file = open(oxts_file_name, newline='')
object_file = open(object_file_name, newline='')
calib_reader = csv.reader(calib_file, delimiter=' ')
oxts_reader = csv.reader(oxts_file, delimiter=' ')
object_reader = csv.reader(object_file, delimiter=' ')
#########################################
# CREATE base coordinate system
#########################################
# The main coordinate system for the scene "odom" represents a static cs (which coincides with first local cs).
vcd.add_coordinate_system("odom",
cs_type=types.CoordinateSystemType.scene_cs)
#########################################
# CREATE vehicle coordinate system
#########################################
# Local coordinate system, moving with the vehicle. Following iso8855 (x-front, y-left, z-up)
vcd.add_coordinate_system("vehicle-iso8855",
cs_type=types.CoordinateSystemType.local_cs,
parent_name="odom")
# Sensor coordinate systems are added
# Add transforms for each time instant
odometry_4x4xN = self.read_odometry_from_oxts(oxts_reader)
# An odometry entry is a 4x4 pose matrix of the lcs wrt wcs (ergo a transform lcs_to_wcs)
# poses_4x4xN_lcs_wrt_wcs = odometry_4x4xN
frames_1xN = np.arange(0, odometry_4x4xN.shape[2], 1).reshape(
(1, odometry_4x4xN.shape[2]))
r, c = frames_1xN.shape
for i in range(0, c):
vcd.add_transform(int(frames_1xN[0, i]),
transform=types.Transform(
src_name="vehicle-iso8855",
dst_name="odom",
transform_src_to_dst_4x4=list(
odometry_4x4xN[:, :, i].flatten())))
#########################################
# CREATE SENSORS coordinate system: LASER
#########################################
# http://www.cvlibs.net/datasets/kitti/setup.php
location_velo_wrt_vehicle_3x1 = np.array(
[[0.76], [0.0], [1.73]]) # according to the documentation
pose_velo_wrt_vehicle_4x4 = utils.create_pose(
utils.identity(3), location_velo_wrt_vehicle_3x1)
vcd.add_stream(stream_name="VELO_TOP",
uri="",
description="Velodyne roof",
stream_type=core.StreamType.lidar)
vcd.add_coordinate_system("VELO_TOP",
cs_type=types.CoordinateSystemType.sensor_cs,
parent_name="vehicle-iso8855",
pose_wrt_parent=list(
pose_velo_wrt_vehicle_4x4.flatten()))
#########################################
# CREATE SENSORS coordinate system: GPS/IMU
#########################################
# Let's build also the pose of the imu
location_imu_wrt_vehicle_4x4 = np.array(
[[-0.05], [0.32], [0.93]]) # according to documentation
pose_imu_wrt_vehicle_4x4 = utils.create_pose(
utils.identity(3), location_imu_wrt_vehicle_4x4)
vcd.add_stream(stream_name="IMU",
uri="",
description="GPS/IMU",
stream_type=core.StreamType.other)
vcd.add_coordinate_system("IMU",
cs_type=types.CoordinateSystemType.sensor_cs,
parent_name="vehicle-iso8855",
pose_wrt_parent=list(
pose_imu_wrt_vehicle_4x4.flatten()))
#########################################
# CREATE SENSORS coordinate system: CAM
#########################################
img_width_px = 1242
img_height_px = 375 # these are rectified dimensions
calib_matrices = {}
for row in calib_reader:
calib_matrices[row[0]] = [
float(x) for x in row[1:] if len(x) > 0
] # filter out some spaces at the end of the row
# From KITTI readme.txt:
# To project a point from Velodyne coordinates into the left color image,
# you can use this formula: x = P2 * R0_rect * Tr_velo_to_cam * y
# For the right color image: x = P3 * R0_rect * Tr_velo_to_cam * y
left_camera_K3x4 = np.reshape(calib_matrices["P2:"], (3, 4))
right_camera_K3x4 = np.reshape(calib_matrices["P3:"], (3, 4))
camera_rectification_3x3 = np.reshape(calib_matrices["R_rect"], (3, 3))
transform_velo_to_camleft_3x4 = np.reshape(
calib_matrices["Tr_velo_cam"], (3, 4)) # WRT to LEFT CAMERA ONLY
camera_rectification_4x4 = np.vstack((np.hstack(
(camera_rectification_3x3, [[0], [0], [0]])), [0, 0, 0, 1]))
transform_velo_to_camleft_4x4 = np.vstack(
(transform_velo_to_camleft_3x4, [0, 0, 0, 1]))
transform_velo_to_camleft_4x4 = np.dot(
camera_rectification_4x4, transform_velo_to_camleft_4x4
) # such that X_cam = transform_velo_to_cam_4x4 * X_velo
pose_camleft_wrt_velo_4x4 = utils.inv(transform_velo_to_camleft_4x4)
vcd.add_stream(stream_name="CAM_LEFT",
uri="",
description="Virtual Left color camera",
stream_type=core.StreamType.camera)
vcd.add_stream_properties(stream_name="CAM_LEFT",
intrinsics=types.IntrinsicsPinhole(
width_px=img_width_px,
height_px=img_height_px,
camera_matrix_3x4=list(
left_camera_K3x4.flatten()),
distortion_coeffs_1xN=None))
vcd.add_coordinate_system("CAM_LEFT",
cs_type=types.CoordinateSystemType.sensor_cs,
parent_name="VELO_TOP",
pose_wrt_parent=list(
pose_camleft_wrt_velo_4x4.flatten()))
# Virtually, cam_left and cam_right are defined as the same coordinate systems, so their scs are the same
# But their projection matrices (3x4) include a right-most non-zero column which shifts 3d points when projected
# into the images, that is why projecting from velodyne to left and right use the same "extrinsics", and just differ
# in the usage of the "intrinsic" matrices P2 and P3
# P2 and P3 might be decomposed so P2 = K2*T2 and P3=K3*T3, so T2 and T3 could host extrinsic information
# while K2 and K3 could host the intrinsic information. This way, the pose of cam_left would be T2*R_rect*Tr_velo
# However, such decomposition seems to be non-trivial.
# x = P2 * R0_rect * Tr_velo_to_cam * y
# x = P3 * R0_rect * Tr_velo_to_cam * y
vcd.add_stream(stream_name="CAM_RIGHT",
uri="",
description="Virtual Right color camera",
stream_type=core.StreamType.camera)
vcd.add_stream_properties(stream_name="CAM_RIGHT",
intrinsics=types.IntrinsicsPinhole(
width_px=img_width_px,
height_px=img_height_px,
camera_matrix_3x4=list(
right_camera_K3x4.flatten()),
distortion_coeffs_1xN=None))
vcd.add_coordinate_system("CAM_RIGHT",
cs_type=types.CoordinateSystemType.sensor_cs,
parent_name="VELO_TOP",
pose_wrt_parent=list(
pose_camleft_wrt_velo_4x4.flatten()))
#########################################
# LABELS
#########################################
for row in object_reader:
frameNum = int(row[0])
#trackID = int(row[1]) + 1 # VCD can't handle negative ids
trackID = int(row[1])
#if trackID == 0:
# continue # Let's ignore DontCare labels
semantic_class = row[2]
truncated = utils.float_2dec(float(row[3]))
occluded = int(row[4])
alpha = utils.float_2dec(float(
row[5])) # this is the observation angle (see cs_overview.pdf)
left = utils.float_2dec(float(row[6]))
top = utils.float_2dec(float(row[7]))
width = utils.float_2dec(float(row[8]) - left)
height = utils.float_2dec(float(row[9]) - top)
if trackID == -1: # This is DontCare, there are multiple boxes
count = vcd.get_element_data_count_per_type(
core.ElementType.object, trackID,
types.ObjectDataType.bbox, frameNum)
name_box = "box2D" + str(count)
else:
name_box = "box2D"
bounding_box = types.bbox(name=name_box,
val=(left + width / 2, top + height / 2,
width, height),
coordinate_system='CAM_LEFT')
# see cs_overview.pdf
dimH = utils.float_2dec(float(row[10]))
dimW = utils.float_2dec(float(row[11]))
dimL = utils.float_2dec(float(row[12]))
locX = utils.float_2dec(float(row[13]))
locY = utils.float_2dec(float(row[14]))
locZ = utils.float_2dec(float(row[15]))
rotY = utils.float_2dec(float(row[16]))
# Note KITTI uses (h, w, l, x, y, z, ry) for cuboids, in camera coordinates (X-to-right, Y-to-bottom, Z-to-front)
# while in VCD (x, y, z, rx, ry, rz, sx, sy, sz) is defined as a dextrogire system, centroid-based
# NOTE: changing locY by locY + dimH/2 as VCD uses centroid and KITTI uses bottom face
# NOTE: All in Camera coordinate system
# NOTE: x = length, y = height, z = width because of convention in readme.txt
# The reference point for the 3D bounding box for each object is centered on the
# bottom face of the box. The corners of bounding box are computed as follows with
# respect to the reference point and in the object coordinate system:
# x_corners = [l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2]^T
# y_corners = [0, 0, 0, 0, -h, -h, -h, -h ]^T
# z_corners = [w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2 ]^T
# with l=length, h=height, and w=width.
cuboid = types.cuboid(name="box3D",
val=(utils.float_2dec(locX),
utils.float_2dec(locY - dimH / 2),
utils.float_2dec(locZ), 0,
utils.float_2dec(rotY), 0,
utils.float_2dec(dimL),
utils.float_2dec(dimH),
utils.float_2dec(dimW)),
coordinate_system="CAM_LEFT")
if not vcd.has(core.ElementType.object, str(trackID)):
# First time
if trackID >= 0:
vcd.add_object(name=semantic_class + str(trackID),
semantic_type=semantic_class,
uid=str(trackID),
frame_value=frameNum)
else: # so this is for DontCare object
vcd.add_object(name=semantic_class,
semantic_type=semantic_class,
uid=str(trackID),
frame_value=frameNum)
vcd.add_object_data(str(trackID), bounding_box, frameNum)
vcd.add_object_data(str(trackID), cuboid, frameNum)
vcd.add_object_data(trackID,
types.num(name="truncated", val=truncated),
frameNum)
vcd.add_object_data(trackID,
types.num(name="occluded", val=occluded),
frameNum)
vcd.add_object_data(trackID, types.num(name="alpha", val=alpha),
frameNum)
#########################################
# Ego-vehicle
#########################################
vcd.add_object(name="Egocar", semantic_type="Egocar", uid=str(-2))
cuboid_ego = types.cuboid(name="box3D",
val=(1.35, 0.0, 0.736, 0.0, 0.0, 0.0, 4.765,
1.82, 1.47),
coordinate_system="vehicle-iso8855")
vcd.add_object_data(str(-2), cuboid_ego)
# Return
return vcd