{"cells":[{"metadata":{},"cell_type":"markdown","source":"# Show Cars with Similar Local Orientation"},{"metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true},"cell_type":"code","source":"import math\nimport random\nfrom pathlib import Path\nfrom collections import defaultdict\nimport numpy as np\nimport pandas as pd\nimport cv2\nfrom scipy.spatial.transform import Rotation\nfrom tqdm.notebook import tqdm\nimport matplotlib.pyplot as plt","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"camera_matrix = np.array(\n    [[2304.5479, 0,  1686.2379],\n     [0, 2305.8757, 1354.9849],\n     [0, 0, 1]], dtype=np.float32)\n\n# code from https://www.kaggle.com/hocop1/centernet-baseline\ndef str2coords(s, names=('id', 'yaw', 'pitch', 'roll', 'x', 'y', 'z')):\n    '''\n    Input:\n        s: PredictionString (e.g. from train dataframe)\n        names: array of what to extract from the string\n    Output:\n        list of dicts with keys from `names`\n    '''\n    coords = []\n    for l in np.array(s.split()).reshape([-1, 7]):\n        coords.append(dict(zip(names, l.astype('float'))))\n        if 'id' in coords[-1]:\n            coords[-1]['id'] = int(coords[-1]['id'])\n    return coords\n\ndef get_img_coords(x, y, z):\n    p = np.array([x, y, z]).T\n    img_p = np.dot(camera_matrix, p)\n    img_p[0] /= img_p[2]\n    img_p[1] /= img_p[2]\n    return img_p[0], img_p[1], z\n\ndef get_centerize_mat(x, y, z):\n    yaw = 0\n    pitch = -np.arctan(x / z)\n    roll = np.arctan(y / z)\n    return Rotation.from_euler(\"xyz\", (roll, pitch, yaw)).as_dcm()\n\ndef get_targets(c):\n    x, y, z = c[\"x\"], c[\"y\"], c[\"z\"]\n    roll, pitch, yaw = c[\"roll\"], c[\"pitch\"], c[\"yaw\"]\n    ix, iy, _ = get_img_coords(x, y, z)\n    Rt2 = get_centerize_mat(x, y, z)\n    Rt1 = Rt2 @ Rotation.from_euler(\"yxz\", (pitch, yaw, roll)).as_dcm()\n    rot = Rotation.from_dcm(Rt1)\n    r1, r2, r3 = rot.as_euler(\"yxz\")\n    r3 = r3 - math.pi if r3 > 0 else r3 + math.pi\n    return dict(x=x, y=y, z=z, r1=r1, r2=r2, r3=r3, ix=ix, iy=iy)","execution_count":null,"outputs":[]},{"metadata":{"_uuid":"d629ff2d2480ee46fbb7e2d37f6b5fab8052498a","_cell_guid":"79c7e3d0-c299-4dcb-8224-4455121ee9b0","trusted":true},"cell_type":"code","source":"train = pd.read_csv(\"../input/pku-autonomous-driving/train.csv\")\n\nangles = []\ncars = []\n\nfor i, row in tqdm(train.iterrows(), total=len(train)):\n    coords = str2coords(row[\"PredictionString\"])\n\n    for c in coords:\n        t = get_targets(c)\n        t[\"img_id\"] = row[\"ImageId\"]\n        cars.append(t)\n        angles.append((t[\"r1\"], t[\"r2\"], t[\"r3\"]))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def imcrop(img, bbox):\n    x1, y1, x2, y2 = bbox\n    if x1 < 0 or y1 < 0 or x2 > img.shape[1] or y2 > img.shape[0]:\n        img, x1, x2, y1, y2 = pad_img_to_fit_bbox(img, x1, x2, y1, y2)\n    return img[y1:y2, x1:x2, :]\n\ndef pad_img_to_fit_bbox(img, x1, x2, y1, y2):\n    img = cv2.copyMakeBorder(img, - min(0, y1), max(y2 - img.shape[0], 0),\n                            -min(0, x1), max(x2 - img.shape[1], 0),cv2.BORDER_REPLICATE)\n    y2 += -min(0, y1)\n    y1 += -min(0, y1)\n    x2 += -min(0, x1)\n    x1 += -min(0, x1)\n    return img, x1, x2, y1, y2\n\ndef get_target_car_img(car):\n    img_dir = Path(\"../input/pku-autonomous-driving/train_images\")\n    img_id, x, y, s = car[\"img_id\"], car[\"ix\"], car[\"iy\"], 10000 / car[\"z\"]\n    img_path = img_dir.joinpath(img_id + \".jpg\")\n    img = cv2.imread(str(img_path))\n    h, w = img.shape[:2]\n    x1 = int(x - s / 2)\n    y1 = int(y - s / 2)\n    x2 = int(x + s / 2)\n    y2 = int(y + s / 2)\n    return imcrop(img, (x1, y1, x2, y2))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def rot_dist(rot1, rot2):\n    diff = Rotation.inv(rot2) * rot1\n    w = np.clip(diff.as_quat()[-1], -1., 1.)\n    w = (math.acos(w) * 360) / math.pi\n    if w > 180:\n        w = 360 - w\n    return w\n\ndef euler_dist(euler1, euler2):\n    rot1 = Rotation.from_euler(\"xyz\", euler1)\n    rot2 = Rotation.from_euler(\"xyz\", euler2)\n    return rot_dist(rot1, rot2)\n\ndef car_dist(car1, car2):\n    euler1 = (car1[\"r1\"], car1[\"r2\"], car1[\"r3\"])\n    euler2 = (car2[\"r1\"], car2[\"r2\"], car2[\"r3\"])\n    return euler_dist(euler1, euler2)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def show_cars(cars):\n    cols, rows = 4, 4\n    img_num = cols * rows\n    fig = plt.figure(figsize=(20,20))\n\n    for i in range(img_num):\n        car = cars[i]\n        img =  get_target_car_img(car)\n        img = cv2.resize(img, (512, 512))\n        ax = fig.add_subplot(rows, cols, i + 1)\n        ax.imshow(img[:, :, ::-1])\n        plt.axis('off')","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# select one car randomly\n# car = random.choice(cars)  # use this!\ncar = cars[10]\nimg = get_target_car_img(car)\nplt.imshow(img[:, :, ::-1])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"car_dists = [(c, car_dist(car, c)) for c in cars[:1000]]  # use only first 1000 cars because distance calculation is slow...\nsorted_car_dists = sorted(car_dists, key=lambda x: x[1])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# show 16 cars with nearest orientations to the above selected car\nshow_cars([c[0] for c in sorted_car_dists[:16]])","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"# Clustering Quaternions"},{"metadata":{"trusted":true},"cell_type":"code","source":"def kmeans(samples, k, reduce, distance, max_iter=300):\n    sample_num = len(samples)\n    centroids  = [samples[i] for i in np.random.choice(sample_num, k)]\n    \n    for i in range(max_iter):\n        dist = 0.0\n        centroid_id_to_samples = defaultdict(list)\n\n        for sample in samples:\n            distances = [distance(sample, c) for c in centroids]\n            nearest_id = np.argmin(np.array(distances))\n            dist += distances[nearest_id]\n            centroid_id_to_samples[nearest_id].append(sample)\n            \n        print(i, dist / sample_num)\n            \n        for k, v in centroid_id_to_samples.items():\n            centroids[k] = reduce(v)\n            \n    return centroids","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# code from https://github.com/christophhagen/averaging-quaternions\n# https://github.com/christophhagen/averaging-quaternions/blob/master/LICENSE\ndef average_rotations(rots):\n    # Number of quaternions to average\n    M = len(rots)\n    Q = np.array([q.as_quat() for q in rots])\n    A = np.zeros(shape=(4, 4))\n\n    for i in range(M):\n        q = Q[i,:]\n        # multiply q with its transposed version q' and add A\n        A = np.outer(q,q) + A\n\n    # scale\n    A = (1.0/M)*A\n    # compute eigenvalues and -vectors\n    eigenValues, eigenVectors = np.linalg.eig(A)\n    # Sort by largest eigenvalue\n    eigenVectors = eigenVectors[:,eigenValues.argsort()[::-1]]\n    # return the real part of the largest eigenvector (has only real part)\n    return Rotation.from_quat(np.real(eigenVectors[:,0]))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"rots = [Rotation.from_euler(\"yxz\", angle) for angle in angles]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"%%time\n# cluster 5000 quaternions into 32 clusters\ncentroids = kmeans(rots[:5000], 32, average_rotations, rot_dist, max_iter=10)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"centroid_id_to_cars = defaultdict(list)\n\nfor car in tqdm(cars[:5000]):\n    car_rot = Rotation.from_euler(\"yxz\", (car[\"r1\"], car[\"r2\"], car[\"r3\"]))\n    distances = [rot_dist(car_rot, c) for c in centroids]\n    nearest_centroid_id = np.argmin(np.array(distances))\n    centroid_id_to_cars[nearest_centroid_id].append(car)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# cluster 0\nshow_cars(random.sample(centroid_id_to_cars[0], 16))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# cluster 1\nshow_cars(random.sample(centroid_id_to_cars[1], 16))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# cluster 2\nshow_cars(random.sample(centroid_id_to_cars[2], 16))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# cluster 3\nshow_cars(random.sample(centroid_id_to_cars[3], 16))","execution_count":null,"outputs":[]}],"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"pygments_lexer":"ipython3","nbconvert_exporter":"python","version":"3.6.4","file_extension":".py","codemirror_mode":{"name":"ipython","version":3},"name":"python","mimetype":"text/x-python"}},"nbformat":4,"nbformat_minor":1}