Cleanup calibration code (#25119)

* 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

common/transformations/camera.py

@@ -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