mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 00:03:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cleanup calibration code (#25119)

Browse Source 
* First attempt

* worksish

* tests pass

* cleanup

* get rid of garbahe

* fix name

* Still used in xx

* add debug functions

* used

* Revert "used"

This reverts commit 276e8ebab06d2d4f0e9927ba32b7d8aca2bf88c3.

* Update ref
old-commit-hash: 772b748689cff4d6dc6aab6a8a20247b29a86056
This commit is contained in:
HaraldSchafer
2022-08-01 20:35:14 -07:00
committed by GitHub
parent 85b4328085
commit e5589e572f
7 changed files with 90 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/transformations/camera.py
+1 -14
View File
@@ -77,6 +77,7 @@ def get_view_frame_from_road_frame(roll, pitch, yaw, height):
return np.hstack((view_from_road, [[0], [height], [0]]))
# aka 'extrinsic_matrix'
def get_view_frame_from_calib_frame(roll, pitch, yaw, height):
device_from_calib= orient.rot_from_euler([roll, pitch, yaw])
@@ -94,12 +95,6 @@ def vp_from_ke(m):
return (m[0, 0]/m[2, 0], m[1, 0]/m[2, 0])
def vp_from_rpy(rpy, intrinsics=fcam_intrinsics):
e = get_view_frame_from_road_frame(rpy[0], rpy[1], rpy[2], 1.22)
ke = np.dot(intrinsics, e)
return vp_from_ke(ke)
def roll_from_ke(m):
# note: different from calibration.h/RollAnglefromKE: i think that one's just wrong
return np.arctan2(-(m[1, 0] - m[1, 1] * m[2, 0] / m[2, 1]),
@@ -163,11 +158,3 @@ def img_from_device(pt_device):
pt_img = pt_view/pt_view[:, 2:3]
return pt_img.reshape(input_shape)[:, :2]
def get_camera_frame_from_calib_frame(camera_frame_from_road_frame, intrinsics=fcam_intrinsics):
camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)]
calib_frame_from_ground = np.dot(intrinsics,
get_view_frame_from_road_frame(0, 0, 0, 1.22))[:, (0, 1, 3)]
ground_from_calib_frame = np.linalg.inv(calib_frame_from_ground)
camera_frame_from_calib_frame = np.dot(camera_frame_from_ground, ground_from_calib_frame)
return camera_frame_from_calib_frame
common/transformations/model.py
+54 -103
View File
@@ -1,10 +1,7 @@
import numpy as np
from common.transformations.camera import (FULL_FRAME_SIZE,
FOCAL,
get_view_frame_from_road_frame,
get_view_frame_from_calib_frame,
vp_from_ke)
get_view_frame_from_calib_frame)
# segnet
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]]])
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
MEDMODEL_INPUT_SIZE = (512, 256)
@@ -63,104 +45,73 @@ sbigmodel_intrinsics = np.array([
[0.0, sbigmodel_fl, 0.5 * (256 + MEDMODEL_CY)],
[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,
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,
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,
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))
# 'camera from model camera'
def get_model_height_transform(camera_frame_from_road_frame, height):
camera_frame_from_road_ground = np.dot(camera_frame_from_road_frame, np.array([
[1, 0, 0],
[0, 1, 0],
[0, 0, 0],
[0, 0, 1],
]))
### This function mimics the update_calibration logic in modeld.cc
### Manually verified to give similar results to xx.uncommon.utils.transform_img
def get_warp_matrix(rpy_calib, wide_cam=False, big_model=False, tici=True):
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
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)
if tici and wide_cam:
intrinsics = tici_ecam_intrinsics
elif tici:
intrinsics = tici_fcam_intrinsics
else:
camera_from_model_camera = np.eye(3)
intrinsics = eon_fcam_intrinsics
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)
camera_frame_from_medmodel_frame = np.dot(camera_frame_from_ground, ground_from_medmodel_frame)
return camera_frame_from_medmodel_frame
def get_camera_frame_from_bigmodel_frame(camera_frame_from_road_frame):
camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)]
bigmodel_frame_from_ground = bigmodel_frame_from_road_frame[:, (0, 1, 3)]
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_model_frame(snu_full, camera_frame_from_model_frame, size):
idxs = camera_frame_from_model_frame.dot(np.column_stack([np.tile(np.arange(size[0]), size[1]),
np.tile(np.arange(size[1]), (size[0], 1)).T.flatten(),
np.ones(size[0] * size[1])]).T).T.astype(int)
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]))
if big_model:
sbigmodel_from_calib = sbigmodel_frame_from_calib_frame[:, (0,1,2)]
calib_from_model = np.linalg.inv(sbigmodel_from_calib)
else:
raise ValueError("shape of input img is weird")
return calib
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
### This is old, just for debugging
def get_warp_matrix_old(rpy_calib, wide_cam=False, big_model=False, tici=True):
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
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
model_height = 1.22
if big_model:
model_from_road = np.dot(sbigmodel_intrinsics,
get_view_frame_from_road_frame(0, 0, 0, model_height))
else:
model_from_road = np.dot(medmodel_intrinsics,
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