changeOfCoordinates.py

# To add a new cell, type '# %%'
# To add a new markdown cell, type '# %% [markdown]'
# %%
from IPython import get_ipython


# %%
# get_ipython().run_line_magic('load_ext', 'autoreload')
# get_ipython().run_line_magic('autoreload', '1')

from collections import namedtuple
from functools import partial
import math
import os
from pprint import pprint
import sys

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


# %%
def deg2Rad(deg):
    return deg / 180 * math.pi

def computePose(alpha, beta, gamma, x, y, z):
    """Computes the pose (rotation and translation) from roll, pitch, and yaw and translation.

    Arguments:
    alpha -- angle around X given in degrees
    beta -- angle around y given in degrees
    gamma -- angle around z given in degrees

    Returns:
    RPose -- the rotation matrix parameterized by alpha, beta, and gamma
    tPose -- the translation vector parameterized by x, y, z
    """

    RPoseX = np.array([[1, 0, 0],
                    [0, math.cos(deg2Rad(alpha)), -1 * math.sin(deg2Rad(alpha))],
                    [0, math.sin(deg2Rad(alpha)), math.cos(deg2Rad(alpha))]], dtype=np.float32)

    RPoseY = np.array([[math.cos(deg2Rad(beta)), 0, math.sin(deg2Rad(beta))],
                    [0, 1, 0],
                    [-1 * math.sin(deg2Rad(beta)), 0, math.cos(deg2Rad(beta))]], dtype=np.float32)

    RPoseZ = np.array([[math.cos(deg2Rad(gamma)), -1 * math.sin(deg2Rad(gamma)), 0],
                    [math.sin(deg2Rad(gamma)), math.cos(deg2Rad(gamma)), 0],
                    [0, 0, 1]], dtype=np.float32)

    RPose = np.dot(np.dot(RPoseX, RPoseY), RPoseZ)

    tPose = np.array([x, y, z], dtype=np.float32).reshape((3, 1))

    return RPose, tPose


def invertRigidTransformation(R, t):
    """Computes inverse of a rigid transformation."""

    RInv = np.linalg.inv(R)
    tInv = -1 * np.dot(RInv, t)

    return RInv, tInv


def convertPoseToChangeOfCoordinates(R, t):
    """Converts a pose given by R, t to a change of coordinates."""

    return invertRigidTransformation(R, t)


def convertChangeOfCoordinatesToPose(R, t):
    """Converts a change of coordinates  given by R, t to a pose."""
    return invertRigidTransformation(R, t)


def composeRigidTransformationRT(R1, t1, R2, t2):
    """Chains two rigid transformations."""

    RChained = np.dot(R1, R2)
    tChained = np.dot(R1, t2) + t1

    return RChained, tChained


def composePosesRT(R1RelTo0, t1RelTo0, R2RelTo1, t2RelTo1):
    """Chains two poses.

    Arguments:
    R1RelTo0 -- rotation of 1 relative to 0
    t1RelTo0 -- translation of 1 relative to 0
    R2RelTo1 -- rotation of 2 relative to 1
    t2RelTo1 -- translation of 2 relative to 1

    Returns:
    composed pose -- pose of 2 relative to 0
    """

    return composeRigidTransformationRT(R1RelTo0, t1RelTo0, R2RelTo1, t2RelTo1)


def composeChangeOfCoordinatesRT(R0To1, t0To1, R1To2, t1To2):
    """Chains two change of coordinates.

    Arguments:
    R0To1 -- rotation which changes coordinates from 0 to 1
    t0To1 -- translation which changes coordinates from 0 to 1
    R1To2 -- rotation which changes coordinates from 1 to 2
    t1To2 -- translation which changes coordinates from 1 to 2

    Returns:
    compose change of coordinates -- change of coordinates which transforms 0 to 2
    """

    return composeRigidTransformationRT(R1To2, t1To2, R0To1, t0To1)


CameraParameters = namedtuple('CameraParameters', ['M', 'distCoeff', 'R', 't'])


# %%
scaleOfSceene = 1.0

# setting up the camera
M = scaleOfSceene * np.eye(3, dtype=np.float32)
distCoeff = np.zeros((5, 1), dtype=np.float32)

RCamRelToW, tCamRelToW = computePose(180.0, 15.0, 0.0, -1 *scaleOfSceene, 0, 10 * scaleOfSceene)
RWToCam, tWToCam = convertPoseToChangeOfCoordinates(RCamRelToW, tCamRelToW)

cameraParameters = CameraParameters(M, distCoeff, RWToCam, tWToCam)
# print("cameraParameters: {}\n".format(cameraParameters))

# setting up points in world coordinates
xyRange = np.linspace(-1 * scaleOfSceene, 1 * scaleOfSceene, 5)
xv, yv = np.meshgrid(xyRange, xyRange)
boardPointsInBoard = np.concatenate((yv, xv, np.zeros(xv.size)), axis=None).reshape(3, -1).transpose()
boardPointsInW = boardPointsInBoard
# print("boardPointsInW: {}\n".format(boardPointsInW))

# project object points into image
imagePoints, _ = cv2.projectPoints(boardPointsInW, cv2.Rodrigues(cameraParameters.R)[0],
                        cameraParameters.t, cameraParameters.M, cameraParameters.distCoeff)
imagePoints = imagePoints.reshape(-1, 2)
# print("imagePoints: {}\n".format(imagePoints))

plt.scatter(imagePoints[:, 0], imagePoints[:, 1])
plt.axhline(y=np.max(imagePoints[:, 1]), linewidth=0.5, color='k')
plt.axhline(0, linewidth=0.5, color='k')
plt.axhline(y=np.min(imagePoints[:, 1]), linewidth=0.5, color='k')
plt.axis('equal')
plt.title('imagePoints')

# solving for change of coordinates and compare to ground truth
retval, rvec, tvec = cv2.solvePnP(boardPointsInW, imagePoints, cameraParameters.M,
                            cameraParameters.distCoeff)

print("maximum deviation {}".format(np.max(np.append(RWToCam - cv2.Rodrigues(rvec)[0], tWToCam - tvec))))


# %%
def generateListOfPoses(alphaRange, betaRange, gammaRange, xRange, yRange, zRange):
    """Computes all poses parameterized by the combinations of elements in the
    alphaRange, betaRange, gammaRange, xRange, yRange, zRange.

    Arguments:
    alphaRange -- a list of angles in degree for the rotation around x
    betaRange -- a list of angles in degree for the rotation around y
    gammaRange -- a list of angles in degree for the rotation around z
    xRange -- a list list of translations in along x
    yRange -- a list list of translations in along y
    zRange -- a list list of translations in along z

    Returns:
    poses -- list of poses parameterized by elements of alphaRange, betaRange, gammaRange,
             xRange, yRange, zRange
    """

    poses = []
    for alpha in alphaRange:
        for beta in betaRange:
            for gamma in gammaRange:
                for x in xRange:
                    for y in yRange:
                        for z in zRange:
                            poses.append(computePose(alpha, beta, gamma, x, y, z))

    return poses


def transformPoints(points, R, t):
    """Transforms input points by the rigid transformation R, t."""

    return (np.dot(R, points.transpose()) + t).transpose()

# setting up points in world coordinates
alphaRange = [0.0]
betaRange = [-5.0, 0.0, 5.0]
gammaRange = [-5.0, 0.0, 5.0]
xRange = [0]
yRange = [0]
zRange = [-0.5 * scaleOfSceene, 0, 0.5 * scaleOfSceene]
posesBoardRelToW = generateListOfPoses(alphaRange, betaRange, gammaRange, xRange, yRange, zRange)

boardPointsInW = [transformPoints(boardPointsInBoard, RBoardRelToW, tBoardRelToW) for (RBoardRelToW, tBoardRelToW) in posesBoardRelToW]

# project object points into image
projectPoints = lambda objectPoints: cv2.projectPoints(objectPoints, cv2.Rodrigues(cameraParameters.R)[0], cameraParameters.t, cameraParameters.M, cameraParameters.distCoeff)[0].reshape(-1, 2)

imagePoints = [projectPoints(objectPoints).astype(np.float32) for objectPoints in boardPointsInW]
# for points in imagePoints:
#     plt.scatter(points[:, 0], points[:, 1])
#     plt.axis('equal')
#     plt.title('imagePoints')

# compute change of coordinates board to camera
changeOfCoordinatesWorldToBoard = [convertPoseToChangeOfCoordinates(RBoardRelToW, tBoardRelToW) for (RBoardRelToW, tBoardRelToW) in posesBoardRelToW]

changeOfCoordinatesBoardToWorld = [invertRigidTransformation(RWToBoard, tWToBoard) for (RWToBoard, tWToBoard) in changeOfCoordinatesWorldToBoard]

changeOfCoordinatesBoardToCam = [composeChangeOfCoordinatesRT(RBoardToW, tBoardToW, RWToCam, tWToCam) for (RBoardToW, tBoardToW) in changeOfCoordinatesBoardToWorld]

maxDeviationTotal = 0
for i in range(0, len(imagePoints)):
    retval, rvec, tvec = cv2.solvePnP(boardPointsInBoard, imagePoints[i], cameraParameters.M,
                                        cameraParameters.distCoeff)

    RBoardToCam = changeOfCoordinatesBoardToCam[i][0]
    tBoardToCam = changeOfCoordinatesBoardToCam[i][1]

    maxDeviation = np.max(np.abs(np.append(RBoardToCam - cv2.Rodrigues(rvec)[0],
                            tBoardToCam - tvec)))
    if maxDeviation > maxDeviationTotal:
        maxDeviationTotal = maxDeviation

print("maximum deviation total: {}".format(maxDeviationTotal))


# %%
boardPointsInBoard = boardPointsInBoard.astype(np.float32)

# calibrate the camera
imageSize = (10, 10)
retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera([boardPointsInBoard for _ in range(0, len(imagePoints))], imagePoints, imageSize, cameraParameters.M, cameraParameters.distCoeff,
flags=(cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_FIX_PRINCIPAL_POINT))

maxDeviationTotal = 0
for i in range(0, len(imagePoints)):
    RBoardToCam = changeOfCoordinatesBoardToCam[i][0]
    tBoardToCam = changeOfCoordinatesBoardToCam[i][1]

    maxDeviation = np.max(np.abs(np.append(RBoardToCam - cv2.Rodrigues(rvecs[i])[0],
                            tBoardToCam - tvecs[i])))
    if maxDeviation > maxDeviationTotal:
        maxDeviationTotal = maxDeviation

print("maximum deviation total: {}".format(maxDeviationTotal))


# %%
# project points into second stereo camera
RCam2RelToW, tCam2RelToW = computePose(180.0, -15.0, 0.0, 1 * scaleOfSceene, 0, 10 * scaleOfSceene)
RWToCam2, tWToCam2 = convertPoseToChangeOfCoordinates(RCam2RelToW, tCam2RelToW)

cameraParameters2 = CameraParameters(M, distCoeff, RWToCam2, tWToCam2)

# project object points into image
projectPoints2 = lambda objectPoints: cv2.projectPoints(objectPoints, cv2.Rodrigues(cameraParameters2.R)[0], cameraParameters2.t, cameraParameters2.M, cameraParameters2.distCoeff)[0].reshape(-1, 2)

imagePoints2 = [projectPoints2(objectPoints).astype(np.float32) for objectPoints in boardPointsInW]
# for points in imagePoints2:
#     plt.scatter(points[:, 0], points[:, 1])
#     plt.axis('equal')
#     plt.title('imagePoints')

# compute change of coordinates cam1 to cam2
RCamToW, tCamToW = invertRigidTransformation(RWToCam, tWToCam)
RCam1ToCam2, tCam1ToCam2 = composeChangeOfCoordinatesRT(RCamToW, tCamToW, RWToCam2, tWToCam2)


# %%
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
    [boardPointsInBoard for _ in range(0, len(imagePoints))], imagePoints, imagePoints2, cameraParameters.M, cameraParameters.distCoeff, cameraParameters2.M, cameraParameters2.distCoeff, imageSize,
    flags=(cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_FIX_PRINCIPAL_POINT))

print("maximum deviation: {}".format(np.max(np.abs(np.append(RCam1ToCam2 - R, tCam1ToCam2 - T)))))


# %%
def compseProjectionMatrix(M, R, t):
    return np.dot(M, np.append(R, t, 1))

projectionMatrix1 = compseProjectionMatrix(cameraParameters.M, cameraParameters.R, cameraParameters.t)
projectionMatrix2 = compseProjectionMatrix(cameraParameters2.M, cameraParameters2.R, cameraParameters2.t)


triangulatedPointsHomogeneous = cv2.triangulatePoints(projectionMatrix1, projectionMatrix2, imagePoints[0].transpose(), imagePoints2[0].transpose())
triangulatedPoints = triangulatedPointsHomogeneous[:3, :] / triangulatedPointsHomogeneous[3, :]
print("maximum deviation: {}".format(np.max(np.abs(np.array(boardPointsInW[0]).transpose() - triangulatedPoints))))


# %%
E, _ = cv2.findEssentialMat(imagePoints[0], imagePoints2[0], cameraParameters.M)
_, R, T, mask = cv2.recoverPose(E, imagePoints[0], imagePoints2[0], cameraParameters.M)

print("maximum deviation: {}".format(np.max(np.abs(RCam1ToCam2 - R))))
print("T: {} and scaling: {}, {} and {} (should be the same for non-zero values)".format(T,
    *(tCam1ToCam2[i] / T[i] for i in range(3))))


# %%
i = 1
H, _ = cv2.findHomography(imagePoints[i], imagePoints2[i])
retval, rotations, translations, normals = cv2.decomposeHomographyMat(H, cameraParameters.M)

projectionMatrix1 = compseProjectionMatrix(cameraParameters.M, np.eye(3, dtype=float),
                                                                np.zeros((3, 1), dtype=float))

RBoardToW, tBoardToW = changeOfCoordinatesBoardToWorld[i]
RBoardToC2, tBoardToC2 = composeChangeOfCoordinatesRT(RBoardToW, tBoardToW,
                            cameraParameters.R, cameraParameters.t)
normalInC2 = np.dot(RBoardToC2, np.array([0, 0, 1], dtype=float).transpose())
boardOriginInC2 = tBoardToC2
d = np.abs(np.dot(normalInC2.transpose(), boardOriginInC2))[0]

for rotation, tvec in zip(rotations, translations):
    projectionMatrix2 = compseProjectionMatrix(cameraParameters.M, rotation, tvec)
    triangulatedPointsHomogeneous = cv2.triangulatePoints(projectionMatrix1, projectionMatrix2,
                                imagePoints[i].transpose(), imagePoints2[i].transpose())
    triangulatedPoints = triangulatedPointsHomogeneous[:3, :] / triangulatedPointsHomogeneous[3, :]

    if np.mean(triangulatedPoints[2, :]) < 0:
        print("Cheirality check not passed!")
    else:
        print("maximum deviation: {}".format(np.max(np.abs(RCam1ToCam2 - rotation))))
        print("tvec: {} and scaling: {}, {} and {} (should be the same for non-zero values)".format(tvec,
            *(tCam1ToCam2[i] / tvec[i] for i in range(3))))
        print("tCam1ToCam2: {}\nscaled: {}\nand max. deviation: {}".format(
            tCam1ToCam2, d * tvec, np.max(np.abs(tCam1ToCam2 - d * tvec))))