# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: Python 3 (ipykernel)
#     language: python
#     name: python3
# ---

# # Inverse perspective mapping

# ## Setting up Colab

colab_nb = 'google.colab' in str(get_ipython())

if colab_nb:
  from google.colab import drive
  drive.mount('/content/drive')

if colab_nb:
  # %cd /content/drive/My Drive/aad/code/tests/lane_detection

# ## Exercise

# Solve the TODO items in `exercises/lane_detection/camera_geometry.py` which are labeled as **"TODO step 2"**.
#
# The cells below will help you check if your implementation is correct. You might want to read them before you start with your implementation.

# ### Unit test

# execute this cell to run unit tests on your implementation of step 2
# %cd ../../../
# !python -m code.tests.lane_detection.camera_geometry_unit_test 2
# %cd -

# ### Test by visual inspection

# When you change the boolean below to `True`, your code will be run. Otherwise the sample solution will be run. The images that the code generates should be the same for your code and the sample solution.

run_student_code = False

# %load_ext autoreload
# %autoreload 2
import sys
from pathlib import Path
sys.path.append(str(Path('../../')))
if run_student_code:
    from exercises.lane_detection import camera_geometry
else:
    from solutions.lane_detection import camera_geometry

import numpy as np
import matplotlib.pyplot as plt
import cv2

# First we construct the pixel coordinates $(u,v)$ for the left lane boundary, in the same way that we did it in the chapter on image formation:

# +
image_fn = str(Path("../../../data/Town04_Clear_Noon_09_09_2020_14_57_22_frame_625_validation_set.png").absolute())
image = cv2.imread(image_fn)
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)

boundary_fn = image_fn.replace(".png", "_boundary.txt")
boundary_gt = np.loadtxt(boundary_fn)

trafo_fn = image_fn.replace(".png", "_trafo.txt")
trafo_world_to_cam = np.loadtxt(trafo_fn)

cg = camera_geometry.CameraGeometry()
K = cg.intrinsic_matrix

left_boundary_3d_gt_world = boundary_gt[:,0:3]
uv = camera_geometry.project_polyline(left_boundary_3d_gt_world, trafo_world_to_cam, K)
u,v = uv[:,0], uv[:,1]
plt.plot(u,v)
plt.imshow(image);
# -

# Now we have image coordinates $(u,v)$ in our numpy array `uv`. Let us try to reconstruct the 3d coordinates using equation

# $$
#     \begin{pmatrix} X_c \\ Y_c \\Z_c \end{pmatrix} = \frac{h}{ \mathbf{n_c}^T \mathbf{K}^{-1} (u,v,1)^T} \mathbf{K}^{-1} \begin{pmatrix} u \\ v \\ 1 \end{pmatrix} 
# $$ 

# The relevant code is implemented in camera_geometry.py in the function `uv_to_roadXYZ_camframe()`.

# Reconstruct the left boundary starting from the known u,v
reconstructed_lb_3d_cam = []
for u,v in uv:
    xyz = cg.uv_to_roadXYZ_camframe(u,v)
    reconstructed_lb_3d_cam.append(xyz)
reconstructed_lb_3d_cam = np.array(reconstructed_lb_3d_cam)


# +
# Map reconstructed left boundary into world reference frame
def map_between_frames(points, trafo_matrix):
    x,y,z = points[:,0], points[:,1], points[:,2]
    homvec = np.stack((x,y,z,np.ones_like(x)))
    return (trafo_matrix @ homvec).T

trafo_cam_to_world = np.linalg.inv(trafo_world_to_cam)
reconstructed_lb_3d_world = map_between_frames(reconstructed_lb_3d_cam, trafo_cam_to_world)
# -

# plot both ground truth and reconstructed left boundary 3d in X-Y-plane
plt.plot(left_boundary_3d_gt_world[:,0], left_boundary_3d_gt_world[:,1], label="ground truth")
plt.plot(reconstructed_lb_3d_world[:,0], reconstructed_lb_3d_world[:,1], ls = "--", label="reconstructed")
plt.axis("equal")
plt.legend();

# You should see that the lines overlap. Finally, we can also do this comparison in the road frame instead of the world frame.

# +
# compare ground truth and reconstructed boundary in road frame
trafo_world_to_road = cg.trafo_cam_to_road @ trafo_world_to_cam
left_boundary_3d_gt_road = map_between_frames(left_boundary_3d_gt_world, trafo_world_to_road)
reconstructed_lb_3d_road = map_between_frames(reconstructed_lb_3d_cam, cg.trafo_cam_to_road)

# plot both ground truth and reconstructed left boundary 3d in Z-(-X)-plane (which is X-Y in road iso 8855)
plt.plot(left_boundary_3d_gt_road[:,2], -left_boundary_3d_gt_road[:,0], label="ground truth")
plt.plot(reconstructed_lb_3d_road[:,2], -reconstructed_lb_3d_road[:,0], ls = "--", label="reconstructed")
plt.axis("equal")
plt.legend();
# -

# You should see that the lines overlap.

StarcoderdataJupyterScriptsDedupFiltered