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