@ -1,10 +1,7 @@
import numpy as np
import numpy as np
from common . transformations . camera import ( FULL_FRAME_SIZE ,
from common . transformations . camera import ( FULL_FRAME_SIZE ,
FOCAL ,
get_view_frame_from_calib_frame )
get_view_frame_from_road_frame ,
get_view_frame_from_calib_frame ,
vp_from_ke )
# segnet
# segnet
SEGNET_SIZE = ( 512 , 384 )
SEGNET_SIZE = ( 512 , 384 )
@ -14,21 +11,6 @@ def get_segnet_frame_from_camera_frame(segnet_size=SEGNET_SIZE, full_frame_size=
[ 0.0 , float ( segnet_size [ 1 ] ) / full_frame_size [ 1 ] ] ] )
[ 0.0 , float ( segnet_size [ 1 ] ) / full_frame_size [ 1 ] ] ] )
segnet_frame_from_camera_frame = get_segnet_frame_from_camera_frame ( ) # xx
segnet_frame_from_camera_frame = get_segnet_frame_from_camera_frame ( ) # xx
# model
MODEL_INPUT_SIZE = ( 320 , 160 )
MODEL_YUV_SIZE = ( MODEL_INPUT_SIZE [ 0 ] , MODEL_INPUT_SIZE [ 1 ] * 3 / / 2 )
MODEL_CX = MODEL_INPUT_SIZE [ 0 ] / 2.
MODEL_CY = 21.
model_fl = 728.0
model_height = 1.22
# canonical model transform
model_intrinsics = np . array ( [
[ model_fl , 0.0 , MODEL_CX ] ,
[ 0.0 , model_fl , MODEL_CY ] ,
[ 0.0 , 0.0 , 1.0 ] ] )
# MED model
# MED model
MEDMODEL_INPUT_SIZE = ( 512 , 256 )
MEDMODEL_INPUT_SIZE = ( 512 , 256 )
@ -63,104 +45,73 @@ sbigmodel_intrinsics = np.array([
[ 0.0 , sbigmodel_fl , 0.5 * ( 256 + MEDMODEL_CY ) ] ,
[ 0.0 , sbigmodel_fl , 0.5 * ( 256 + MEDMODEL_CY ) ] ,
[ 0.0 , 0.0 , 1.0 ] ] )
[ 0.0 , 0.0 , 1.0 ] ] )
model_frame_from_road_frame = np . dot ( model_intrinsics ,
get_view_frame_from_road_frame ( 0 , 0 , 0 , model_height ) )
bigmodel_frame_from_road_frame = np . dot ( bigmodel_intrinsics ,
get_view_frame_from_road_frame ( 0 , 0 , 0 , model_height ) )
bigmodel_frame_from_calib_frame = np . dot ( bigmodel_intrinsics ,
bigmodel_frame_from_calib_frame = np . dot ( bigmodel_intrinsics ,
get_view_frame_from_calib_frame ( 0 , 0 , 0 , 0 ) )
get_view_frame_from_calib_frame ( 0 , 0 , 0 , 0 ) )
sbigmodel_frame_from_road_frame = np . dot ( sbigmodel_intrinsics ,
get_view_frame_from_road_frame ( 0 , 0 , 0 , model_height ) )
sbigmodel_frame_from_calib_frame = np . dot ( sbigmodel_intrinsics ,
sbigmodel_frame_from_calib_frame = np . dot ( sbigmodel_intrinsics ,
get_view_frame_from_calib_frame ( 0 , 0 , 0 , 0 ) )
get_view_frame_from_calib_frame ( 0 , 0 , 0 , 0 ) )
medmodel_frame_from_road_frame = np . dot ( medmodel_intrinsics ,
get_view_frame_from_road_frame ( 0 , 0 , 0 , model_height ) )
medmodel_frame_from_calib_frame = np . dot ( medmodel_intrinsics ,
medmodel_frame_from_calib_frame = np . dot ( medmodel_intrinsics ,
get_view_frame_from_calib_frame ( 0 , 0 , 0 , 0 ) )
get_view_frame_from_calib_frame ( 0 , 0 , 0 , 0 ) )
model_frame_from_bigmodel_frame = np . dot ( model_intrinsics , np . linalg . inv ( bigmodel_intrinsics ) )
medmodel_frame_from_bigmodel_frame = np . dot ( medmodel_intrinsics , np . linalg . inv ( bigmodel_intrinsics ) )
medmodel_frame_from_bigmodel_frame = np . dot ( medmodel_intrinsics , np . linalg . inv ( bigmodel_intrinsics ) )
# 'camera from model camera'
### This function mimics the update_calibration logic in modeld.cc
def get_model_height_transform ( camera_frame_from_road_frame , height ) :
### Manually verified to give similar results to xx.uncommon.utils.transform_img
camera_frame_from_road_ground = np . dot ( camera_frame_from_road_frame , np . array ( [
def get_warp_matrix ( rpy_calib , wide_cam = False , big_model = False , tici = True ) :
[ 1 , 0 , 0 ] ,
from common . transformations . orientation import rot_from_euler
[ 0 , 1 , 0 ] ,
from common . transformations . camera import view_frame_from_device_frame , eon_fcam_intrinsics , tici_ecam_intrinsics , tici_fcam_intrinsics
[ 0 , 0 , 0 ] ,
[ 0 , 0 , 1 ] ,
] ) )
camera_frame_from_road_high = np . dot ( camera_frame_from_road_frame , np . array ( [
[ 1 , 0 , 0 ] ,
[ 0 , 1 , 0 ] ,
[ 0 , 0 , height - model_height ] ,
[ 0 , 0 , 1 ] ,
] ) )
road_high_from_camera_frame = np . linalg . inv ( camera_frame_from_road_high )
high_camera_from_low_camera = np . dot ( camera_frame_from_road_ground , road_high_from_camera_frame )
return high_camera_from_low_camera
# camera_frame_from_model_frame aka 'warp matrix'
# was: calibration.h/CalibrationTransform
def get_camera_frame_from_model_frame ( camera_frame_from_road_frame , height = model_height , camera_fl = FOCAL ) :
vp = vp_from_ke ( camera_frame_from_road_frame )
model_zoom = camera_fl / model_fl
model_camera_from_model_frame = np . array ( [
[ model_zoom , 0.0 , vp [ 0 ] - MODEL_CX * model_zoom ] ,
[ 0.0 , model_zoom , vp [ 1 ] - MODEL_CY * model_zoom ] ,
[ 0.0 , 0.0 , 1.0 ] ,
] )
# This function is super slow, so skip it if height is very close to canonical
# TODO: speed it up!
if abs ( height - model_height ) > 0.001 :
camera_from_model_camera = get_model_height_transform ( camera_frame_from_road_frame , height )
else :
camera_from_model_camera = np . eye ( 3 )
return np . dot ( camera_from_model_camera , model_camera_from_model_frame )
def get_camera_frame_from_medmodel_frame ( camera_frame_from_road_frame ) :
camera_frame_from_ground = camera_frame_from_road_frame [ : , ( 0 , 1 , 3 ) ]
medmodel_frame_from_ground = medmodel_frame_from_road_frame [ : , ( 0 , 1 , 3 ) ]
ground_from_medmodel_frame = np . linalg . inv ( medmodel_frame_from_ground )
if tici and wide_cam :
camera_frame_from_medmodel_frame = np . dot ( camera_frame_from_ground , ground_from_medmodel_frame )
intrinsics = tici_ecam_intrinsics
elif tici :
intrinsics = tici_fcam_intrinsics
else :
intrinsics = eon_fcam_intrinsics
return camera_frame_from_medmodel_frame
if big_model :
sbigmodel_from_calib = sbigmodel_frame_from_calib_frame [ : , ( 0 , 1 , 2 ) ]
calib_from_model = np . linalg . inv ( sbigmodel_from_calib )
else :
medmodel_from_calib = medmodel_frame_from_calib_frame [ : , ( 0 , 1 , 2 ) ]
calib_from_model = np . linalg . inv ( medmodel_from_calib )
device_from_calib = rot_from_euler ( rpy_calib )
camera_from_calib = intrinsics . dot ( view_frame_from_device_frame . dot ( device_from_calib ) )
warp_matrix = camera_from_calib . dot ( calib_from_model )
return warp_matrix
def get_camera_frame_from_bigmodel_frame ( camera_frame_from_road_frame ) :
### This is old, just for debugging
camera_frame_from_ground = camera_frame_from_road_frame [ : , ( 0 , 1 , 3 ) ]
def get_warp_matrix_old ( rpy_calib , wide_cam = False , big_model = False , tici = True ) :
bigmodel_frame_from_ground = bigmodel_frame_from_road_frame [ : , ( 0 , 1 , 3 ) ]
from common . transformations . orientation import rot_from_euler
from common . transformations . camera import view_frame_from_device_frame , eon_fcam_intrinsics , tici_ecam_intrinsics , tici_fcam_intrinsics
ground_from_bigmodel_frame = np . linalg . inv ( bigmodel_frame_from_ground )
camera_frame_from_bigmodel_frame = np . dot ( camera_frame_from_ground , ground_from_bigmodel_frame )
return camera_frame_from_bigmodel_frame
def get_view_frame_from_road_frame ( roll , pitch , yaw , height ) :
device_from_road = rot_from_euler ( [ roll , pitch , yaw ] ) . dot ( np . diag ( [ 1 , - 1 , - 1 ] ) )
view_from_road = view_frame_from_device_frame . dot ( device_from_road )
return np . hstack ( ( view_from_road , [ [ 0 ] , [ height ] , [ 0 ] ] ) )
if tici and wide_cam :
intrinsics = tici_ecam_intrinsics
elif tici :
intrinsics = tici_fcam_intrinsics
else :
intrinsics = eon_fcam_intrinsics
def get_model_frame ( snu_full , camera_frame_from_model_frame , size ) :
model_height = 1.22
idxs = camera_frame_from_model_frame . dot ( np . column_stack ( [ np . tile ( np . arange ( size [ 0 ] ) , size [ 1 ] ) ,
if big_model :
np . tile ( np . arange ( size [ 1 ] ) , ( size [ 0 ] , 1 ) ) . T . flatten ( ) ,
model_from_road = np . dot ( sbigmodel_intrinsics ,
np . ones ( size [ 0 ] * size [ 1 ] ) ] ) . T ) . T . astype ( int )
get_view_frame_from_road_frame ( 0 , 0 , 0 , model_height ) )
calib_flat = snu_full [ idxs [ : , 1 ] , idxs [ : , 0 ] ]
if len ( snu_full . shape ) == 3 :
calib = calib_flat . reshape ( ( size [ 1 ] , size [ 0 ] , 3 ) )
elif len ( snu_full . shape ) == 2 :
calib = calib_flat . reshape ( ( size [ 1 ] , size [ 0 ] ) )
else :
else :
raise ValueError ( " shape of input img is weird " )
model_from_road = np . dot ( medmodel_intrinsics ,
return calib
get_view_frame_from_road_frame ( 0 , 0 , 0 , model_height ) )
ground_from_model = np . linalg . inv ( model_from_road [ : , ( 0 , 1 , 3 ) ] )
E = get_view_frame_from_road_frame ( * rpy_calib , 1.22 )
camera_frame_from_road_frame = intrinsics . dot ( E )
camera_frame_from_ground = camera_frame_from_road_frame [ : , ( 0 , 1 , 3 ) ]
warp_matrix = camera_frame_from_ground . dot ( ground_from_model )
return warp_matrix