keyframe_utils.py

'''
Key frame Utils 

Imported by camera pose estimation
'''

import numpy as np
import math
import tensorflow as tf

class Keyframe:
    def __init__(self, pose, depth, uncertainty, frame, image,covariance):
        self.T = pose # 4x4 transformation matrix from previous keyframe
        self.D = depth
        self.U = uncertainty
        self.F = frame # grayscale image
        self.I = image # RGB image
        self.C = covariance # Covariance of pose # 6x6

def tf_eulerAnglesToRotationMatrix(theta0,theta1,theta2):
    '''
    Converts rotation angles about x,y and z axis to a rotation matrix
    '''
    R_x = [[1, 0, 0],[0, math.cos(theta0), -math.sin(theta0)],[0, math.sin(theta0), math.cos(theta0)]]
    R_y = [[math.cos(theta1), 0, math.sin(theta1)],[0, 1, 0],[-math.sin(theta1), 0, math.cos(theta1)]]
    R_z = [[math.cos(theta2), -math.sin(theta2), 0],[math.sin(theta2), math.cos(theta2), 0],[0, 0, 1]]
    #R = tf.transpose(tf.matmul(tf.transpose(R_z),tf.matmul(tf.transpose(R_y),tf.transpose(R_x))))
    R = tf.matmul(tf.matmul(R_z,R_y),R_x)
    return R

def tf_get_back_T(T): # Returns 4x4 matrix
    theta0 = T[3]
    theta1 = T[4]
    theta2 = T[5]
    R = tf_eulerAnglesToRotationMatrix(theta0,theta1,theta2)
    temp = [[R[0][0],R[0][1],R[0][2],tf.cast(T[0],tf.float32)],[R[1][0],R[1][1],R[1][2],tf.cast(T[1],tf.float32)],[R[2][0],R[2][1],R[2][2],tf.cast(T[2],tf.float32)],[0,0,0,1.0]]
    #print()
    #print("temp",temp)
    #print()
    pose = tf.matmul([[1.0,0,0,0],[0,1.0,0,0],[0,0,1.0,0],[0,0,0,1.0]],temp)
    return pose

def tf_get_min_rep(T): # Returns 6 vector
    R = T[:,:3]

    # Check if R is rotation matrix

    sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
    singular = sy < 1e-6

    if not singular:
        theta0 = math.atan2(R[2, 1], R[2, 2])
        theta1 = math.atan2(-R[2, 0], sy)
        theta2 = math.atan2(R[1, 0], R[0, 0])
    else:
        theta0 = math.atan2(-R[1, 2], R[1, 1])
        theta1 = math.atan2(-R[2, 0], sy)
        theta2 = 0

    temp = [[T[0][3],T[1][3],T[2][3],theta0,theta1,theta2]]
    return tf.matmul(temp,np.eye(6).astype(np.float32))

def isRotationMatrix(R):
    '''
    Checks if a matrix is a valid rotation matrix.
    '''
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype=R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6


def extract_angles(R):
    '''
    Extract rotation angles

    Returns: aplha, beta, gamma (as np array)
    '''

    assert(isRotationMatrix(R))  # Throws error if false

    sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])

    singular = sy < 1e-6

    if not singular:
        x = math.atan2(R[2, 1], R[2, 2])
        y = math.atan2(-R[2, 0], sy)
        z = math.atan2(R[1, 0], R[0, 0])
    else:
        x = math.atan2(-R[1, 2], R[1, 1])
        y = math.atan2(-R[2, 0], sy)
        z = 0

    return np.array([x, y, z])

def get_min_rep(T):
    '''
    Convert 3*4 matrix into 6*1 vector

    [x y z alpha beta gamma]

    '''
    t = T[:, 3]
    x, y, z = t

    angles = extract_angles(T[:, :3])

    T_vect = np.zeros(6)
    T_vect[:3] = t
    T_vect[3:6] = angles
    return T_vect



def eulerAnglesToRotationMatrix(theta):
    '''
    Converts rotation angles about x,y and z axis to a rotation matrix
    '''
    R_x = np.array([[1, 0, 0],
                    [0, math.cos(theta[0]), -math.sin(theta[0])],
                    [0, math.sin(theta[0]), math.cos(theta[0])]
                    ])

    R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
                    [0, 1, 0],
                    [-math.sin(theta[1]), 0, math.cos(theta[1])]
                    ])

    R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
                    [math.sin(theta[2]), math.cos(theta[2]), 0],
                    [0, 0, 1]
                    ])
    R = np.dot(R_z, np.dot(R_y, R_x))

    return R


def fix_u(u_prop):
    '''
    Fixes a pixel location if it is negative or out of bounds

    Arguments;
            u_prop: pixel location

    Returns:
            u_prop: fixed pixel location
    '''
    if u_prop[0] >= im_size[0]:
        u_prop[0] = im_size[0] - 1
    elif u_prop[0] < 0:
        u_prop[0] = 0
    if u_prop[1] >= im_size[1]:
        u_prop[1] = im_size[1] - 1
    elif u_prop[1] < 0:
        u_prop[1] = 0
    return u_prop


def get_back_T(T_fl):
    '''
    Converts the minimal representation of the pose into the normal 3x4 transformation matrix
    '''
    # print "The flattened pose input is ",T_fl,'\n\n\n'
    T = np.ones((3, 4))
    T[:, 3] = T_fl[:3]  # 4th column of T = first 3 elements of T_fl
    R = eulerAnglesToRotationMatrix(T_fl[3:6])
    T[:, :3] = R
    return T

def get_delD(D):
    return 0.01  # Change later to calculate based on input depth map


def huber_norm(x):
    '''
    Calculates and Returns the huber norm

    Arguments:
            x: Input

    Returns:
            Huber norm of x
    '''
    return x**2

    delta = 1  # Change later
    if abs(x) < delta:
        return 0.5 * (x**2)
    else:
        return delta * (abs(x) - (delta / 2))