From df1ac89a815d229a13db35de541e1f5b3a6210f6 Mon Sep 17 00:00:00 2001 From: Piv <18462828+Piv200@users.noreply.github.com> Date: Tue, 10 Aug 2021 20:39:52 +0930 Subject: [PATCH] Add euler to rotation matrix, grid flattening --- .../{third-party => thirdparty}/utils.py | 62 ++++++++++++-- unsupervised/warp.py | 85 ++++++++++--------- unsupervised/warp_tests.py | 15 ++-- 3 files changed, 110 insertions(+), 52 deletions(-) rename unsupervised/{third-party => thirdparty}/utils.py (86%) diff --git a/unsupervised/third-party/utils.py b/unsupervised/thirdparty/utils.py similarity index 86% rename from unsupervised/third-party/utils.py rename to unsupervised/thirdparty/utils.py index 22ca1cb..fd82ab6 100644 --- a/unsupervised/third-party/utils.py +++ b/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 @@ -36,7 +38,7 @@ def euler2mat(z, y, x): 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_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) @@ -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')) - y = 0.5 * ((y + 1.0) * tf.cast(max_y-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')) # 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) - wb = (x1-x) * (y-y0) - wc = (x-x0) * (y1-y) - wd = (x-x0) * (y-y0) + wa = (x1 - x) * (y1 - y) + wb = (x1 - x) * (y - y0) + wc = (x - x0) * (y1 - y) + 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 diff --git a/unsupervised/warp.py b/unsupervised/warp.py index 190aa4d..e00bd1e 100644 --- a/unsupervised/warp.py +++ b/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) - y = tf.clip_by_value(y, -np.pi, np.pi) - x = tf.clip_by_value(x, -np.pi, np.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]) + 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) 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) - return rotMat + cosz = tf.cos(z) + 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 diff --git a/unsupervised/warp_tests.py b/unsupervised/warp_tests.py index a96c4ca..7a0c1c2 100644 --- a/unsupervised/warp_tests.py +++ b/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]) - rot_mat = rotation_matrices[0] - - # TODO: Element-wise checks... + self.assertEqual(rotation_matrices.shape, [2, 3, 3]) def test_coordinates(self): height = 1000