Add euler to rotation matrix, grid flattening

unsupervised/thirdparty/utils.py

@@ -3,7 +3,9 @@ Utils to load and split image/video data.
 """
 
 from __future__ import division
+
 import math
+
 import tensorflow as tf
 
 
@@ -58,6 +60,49 @@ def euler2mat(z, y, x):
     return rotMat
 
 
+def euler2mat_noNDim(x, y, z):
+    """
+
+    :param x: Tensor of shape (B, 1) - x axis rotation
+    :param y: Tensor of shape (B, 1) - y axis rotation
+    :param z: Tensor of shape (B, 1) - z axis rotation
+    :return:  Rotation matrix for the given euler anglers, in the order rotation(x).rotation(y).rotation(z)
+    """
+    batch_size = tf.shape(z)[0]
+
+    # Euler angles should be between -pi and pi, clip so the pose network is coerced to this range
+    z = tf.clip_by_value(z, -math.pi, math.pi)
+    y = tf.clip_by_value(y, -math.pi, math.pi)
+    x = tf.clip_by_value(x, -math.pi, math.pi)
+
+    zeros = tf.zeros([batch_size, 1])
+    ones = tf.ones([batch_size, 1])
+
+    cosx = tf.cos(x)
+    sinx = tf.sin(x)
+    rotx_1 = tf.concat([ones, zeros, zeros], axis=1)
+    rotx_2 = tf.concat([zeros, cosx, -sinx], axis=1)
+    rotx_3 = tf.concat([zeros, sinx, cosx], axis=1)
+    xmat = tf.reshape(tf.concat([rotx_1, rotx_2, rotx_3], axis=1), [batch_size, 3, 3])
+
+    cosz = tf.cos(z)
+    sinz = tf.sin(z)
+    rotz_1 = tf.concat([cosz, -sinz, zeros], axis=1)
+    rotz_2 = tf.concat([sinz, cosz, zeros], axis=1)
+    rotz_3 = tf.concat([zeros, zeros, ones], axis=1)
+    zmat = tf.reshape(tf.concat([rotz_1, rotz_2, rotz_3], axis=1), [batch_size, 3, 3])
+
+    cosy = tf.cos(y)
+    siny = tf.sin(y)
+    roty_1 = tf.concat([cosy, zeros, siny], axis=1)
+    roty_2 = tf.concat([zeros, ones, zeros], axis=1)
+    roty_3 = tf.concat([-siny, zeros, cosy], axis=1)
+    ymat = tf.reshape(tf.concat([roty_1, roty_2, roty_3], axis=1), [batch_size, 3, 3])
+
+    rotMat = tf.matmul(tf.matmul(zmat, ymat), xmat)
+    return rotMat
+
+
 def pose_vec2mat(vec):
     """Converts 6DoF parameters to transformation matrix
     Args:
@@ -281,6 +326,7 @@ def bilinear_sampler(imgs, coords):
         ])
         return output
 
+
 # Spatial transformer network bilinear sampler, taken from https://github.com/kevinzakka/spatial-transformer-network/blob/master/stn/transformer.py
 
 
@@ -309,8 +355,8 @@ def stn_bilinear_sampler(img, x, y):
     # rescale x and y to [0, W-1/H-1]
     x = tf.cast(x, 'float32')
     y = tf.cast(y, 'float32')
-    x = 0.5 * ((x + 1.0) * tf.cast(max_x-1, 'float32'))
+    x = 0.5 * ((x + 1.0) * tf.cast(max_x - 1, 'float32'))
-    y = 0.5 * ((y + 1.0) * tf.cast(max_y-1, 'float32'))
+    y = 0.5 * ((y + 1.0) * tf.cast(max_y - 1, 'float32'))
 
     # grab 4 nearest corner points for each (x_i, y_i)
     x0 = tf.cast(tf.floor(x), 'int32')
@@ -337,10 +383,10 @@ def stn_bilinear_sampler(img, x, y):
     y1 = tf.cast(y1, 'float32')
 
     # calculate deltas
-    wa = (x1-x) * (y1-y)
+    wa = (x1 - x) * (y1 - y)
-    wb = (x1-x) * (y-y0)
+    wb = (x1 - x) * (y - y0)
-    wc = (x-x0) * (y1-y)
+    wc = (x - x0) * (y1 - y)
-    wd = (x-x0) * (y-y0)
+    wd = (x - x0) * (y - y0)
 
     # add dimension for addition
     wa = tf.expand_dims(wa, axis=3)
@@ -349,6 +395,6 @@ def stn_bilinear_sampler(img, x, y):
     wd = tf.expand_dims(wd, axis=3)
 
     # compute output
-    out = tf.add_n([wa*Ia, wb*Ib, wc*Ic, wd*Id])
+    out = tf.add_n([wa * Ia, wb * Ib, wc * Ic, wd * Id])
 
     return out

unsupervised/warp.py

@@ -1,53 +1,42 @@
-import numpy as np
+import math
+
 import tensorflow as tf
 
 
-def euler_to_rotation_matrix(x, y, z):
+def euler_to_matrix(x, y, z):
     """
 
     :param x: Tensor of shape (B, 1) - x axis rotation
     :param y: Tensor of shape (B, 1) - y axis rotation
     :param z: Tensor of shape (B, 1) - z axis rotation
-    :return:  Rotation matrix for the given euler anglers, in the  order rotation(x).rotation(y).rotation(z)
+    :return:  Rotation matrix for the given euler anglers, in the order rotation(x) -> rotation(y) -> rotation(z)
     """
-    B = tf.shape(z)[0]
+    batch_size = tf.shape(z)[0]
 
     # Euler angles should be between -pi and pi, clip so the pose network is coerced to this range
-    z = tf.clip_by_value(z, -np.pi, np.pi)
+    z = tf.clip_by_value(z, -math.pi, math.pi)
-    y = tf.clip_by_value(y, -np.pi, np.pi)
+    y = tf.clip_by_value(y, -math.pi, math.pi)
-    x = tf.clip_by_value(x, -np.pi, np.pi)
+    x = tf.clip_by_value(x, -math.pi, math.pi)
-
-    # Expand to B x 1 x 1
-    z = tf.expand_dims(tf.expand_dims(z, -1), -1)
-    y = tf.expand_dims(tf.expand_dims(y, -1), -1)
-    x = tf.expand_dims(tf.expand_dims(x, -1), -1)
-
-    zeros = tf.zeros([B, 1, 1])
-    ones = tf.ones([B, 1, 1])
 
     cosx = tf.cos(x)
     sinx = tf.sin(x)
-    rotx_1 = tf.concat([ones, zeros, zeros], axis=3)
-    rotx_2 = tf.concat([zeros, cosx, -sinx], axis=3)
-    rotx_3 = tf.concat([zeros, sinx, cosx], axis=3)
-    xmat = tf.concat([rotx_1, rotx_2, rotx_3], axis=2)
-
-    cosz = tf.cos(z)
-    sinz = tf.sin(z)
-    rotz_1 = tf.concat([cosz, -sinz, zeros], axis=3)
-    rotz_2 = tf.concat([sinz, cosz, zeros], axis=3)
-    rotz_3 = tf.concat([zeros, zeros, ones], axis=3)
-    zmat = tf.concat([rotz_1, rotz_2, rotz_3], axis=2)
 
     cosy = tf.cos(y)
     siny = tf.sin(y)
-    roty_1 = tf.concat([cosy, zeros, siny], axis=3)
-    roty_2 = tf.concat([zeros, ones, zeros], axis=3)
-    roty_3 = tf.concat([-siny, zeros, cosy], axis=3)
-    ymat = tf.concat([roty_1, roty_2, roty_3], axis=2)
 
-    rotMat = tf.matmul(tf.matmul(xmat, ymat), zmat)
+    cosz = tf.cos(z)
-    return rotMat
+    sinz = tf.sin(z)
+
+    # Otherwise this will need to be reversed
+    # Rotate about x, y then z. z goes first here as rotation is always left side of coordinates
+    # R = Rz(φ)Ry(θ)Rx(ψ)
+    # = |   cos(θ)cos(φ)    sin(ψ)sin(θ)cos(φ) − cos(ψ)sin(φ)   cos(ψ)sin(θ)cos(φ) + sin(ψ)sin(φ)   |
+    #   |   cos(θ)sin(φ)    sin(ψ)sin(θ)sin(φ) + cos(ψ)cos(φ)   cos(ψ)sin(θ)sin(φ) − sin(ψ)cos(φ)   |
+    #   |   −sin(θ)         sin(ψ)cos(θ)                        cos(ψ)cos(θ)                        |
+    row_1 = tf.concat([cosy * cosz, sinx * siny * cosz - cosx * sinz, cosx * siny * cosz + sinx * sinz], 1)
+    row_2 = tf.concat([cosy * sinz, sinx * siny * sinz + cosx * cosz, cosx * siny * sinz - sinx * cosz], 1)
+    row_3 = tf.concat([-siny, sinx * cosy, cosx * cosy], 1)
+    return tf.reshape(tf.concat([row_1, row_2, row_3], axis=1), [batch_size, 3, 3])
 
 
 def pose_vec2mat(vec):
@@ -57,13 +46,14 @@ def pose_vec2mat(vec):
     Returns:
         A transformation matrix -- [B, 4, 4]
     """
+    # TODO: FIXME
     batch_size, _ = vec.get_shape().as_list()
     translation = tf.slice(vec, [0, 0], [-1, 3])
     translation = tf.expand_dims(translation, -1)
     rx = tf.slice(vec, [0, 3], [-1, 1])
     ry = tf.slice(vec, [0, 4], [-1, 1])
     rz = tf.slice(vec, [0, 5], [-1, 1])
-    rot_mat = euler_to_rotation_matrix(rx, ry, rz)
+    rot_mat = euler_to_matrix(rx, ry, rz)
     rot_mat = tf.squeeze(rot_mat, axis=[1])
     filler = tf.constant([0.0, 0.0, 0.0, 1.0], shape=[1, 1, 4])
     filler = tf.tile(filler, [batch_size, 1, 1])
@@ -93,12 +83,23 @@ def image_coordinate(batch, height, width):
     return tf.repeat(tf.expand_dims(stacked, axis=0), batch, axis=0)
 
 
+def intrinsics_vector_to_matrix(intrinsics):
+    """
+    Convert 4 element
+    :param intrinsics: Tensor of shape (B, 4), intrinsics for each image
+    :return: Tensor of shape (B, 4, 4), intrinsics for each batch
+    """
+    pass
+
+
 def projective_inverse_warp(target_img, source_img, depth, pose, intrinsics, coordinates):
     """
     Calculate the reprojected image from the source to the target, based on the given depth, pose and intrinsics
 
     SFM Learner inverse warp step
-        ps ~ K.T(t->s).Dt(pt).K^-1.pt
+        ps ~ K.T(t->s).Dt(pt)*K^-1.pt
+
+        Note that the depth pixel Dt(pt) is multiplied by every coordinate value (just element-wise, not matrix multiplication)
 
     Idea is to map the pixel coordinates of the target image to 3d space (Dt(pt).K^-1.pt), then map these onto
     the source image in pixel coordinates (K.T(t->s).{3d coord}), then using the projected coordinates we sample 
@@ -108,20 +109,28 @@ def projective_inverse_warp(target_img, source_img, depth, pose, intrinsics, coo
     :param source_img: Tensor, same shape as target_img
     :param depth: Tensor, (batch, height, width, 1)
     :param pose: (batch, 6)
-    :param intrinsics: (batch, 3, 3)
+    :param intrinsics: (batch, 4) (fx, fy, px, py) TODO: Intrinsics per image (per source/target image)?
     :param coordinates: (batch, height, width, 3) - coordinates for the image. Pass this in so it doesn't need to be
         calculated on every warp step
     :return: The source image reprojected to the target
     """
     # Convert pose vector (output of pose net) to pose matrix (4x4)
+    pose_4x4 = pose_vec2mat(pose)
 
     # Convert intrinsics matrix (3x3) to (4x4) so it can be multiplied by the pose net
     # intrinsics_4x4 =
-
     # Calculate inverse of the 4x4 intrinsics matrix
     tf.linalg.inv()
 
-    # Create grid of homogenous coordinates
+    # Create grid (or array?) of homogenous coordinates
+    grid_coords = image_coordinate(*depth.shape)
+    # Flatten the image coords to [B, 3, height * width] so each point can be used in calculations
+    grid_coords = tf.transpose(tf.reshape(grid_coords, [0, 2, 1]))
+
+    # Get grid coordinates as array
+
+    # Do the function
+
+    # sample from the source image using the coordinates applied by the function
 
-    #
     pass

unsupervised/warp_tests.py

@@ -9,17 +9,20 @@ import warp
 class MyTestCase(unittest.TestCase):
     def test_euler_to_rotation_matrix(self):
         # quarter rotation in every
-        x, y, z = tf.expand_dims(tf.expand_dims(tf.constant(np.pi / 2), 0), 0)
+        x = y = z = tf.expand_dims(tf.expand_dims(tf.constant(np.pi / 2), 0), 0)
+        x2 = y2 = z2 = tf.expand_dims(tf.expand_dims(tf.constant(np.pi / 4), 0), 0)
+
+        x_batch = tf.concat([x, x2], 0)
+        y_batch = tf.concat([y, y2], 0)
+        z_batch = tf.concat([z, z2], 0)
 
         # TODO: Construct expected final rotation matrix, just 3x3 using numpy, so that we can do an
         #   elementwise comparison later. Probably also want to check the
 
-        rotation_matrices = warp.euler_to_rotation_matrix(x, y, z)
+        rotation_matrices = warp.euler_to_matrix(x_batch, y_batch, z_batch)
+        # old_rot = utils.euler2mat_noNDim(x_batch, y_batch, z_batch)
 
-        self.assertEqual(rotation_matrices.shape, [1, 3, 3])
+        self.assertEqual(rotation_matrices.shape, [2, 3, 3])
-        rot_mat = rotation_matrices[0]
-
-        # TODO: Element-wise checks...
 
     def test_coordinates(self):
         height = 1000