Beispiel #1
0
  def testGradientRandomValues(self):
    with self.test_session():
      us = [2, 3]
      u = tf.reshape([0.854, -0.616, 0.767, 0.725, -0.927, 0.159], shape=us)
      v = tf.reshape([-0.522, 0.755, 0.407, -0.652, 0.241, 0.247], shape=us)
      s = tf.cross(u, v)
      jacob_u, jacob_v = tf.test.compute_gradient([u, v], [us, us], s, us)

    self.assertAllClose(jacob_u[0], jacob_u[1], rtol=1e-3, atol=1e-3)
    self.assertAllClose(jacob_v[0], jacob_v[1], rtol=1e-3, atol=1e-3)
Beispiel #2
0
    def _call(self, inputs):
        x = inputs
        norm_x = tf.nn.l2_normalize(x, axis=1)
        norm_support = tf.nn.l2_normalize(self.support, axis=0)
        norm_mix = tf.cross(norm_x, norm_support)
        norm_mix = norm_mix*tf.inv(tf.reduce_sum(norm_mix))
        sampledIndex = tf.multinomial(tf.log(norm_mix), self.rank)
        new_support = dot(self.support,tf.diag(norm_mix),sparse=True)





        # dropout
        if self.sparse_inputs:
            x = sparse_dropout(x, 1-self.dropout, self.num_features_nonzero)
        else:
            x = tf.nn.dropout(x, 1-self.dropout)

        # convolve
        # supports = list()
        # for i in range(len(self.support)):
        #     if not self.featureless:
        #         pre_sup = dot(x, self.vars['weights_' + str(i)],
        #                       sparse=self.sparse_inputs)
        #     else:
        #         pre_sup = self.vars['weights_' + str(i)]
        #     support = dot(self.support[i], pre_sup, sparse=True)
        #     supports.append(support)
        # output = tf.add_n(supports)
        if not self.featureless:
            pre_sup = dot(x, self.vars['weights_0'],
                          sparse=self.sparse_inputs)
        else:
            pre_sup = self.vars['weights_0']
        output = dot(new_support, pre_sup, sparse=True)

        # bias
        if self.bias:
            output += self.vars['bias']

        return self.act(output)
Beispiel #3
0
    def get_ver_norm_bk(ver_xyz, ver_tri, tri, scope_name):
        """
        Compute vertex normals and vertex contour mask(for 2d lmk loss).

        :param:
            ver_xyz: [batch, N, 3], vertex geometry
            tri: [M, 3], mesh triangles definition

        :return:
            ver_normals: [batch, N, 3], vertex normals
            ver_contour_mask: [batch, N, 1], vertex contour mask, indicating whether the vertex is on the contour
        """
        batch_size, n_ver, n_channels = ver_xyz.get_shape().as_list()
        n_ver_tri_pair = ver_tri.get_shape().as_list()[0]

        n_tri = tri.get_shape().as_list()[0]
        v1_idx, v2_idx, v3_idx = tf.unstack(tri, 3, axis=-1)
        v1 = tf.gather(ver_xyz, v1_idx, axis=1, name="v1_tri")
        v2 = tf.gather(ver_xyz, v2_idx, axis=1, name="v2_tri")
        v3 = tf.gather(ver_xyz, v3_idx, axis=1, name="v3_tri")

        EPS = 1e-8
        tri_normals = tf.cross(v2 - v1, v3 - v1)
        tri_visible = tf.cast(tf.greater(tri_normals[:, :, 2:], float(EPS)),
                              tf.float32)

        with tf.variable_scope(scope_name, tf.AUTO_REUSE):

            var_sum_of_tri_normals = tf.get_variable('ver_sum_of_tri_normals',
                                                     [batch_size, n_ver, 3],
                                                     tf.float32,
                                                     tf.zeros_initializer(),
                                                     trainable=False)

            var_sum_of_tri_visible = tf.get_variable('ver_sum_of_tri_visible',
                                                     [batch_size, n_ver, 1],
                                                     tf.float32,
                                                     tf.zeros_initializer(),
                                                     trainable=False)

            var_sum_of_tri_counts = tf.get_variable('ver_sum_of_counts',
                                                    [batch_size, n_ver, 1],
                                                    tf.float32,
                                                    tf.zeros_initializer(),
                                                    trainable=False)

        init_sum_of_tri_normals = tf.zeros_like(var_sum_of_tri_normals)
        init_sum_of_tri_visible = tf.zeros_like(var_sum_of_tri_visible)
        init_sum_of_tri_counts = tf.zeros_like(var_sum_of_tri_counts)
        assign_op = tf.group([
            tf.assign(var_sum_of_tri_normals, init_sum_of_tri_normals),
            tf.assign(var_sum_of_tri_visible, init_sum_of_tri_visible),
            tf.assign(var_sum_of_tri_counts, init_sum_of_tri_counts)
        ],
                             name='assign_op')

        with tf.control_dependencies([assign_op]):
            to_ver_ids, from_tri_ids = tf.split(ver_tri, 2, axis=1)
            tmp_ver_tri_normals = tf.gather(tri_normals,
                                            tf.squeeze(from_tri_ids),
                                            axis=1)
            tmp_ver_tri_visible = tf.gather(tri_visible,
                                            tf.squeeze(from_tri_ids),
                                            axis=1)
            tmp_ver_tri_counts = tf.ones([batch_size, n_ver_tri_pair, 1],
                                         tf.float32)

            print(tmp_ver_tri_normals.get_shape().as_list())

            batch_indices = tf.reshape(tf.tile(
                tf.expand_dims(tf.range(batch_size), axis=1),
                [1, n_ver_tri_pair]), [batch_size, n_ver_tri_pair, 1],
                                       name='batch_indices')
            to_ver_ids = tf.tile(tf.expand_dims(to_ver_ids, axis=0),
                                 [batch_size, 1, 1])
            batch_to_ver_ids = tf.concat([batch_indices, to_ver_ids], axis=2)
            var_sum_of_tri_normals = tf.scatter_nd_add(var_sum_of_tri_normals,
                                                       batch_to_ver_ids,
                                                       tmp_ver_tri_normals)
            var_sum_of_tri_visible = tf.scatter_nd_add(var_sum_of_tri_visible,
                                                       batch_to_ver_ids,
                                                       tmp_ver_tri_visible)
            var_sum_of_tri_counts = tf.scatter_nd_add(var_sum_of_tri_counts,
                                                      batch_to_ver_ids,
                                                      tmp_ver_tri_counts)
            ver_normals = tf.div(var_sum_of_tri_normals,
                                 var_sum_of_tri_counts + EPS)
            ver_normals = tf.nn.l2_normalize(ver_normals)

            ver_visible = tf.div(var_sum_of_tri_visible,
                                 var_sum_of_tri_counts + EPS)

            cond1 = tf.less(ver_visible, float(1.0))
            cond2 = tf.greater(ver_visible, float(0.0))
            ver_contour_mask = tf.cast(
                tf.logical_and(cond1, cond2),
                tf.float32,
                name="ver_votes_final",
            )
            return ver_normals, ver_contour_mask
Beispiel #4
0
    def timeflow(self, t=0.0):
        self.setalpha(t)
        for p in range(numLeg):
            Qs = [tf.matmul(self.body.Q, self.leg[p].sub[0].Q)]
            #List of rotation matrices of each sublegs in space frame
            #Type : list of [3,3] Tensor

            for i in range(1, numsubleg):
                Qs.append(tf.matmul(Qs[i - 1], self.leg[p].sub[i].Q))

            e = [
                tf.matmul(Qs[i], tf.reshape(self.leg[p].sub[i].axis, [3, 1]))
                for i in range(numsubleg)
            ]
            #List of axes of each sublegs in space frame
            #Type : list of [3,1] Tensor

            Qalpha = [
                tf.scalar_mul(self.leg[p].sub[i].alpha, e[i])
                for i in range(numsubleg)
            ]

            Qw = [
                tf.scalar_mul(self.leg[p].sub[i].omega, e[i])
                for i in range(numsubleg)
            ]

            wbs = tf.matmul(self.body.Q, tf.reshape(self.body.wb,
                                                    [3, 1]))  #[3,1] matrix

            ws = [wbs + Qw[0]]
            for i in range(1, numsubleg):
                ws.append(ws[i - 1] + Qw[i])

            w = [
                tf.matmul(tf.transpose(Qs[i]), tf.reshape(ws[i], [3, 1]))
                for i in range(numsubleg)
            ]

            ls = [[
                tf.matmul(Qs[i], tf.reshape(self.leg[p].sub[i].l[0], [3, 1])),
                tf.matmul(Qs[i], tf.reshape(self.leg[p].sub[i].l[1], [3, 1]))
            ] for i in range(numsubleg)]  #ls = 2Dtensor

            lbtomotbs = tf.matmul(self.body.Q,
                                  tf.reshape(self.body.lbtomot[p],
                                             [3, 1]))  # lbtomotbs = 2Dtensor
            lbtomots = [tf.add(lbtomotbs, ls[0][0])]  # lbtomots = 2Dtensor

            for i in range(1, numsubleg):
                lbtomots.append(
                    tf.add(lbtomots[i - 1], tf.add(ls[i - 1][1], ls[i][0])))

            A = [
                tf.add(
                    tf.cross(wbs, tf.cross(wbs, lbtomotbs)),
                    tf.add(tf.cross(Qalpha[0], lbtomots[0]),
                           tf.cross(ws[0], tf.cross(ws[0], lbtomots[0]))))
            ]
            for i in range(1, numsubleg):
                A.append(tf.add(tf.cross(Qalpha[i - 1], tf.add())))
Beispiel #5
0
assert sys.version_info >= (3, 5)
# Scikit-Learn ≥0.20 is required
assert sklearn.__version__ >= "0.20"
# 屏蔽警告:Your CPU supports instructions that this TensorFlow binary was not compiled to use: AVX2 FMA
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'
# Open graph session
sess = tf.Session()

show_values(tf.div(3, 4), "tf.div(3,4) = 整数除")
show_values(tf.truediv(3, 4), "tf.truediv(3,4) = 浮点除")
show_values(tf.floordiv(3.0, 4.0), "tf.floordiv(3.0,4.0) = 浮点取整除")
show_values(tf.mod(22.0, 5.0), "tf.mod(22.0,5.0) = 取模")
# 张量点积--Compute the pairwise cross product
# 张量点积:即两个向量的叉乘,又叫向量积、外积、叉积,叉乘的运算结果是一个向量而不是一个标量。
# 两个向量的点积与这两个向量组成的坐标平面垂直。
show_values(tf.cross([1., 0., 0.], [0., 1., 0.]),
            "tf.cross([1., 0., 0.], [0., 1., 0.]) = 张量点积")
# 张量点积必须是三维的
# show_values(tf.cross([1., 0., 0., 0.], [0., 1., 0., 0.]),
#             "tf.cross([1., 0., 0.,0.], [0., 1., 0.,0.]) = 张量点积")

# ToSee:P11,数学函数列表

show_values(tf.div(tf.sin(3.1416 / 4.), tf.cos(3.1416 / 4.)),
            "tan(pi/4) = 1 = tf.div(tf.sin(3.1416/4.),tf.cos(3.1416/4.))")

test_nums = range(15)
# What should we get with list comprehension
expected_output = [3 * x * x - x + 10 for x in test_nums]
print('-' * 50)
print("[3 * x ^ 2 - x + 10 for x in test_nums] = ")
Beispiel #6
0
    def compute_camera_matrices(self, camera_position, camera_look_at,
                                camera_up):
        """Computes camera viewing matrices.

        Functionality mimes gluLookAt (third_party/GL/glu/include/GLU/glu.h).

        Args:
          camera_position: 2-D float32 tensor with shape [batch_size, 3] containing the XYZ world
              space position of the camera.
          camera_look_at: 2-D float32 tensor with shape [batch_size, 3] containing a position
              along the center of the camera's gaze.
          camera_up: 2-D float32 tensor with shape [batch_size, 3] specifying the
              world's up direction; the output camera will have no tilt with respect
              to this direction.

        Returns:
          A [batch_size, 4, 4] float tensor containing a right-handed camera
          extrinsics matrix that maps points from world space to points in eye space.
        """
        batch_size = camera_look_at.shape[0].value
        vector_degeneracy_cutoff = 1e-6

        forward = camera_look_at - camera_position
        forward_norm = tf.norm(forward, ord='euclidean', axis=1, keepdims=True)
        tf.assert_greater(
            forward_norm,
            vector_degeneracy_cutoff,
            message=
            'Camera matrix is degenerate because eye and center are close.')
        forward = tf.divide(forward, forward_norm)

        to_side = tf.cross(forward, camera_up)
        to_side_norm = tf.norm(to_side, ord='euclidean', axis=1, keepdims=True)
        tf.assert_greater(
            to_side_norm,
            vector_degeneracy_cutoff,
            message=
            'Camera matrix is degenerate because up and gaze are close or because up is degenerate.'
        )

        to_side = tf.divide(to_side, to_side_norm)
        cam_up = tf.cross(to_side, forward)

        w_column = tf.constant(batch_size * [[0., 0., 0., 1.]],
                               dtype=tf.float32)
        w_column = tf.reshape(w_column, [batch_size, 4, 1])
        view_rotation = tf.stack([
            to_side, cam_up, -forward,
            tf.zeros_like(to_side, dtype=tf.float32)
        ],
                                 axis=1)
        view_rotation = tf.concat([view_rotation, w_column], axis=2)

        identity_batch = tf.tile(tf.expand_dims(tf.eye(3), 0),
                                 [batch_size, 1, 1])
        view_translation = tf.concat(
            [identity_batch,
             tf.expand_dims(-camera_position, 2)], 2)
        view_translation = tf.concat(
            [view_translation,
             tf.reshape(w_column, [batch_size, 1, 4])], 1)
        camera_matrices = tf.matmul(view_rotation, view_translation)

        return camera_matrices
Beispiel #7
0
def expSE3(x, batch):

    one_6th = tf.constant(1.0 / 6.0)
    one_20th = tf.constant(1.0 / 20.0)

    w = x[:, 3:6]

    theta_sq = tf.tensordot(w, w, axes=[[1], [1]])
    #theta_sq = tf.matmul(w,w,transpose_a=False,transpose_b=True)
    theta_sq = theta_sq[:, 0]

    theta = tf.sqrt(theta_sq)
    #w = tf.squeeze(w)
    cross_ = tf.cross(w, x[:, 0:3])

    A1 = 1.0 - one_6th * theta_sq
    B1 = 0.5 * tf.ones_like(A1)
    translation1 = (x[:, 0:3] + 0.5 * cross_)

    C2 = one_6th * (1.0 - one_20th * theta_sq)
    A2 = 1.0 - theta_sq * C2
    B2 = 0.5 - 0.25 * one_6th * theta_sq

    inv_theta = 1.0 / theta
    A3 = tf.sin(theta) * inv_theta
    B3 = (1 - tf.cos(theta)) * (inv_theta * inv_theta)
    C3 = (1 - A3) * (inv_theta * inv_theta)

    bool_tensor = tf.less(theta_sq, 1e-8)

    bool_tensor2 = tf.less(theta_sq, 1e-6)
    A_Alternate = tf.where(bool_tensor2, A2, A3)
    B_Alternate = tf.where(bool_tensor2, B2, B3)
    C = tf.where(bool_tensor2, C2, C3)

    A = tf.where(bool_tensor, A1, A_Alternate)
    B = tf.where(bool_tensor, B1, B_Alternate)

    translation2 = (x[:, 0:3] + tf.expand_dims(B, -1) * cross_ +
                    tf.expand_dims(C, -1) * (tf.cross(w, cross_)))

    translation = tf.where(bool_tensor, translation1, translation2)

    wx2 = w[:, 0] * w[:, 0]
    wy2 = w[:, 1] * w[:, 1]
    wz2 = w[:, 2] * w[:, 2]

    r00 = 1.0 - B * (wy2 + wz2)
    r11 = 1.0 - B * (wx2 + wz2)
    r22 = 1.0 - B * (wx2 + wz2)

    a = A * w[:, 2]
    b = B * (w[:, 0] * w[:, 1])
    r01 = b - a
    r10 = b + a

    a = A * w[:, 1]
    b = B * (w[:, 0] * w[:, 2])
    r02 = b + a
    r20 = b - a

    a = A * w[:, 0]
    b = B * (w[:, 1] * w[:, 2])
    r12 = b - a
    r21 = b + a

    c0 = tf.stack([r00, r01, r02, translation[:, 0]])
    c1 = tf.stack([r10, r11, r12, translation[:, 1]])
    c2 = tf.stack([r20, r21, r22, translation[:, 2]])

    c4 = tf.constant([0.0, 0.0, 0.0, 1.0], shape=[1, 4])
    c4 = tf.transpose(tf.tile(c4, [batch, 1]))

    SE3out = tf.transpose(tf.stack([c0, c1, c2, c4]), [2, 0, 1])

    return SE3out
Beispiel #8
0
    batch_size * npoint         int32
    '''
    return sampling_module.farthest_point_sample(inp, npoint)
ops.NoGradient('FarthestPointSample')
    

if __name__=='__main__':
    import numpy as np
    np.random.seed(100)
    triangles=np.random.rand(1,5,3,3).astype('float32')
    with tf.device('/gpu:1'):
        inp=tf.constant(triangles)
        tria=inp[:,:,0,:]
        trib=inp[:,:,1,:]
        tric=inp[:,:,2,:]
        areas=tf.sqrt(tf.reduce_sum(tf.cross(trib-tria,tric-tria)**2,2)+1e-9)
        randomnumbers=tf.random_uniform((1,8192))
        triids=prob_sample(areas,randomnumbers)
        tria_sample=gather_point(tria,triids)
        trib_sample=gather_point(trib,triids)
        tric_sample=gather_point(tric,triids)
        us=tf.random_uniform((1,8192))
        vs=tf.random_uniform((1,8192))
        uplusv=1-tf.abs(us+vs-1)
        uminusv=us-vs
        us=(uplusv+uminusv)*0.5
        vs=(uplusv-uminusv)*0.5
        pt_sample=tria_sample+(trib_sample-tria_sample)*tf.expand_dims(us,-1)+(tric_sample-tria_sample)*tf.expand_dims(vs,-1)
        print 'pt_sample: ', pt_sample
        reduced_sample=gather_point(pt_sample,farthest_point_sample(1024,pt_sample))
        print reduced_sample
Beispiel #9
0
def batch_laplacian(v, f, return_sparse=True):
    # v: B x N x 3
    # f: M x 3

    num_b = tf.shape(v)[0]
    num_v = tf.shape(v)[1]
    num_f = tf.shape(f)[0]

    v_a = f[:, 0]
    v_b = f[:, 1]
    v_c = f[:, 2]

    a = tf.gather(v, v_a, axis=1)
    b = tf.gather(v, v_b, axis=1)
    c = tf.gather(v, v_c, axis=1)

    ab = a - b
    bc = b - c
    ca = c - a

    cot_a = -1 * tf.reduce_sum(ab * ca, axis=2) / \
        tf.sqrt(tf.reduce_sum(tf.cross(ab, ca) ** 2, axis=-1))
    cot_b = -1 * tf.reduce_sum(bc * ab, axis=2) / \
        tf.sqrt(tf.reduce_sum(tf.cross(bc, ab) ** 2, axis=-1))
    cot_c = -1 * tf.reduce_sum(ca * bc, axis=2) / \
        tf.sqrt(tf.reduce_sum(tf.cross(ca, bc) ** 2, axis=-1))

    I = tf.tile(
        tf.expand_dims(tf.concat((v_a, v_c, v_a, v_b, v_b, v_c), axis=0), 0),
        (num_b, 1))
    J = tf.tile(
        tf.expand_dims(tf.concat((v_c, v_a, v_b, v_a, v_c, v_b), axis=0), 0),
        (num_b, 1))

    W = 0.5 * tf.concat((cot_b, cot_b, cot_c, cot_c, cot_a, cot_a), axis=1)

    batch_dim = tf.tile(tf.expand_dims(tf.range(num_b), 1), (1, num_f * 6))

    indices = tf.reshape(tf.stack((batch_dim, J, I), axis=2),
                         (num_b, 6, -1, 3))
    W = tf.reshape(W, (num_b, 6, -1))

    l_indices = [
        tf.cast(tf.reshape(indices[:, i], (-1, 3)), tf.int64) for i in range(6)
    ]
    shape = tf.cast(tf.stack((num_b, num_v, num_v)), tf.int64)
    sp_L_raw = [
        tf.sparse_reorder(
            tf.SparseTensor(l_indices[i], tf.reshape(W[:, i], (-1, )), shape))
        for i in range(6)
    ]

    L = sp_L_raw[0]
    for i in range(1, 6):
        L = tf.sparse_add(L, sp_L_raw[i])

    dia_values = tf.sparse_reduce_sum(L, axis=-1) * -1

    I = tf.tile(tf.expand_dims(tf.range(num_v), 0), (num_b, 1))
    batch_dim = tf.tile(tf.expand_dims(tf.range(num_b), 1), (1, num_v))
    indices = tf.reshape(tf.stack((batch_dim, I, I), axis=2), (-1, 3))

    dia = tf.sparse_reorder(
        tf.SparseTensor(tf.cast(indices, tf.int64),
                        tf.reshape(dia_values, (-1, )), shape))

    return tf.sparse_add(L, dia)
Beispiel #10
0
 def test_Cross(self):
     t = tf.cross(*self.random((4, 3), (4, 3)))
     self.check(t)
    def timeflow(self, t=0.0):

        wbs = tf.matmul(self.body.wb, self.body.Q)  #[1,3] matrix
        tot_lbtomots = []

        Feqc = tf.constant([0, 0, -Mtot * 9.81], dtype=tf.float32)
        Teqc = tf.zeros([1, 3], dtype=tf.float32)

        #List of External Forces
        Flist = []
        llist = []
        for p in range(numLeg):
            for i in range(numsubleg):
                #print('alpha = ',self.leg[p].sub[i].alpha);
                self.leg[p].sub[i].omega += self.leg[p].sub[
                    i].alpha * dtime  #omega를 시간에 따라 갱신
                #print('omega = ',self.leg[p].sub[i].omega);
                self.leg[p].sub[i].theta += self.leg[p].sub[
                    i].omega * dtime  #theta를 시간에 따라 갱신
                #print('theta = ',self.leg[p].sub[i].theta);
                self.leg[p].sub[i].Q = tf.scalar_mul(tf.cos(self.leg[p].sub[i].theta), tf.eye(3, dtype=tf.float32)) + \
                tf.scalar_mul(1.-tf.cos(self.leg[p].sub[i].theta), tf.matmul(self.leg[p].sub[i].axis, self.leg[p].sub[i].axis, transpose_a = True)) + \
                tf.scalar_mul(tf.sin(self.leg[p].sub[i].theta), tf.cross(tf.tile(self.leg[p].sub[i].axis,[3,1]), tf.eye(3, dtype=tf.float32)))

            Qs = [tf.matmul(self.leg[p].sub[0].Q,
                            self.body.Q)]  #Qs는 i번째 subleg에서 space로의 좌표변환
            #List of rotation matrices of each sublegs in space frame
            #Type : list of [3,3] Tensor
            for i in range(1, numsubleg):
                Qs.append(tf.matmul(self.leg[p].sub[i].Q, Qs[i - 1]))

            e = [
                tf.matmul(self.leg[p].sub[i].axis, Qs[i])
                for i in range(numsubleg)
            ]
            #List of axes of each sublegs in space frame
            #Type : list of [None,3] Tensor

            Qw = [
                tf.scalar_mul(self.leg[p].sub[i].omega, e[i])
                for i in range(numsubleg)
            ]

            ws = [wbs + Qw[0]]
            for i in range(1, numsubleg):
                ws.append(ws[i - 1] + Qw[i])

            ls = [[
                tf.matmul(self.leg[p].sub[i].l[0], Qs[i]),
                tf.matmul(self.leg[p].sub[i].l[1], Qs[i])
            ] for i in range(numsubleg)]  #ls = 2Dtensor

            lbtomotbs = tf.matmul(self.body.lbtomot[p],
                                  self.body.Q)  # lbtomotbs = 2Dtensor

            lbtomots = [lbtomotbs + ls[0][0]]  # lbtomots = 2Dtensor

            for i in range(1, numsubleg):
                lbtomots.append(lbtomots[i - 1] + ls[i - 1][1] + ls[i][0])

            #Calculating External Forces
            vstmp = self.body.vs + tf.cross(wbs, lbtomotbs)
            NormalScale = 300.0
            TanhConst = 100.0

            Zfilter = tf.constant([[0., 0., 1.]])
            negZfilter = tf.constant([[0., 0., -1.]])
            XYfilter = tf.constant([[1., 1., 0.]])
            for i in range(numsubleg):
                Fz_primi = tf.multiply(negZfilter,
                                       self.body.rs + lbtomots[i] + ls[i][1])

                colbool = tf.reshape(
                    tf.matmul(Zfilter, tf.nn.relu(Fz_primi), transpose_b=True),
                    [])

                Fz = tf.scalar_mul(NormalScale, tf.nn.relu(Fz_primi))
                vstmp += tf.cross(ws[i], ls[i][0] + ls[i][1])
                vstmp_z = tf.reshape(
                    tf.matmul(Zfilter, vstmp, transpose_b=True), [])

                Vscale = 3. - tf.nn.tanh(TanhConst * vstmp_z)

                Fnormal = tf.scalar_mul(Vscale, Fz)

                Flist.append(Fnormal)

                vstmp_xy = tf.multiply(XYfilter, vstmp)
                Ffric = tf.scalar_mul(-Fricscale,
                                      tf.scalar_mul(colbool, vstmp_xy))

                Flist.append(Ffric)

                Feqc += Fnormal
                Feqc += Ffric

                Teqc += tf.cross(lbtomots[i] + ls[i][1], Fnormal + Ffric)
                llist.append(tf.norm(lbtomots[i] + ls[i][1]))
                #Teqc+=

            tot_lbtomots += lbtomots
        asb = tf.scalar_mul(Mtotinv, Feqc)
        alphab = tf.matmul(tf.matmul(Teqc, self.body.Q, transpose_b=True),
                           Ibinv)
        self.body.wb += tf.scalar_mul(dtime, alphab)
        self.body.Q += tf.scalar_mul(
            dtime, tf.cross(tf.concat([wbs, wbs, wbs], axis=0), self.body.Q))
        self.body.vs += tf.scalar_mul(dtime, asb)
        self.body.rs += tf.scalar_mul(dtime, self.body.vs)
        # Q to quaternion
        '''
        qw = tf.scalar_mul(0.5, tf.sqrt(tf.reduce_sum(tf.diag_part(self.body.Q))+1.))
        qv = tf.reduce_sum(tf.cross(self.body.Q, tf.eye(3, dtype = tf.float32)), axis = 0)/tf.scalar_mul(4., qw)

        # quaternion normalization
        
        qvsquare = tf.reduce_sum(tf.square(qv))
        qnorm = tf.sqrt(tf.square(qw)+qvsquare)
        qw /= qnorm
        qv /= qnorm
        # quaternion to Q

        self.body.Q = tf.scalar_mul(qw*qw-qvsquare,tf.eye(3, dtype = tf.float32))\
            + 2 * tf.matmul(tf.reshape(qv, [3, 1]), tf.reshape(qv, [1, 3]))\
            - 2 * qw * tf.cross(tf.tile(tf.reshape(qv, [1,3]), [3,1]), tf.eye(3, dtype = tf.float32))
        '''
        return llist, Flist, asb, Qs, [self.body.rs] + [
            x + self.body.rs for x in tot_lbtomots
        ]  #, qnorm, qw
Beispiel #12
0
    def draw_rendering_net(self,
                           input_params,
                           position,
                           rotate_theta,
                           variable_scope_name,
                           with_cos=True,
                           pd_ps_wanted="both"):
        '''
        input_params = (rendering parameters) shape = [self.fitting_batch_size,self.parameter_len] i.e.[24576,10]
        position = (rendering positions) shape=[self.fitting_batch_size,3]
        variable_scope_name = (for variable check a string like"rendering1") 
        rotate_theta = [self.fitting_batch_size,1]
        return shape = (rendered results)[batch,lightnum,1] or [batch,lightnum,3]


        with_cos: if True,lumitexl is computed with cos and dir
        '''
        with tf.variable_scope(variable_scope_name):
            ###[STEP 0]
            #load constants
            view_mat_for_normal_t, view_mat_model_t, light_normals, light_poses, cam_pos = self.__net_load_constants(
                variable_scope_name)
            #rotate object
            rotate_axis = tf.constant(
                np.repeat(np.array([0, 0, 1], np.float32).reshape([1, 3]),
                          self.fitting_batch_size,
                          axis=0))
            view_mat_model = self.rotation_axis(rotate_theta,
                                                rotate_axis)  #[batch,4,4]
            self.endPoints[variable_scope_name +
                           "view_mat_model"] = view_mat_model
            view_mat_model_t = tf.matrix_transpose(view_mat_model)

            view_mat_for_normal = tf.matrix_transpose(
                tf.matrix_inverse(view_mat_model))
            self.endPoints[variable_scope_name +
                           "view_mat_for_normal"] = view_mat_for_normal
            view_mat_for_normal_t = tf.matrix_transpose(view_mat_for_normal)
            # self.endPoints[variable_scope_name+"view_mat_for_normal_t"] = view_mat_for_normal
            ###[STEP 1]
            ##define input
            with tf.variable_scope("fittinger"):
                self.endPoints[variable_scope_name +
                               "input_parameters"] = input_params
                self.endPoints[variable_scope_name + "positions"] = position
                view_dir = cam_pos - position  #shape=[batch,3]
                view_dir = self.normalized(view_dir)  #shape=[batch,3]
                self.endPoints[variable_scope_name + "view_dir"] = view_dir

                #build local frame
                frame_t, frame_b = self.build_frame_f_z(
                    view_dir, None, with_theta=False)  #[batch,3]
                frame_n = view_dir
                self.endPoints[variable_scope_name + "frame_t"] = frame_t
                self.endPoints[variable_scope_name + "frame_b"] = frame_b
                self.endPoints[variable_scope_name + "frame_n"] = frame_n

            ###[STEP 1.1]
            ###split input parameters into position and others
            if self.if_grey_scale:
                n_2d, theta, ax, ay, pd, ps = tf.split(input_params,
                                                       [2, 1, 1, 1, 1, 1],
                                                       axis=1)
                self.endPoints[variable_scope_name + "pd"] = pd
                pd = tf.tile(pd, [1, 3])
                ps = tf.tile(ps, [1, 3])
            else:
                n_local, t_local, ax, ay, pd, ps = tf.split(input_params,
                                                            [2, 1, 1, 1, 3, 3],
                                                            axis=1)

            #position shape=[bach,3]
            # n_2d = tf.clip_by_value(n_2d,0.0,1.0)
            n_local = self.back_hemi_octa_map(n_2d)  #[batch,3]
            self.endPoints[variable_scope_name + "normal_local"] = n_local
            t_local, _ = self.build_frame_f_z(n_local, theta, with_theta=True)
            n_local_x, n_local_y, n_local_z = tf.split(
                n_local, [1, 1, 1], axis=1)  #[batch,1],[batch,1],[batch,1]
            n = n_local_x * frame_t + n_local_y * frame_b + n_local_z * frame_n  #[batch,3]
            self.endPoints[variable_scope_name + "normal"] = n
            t_local_x, t_local_y, t_local_z = tf.split(
                t_local, [1, 1, 1], axis=1)  #[batch,1],[batch,1],[batch,1]
            t = t_local_x * frame_t + t_local_y * frame_b + t_local_z * frame_n  #[batch,3]
            b = tf.cross(n, t)  #[batch,3]

            #rotate frame
            pn = tf.expand_dims(tf.concat(
                [n, tf.ones([n.shape[0], 1], tf.float32)], axis=1),
                                axis=1)
            pt = tf.expand_dims(tf.concat(
                [t, tf.ones([t.shape[0], 1], tf.float32)], axis=1),
                                axis=1)
            pb = tf.expand_dims(tf.concat(
                [b, tf.ones([b.shape[0], 1], tf.float32)], axis=1),
                                axis=1)

            n = tf.squeeze(tf.matmul(pn, view_mat_for_normal_t), axis=1)
            t = tf.squeeze(tf.matmul(pt, view_mat_for_normal_t), axis=1)
            b = tf.squeeze(tf.matmul(pb, view_mat_for_normal_t), axis=1)
            n, _ = tf.split(n, [3, 1], axis=1)  #shape=[batch,3]
            t, _ = tf.split(t, [3, 1], axis=1)  #shape=[batch,3]
            b, _ = tf.split(b, [3, 1], axis=1)  #shape=[batch,3]
            self.endPoints[variable_scope_name + "n"] = n
            self.endPoints[variable_scope_name + "t"] = t
            self.endPoints[variable_scope_name + "b"] = b
            # n = tf.tile(tf.constant([0,0,1],dtype=tf.float32,shape=[1,3]),[self.fitting_batch_size,1])#self.normalized(n)
            # t = tf.tile(tf.constant([0,1,0],dtype=tf.float32,shape=[1,3]),[self.fitting_batch_size,1])#self.normalized(t)
            # ax = tf.clip_by_value(ax,0.006,0.503)
            # ay = tf.clip_by_value(ay,0.006,0.503)
            # pd = tf.clip_by_value(pd,0,1)
            # ps = tf.clip_by_value(ps,0,10)

            #regularizer of params
            # regular_loss_n = self.regularizer_relu(n_2d,1e-3,1.0)+self.regularizer_relu(theta,0.0,math.pi)+self.regularizer_relu(ax,0.006,0.503)+self.regularizer_relu(ay,0.006,0.503)+self.regularizer_relu(pd,1e-3,1)+self.regularizer_relu(ps,1e-3,10)
            # self.endPoints["regular_loss"] = regular_loss_n

            self.endPoints[variable_scope_name + "render_params"] = tf.concat(
                [n, t, b, ax, ay, pd, ps], axis=-1)
            ###[STEP 2]
            ##define rendering
            with tf.variable_scope("rendering"):
                self.endPoints[variable_scope_name +
                               "position_origin"] = position
                position = tf.expand_dims(tf.concat(
                    [position,
                     tf.ones([position.shape[0], 1], tf.float32)],
                    axis=1),
                                          axis=1)
                position = tf.squeeze(tf.matmul(position, view_mat_model_t),
                                      axis=1)  #position@view_mat_model_t
                position, _ = tf.split(position, [3, 1],
                                       axis=1)  #shape=[batch,3]
                self.endPoints[variable_scope_name +
                               "position_rotated"] = position

                #get real view dir
                view_dir = cam_pos - position  #shape=[batch,3]
                view_dir = self.normalized(view_dir)  #shape=[batch,3]
                self.endPoints[variable_scope_name +
                               "view_dir_rotated"] = view_dir

                light_poses_broaded = tf.tile(
                    tf.expand_dims(light_poses,
                                   axis=0), [self.fitting_batch_size, 1, 1],
                    name="expand_light_poses")  #shape is [batch,lightnum,3]
                light_normals_broaded = tf.tile(
                    tf.expand_dims(light_normals,
                                   axis=0), [self.fitting_batch_size, 1, 1],
                    name="expand_light_normals")  #shape is [batch,lightnum,3]
                position_broded = tf.tile(tf.expand_dims(position, axis=1),
                                          [1, self.lumitexel_size, 1],
                                          name="expand_position")
                wi = light_poses_broaded - position_broded
                wi = self.normalized_nd(wi)  #shape is [batch,lightnum,3]
                self.endPoints[variable_scope_name + "wi"] = wi

                wi_local = tf.concat([
                    self.dot_ndm_vector(wi, tf.expand_dims(t, axis=1)),
                    self.dot_ndm_vector(wi, tf.expand_dims(b, axis=1)),
                    self.dot_ndm_vector(wi, tf.expand_dims(n, axis=1))
                ],
                                     axis=-1)  #shape is [batch,lightnum,3]

                view_dir_broaded = tf.tile(
                    tf.expand_dims(view_dir, axis=1),
                    [1, self.lumitexel_size, 1])  #shape is [batch,lightnum,3]
                wo_local = tf.concat([
                    self.dot_ndm_vector(view_dir_broaded,
                                        tf.expand_dims(t, axis=1)),
                    self.dot_ndm_vector(view_dir_broaded,
                                        tf.expand_dims(b, axis=1)),
                    self.dot_ndm_vector(view_dir_broaded,
                                        tf.expand_dims(n, axis=1))
                ],
                                     axis=-1)  #shape is [batch,lightnum,3]

                self.endPoints[variable_scope_name + "wi_local"] = wo_local
                form_factors = self.compute_form_factors(
                    position, n, light_poses_broaded, light_normals_broaded,
                    variable_scope_name, with_cos)  #[batch,lightnum,1]
                self.endPoints[variable_scope_name +
                               "form_factors"] = form_factors
                lumi = self.calc_light_brdf(wi_local, wo_local, ax, ay, pd, ps,
                                            pd_ps_wanted)  #[batch,lightnum,3]
                self.endPoints[variable_scope_name +
                               "lumi_without_formfactor"] = lumi

                lumi = lumi * form_factors * 1e4 * math.pi * 1e-2  #[batch,lightnum,3]
                if self.if_grey_scale:
                    lumi = tf.reduce_mean(
                        lumi, axis=2, keepdims=True
                    )  #[batch,lightnum,1] for greyscale#TODO should be depended by if is grey

                wi_dot_n = self.dot_ndm_vector(wi, tf.expand_dims(
                    n, axis=1))  #[batch,lightnum,1]
                lumi = lumi * ((tf.sign(wi_dot_n) + 1.0) * 0.5)
                # judgements = tf.less(wi_dot_n,1e-5)
                # lumi = tf.where(judgements,tf.zeros([self.fitting_batch_size,self.lumitexel_size,1]),lumi)

                n_dot_view_dir = self.dot_ndm_vector(
                    view_dir_broaded,
                    tf.tile(tf.expand_dims(n, axis=1),
                            [1, self.lumitexel_size, 1]))  #[batch,lightnum,1]

                n_dot_views = tf.gather(n_dot_view_dir, 0, axis=1)
                self.endPoints[variable_scope_name +
                               "n_dot_view_dir"] = n_dot_views
                n_dot_view_penalty = self.regularizer_relu(
                    n_dot_views, 1e-6, 1.0)
                self.endPoints[variable_scope_name +
                               "n_dot_view_penalty"] = n_dot_view_penalty

                judgements = tf.less(n_dot_view_dir, 0.0)
                if self.if_grey_scale:
                    rendered_results = tf.where(judgements,
                                                tf.zeros([
                                                    self.fitting_batch_size,
                                                    self.lumitexel_size, 1
                                                ]), lumi)  #[batch,lightnum]
                else:
                    rendered_results = tf.where(tf.tile(judgements, [1, 1, 3]),
                                                tf.zeros([
                                                    self.fitting_batch_size,
                                                    self.lumitexel_size, 3
                                                ]), lumi)  #[batch,lightnum]
                self.endPoints[variable_scope_name +
                               "rendered_results"] = rendered_results

        return rendered_results
Beispiel #13
0
 def calculateDistance(self, pA, pB, pC, pD):
     vAB = pB - pA # u
     vCD = pD - pC # v
     vCA = pA - pC # r
     
     if tf.tensordot(tf.cross(vAB, vCD), vCA, axes=1) != 0: # check determination in 3-dimension or planar
         a = tf.tensordot( vAB, vAB, axes=1)
         b = tf.tensordot(-vAB, vCD, axes=1)
         c = tf.tensordot( vCD, vCD, axes=1)
         d = tf.tensordot( vAB, vCA, axes=1)
         e = tf.tensordot(-vCD, vCA, axes=1)
         f = tf.tensordot( vCA, vCA, axes=1)
         s = (b*e-c*d)/(a*c-b*b) # parameter in vector A (vector M, L1, AB, i-th robot link)
         t = (b*d-a*e)/(a*c-b*b) # parameter in vector B (vector N, L2, CD, j-th line obstacle)
         
         if (s < 0) and (t < 0):  # AC
             distance = tf.linalg.norm(vCA)
         elif (s < 0) and (0 <= t) and (t <= 1): # AC + tCD
             distance = tf.linalg.norm(-vCA + t*vCD)
         elif (s < 0) and (1 < t):  # AD
             distance = tf.linalg.norm(pD-pA)
         elif (0 <= s) and (s <= 1) and (t < 0): # CA + sAB
             distance = tf.linalg.norm(vCA + s*vAB)
         elif (0 <= s) and (s <= 1) and (0 <= t) and (t <= 1):
             distance = tf.sqrt(a*s**2 + 2*b*s*t + c*t**2 + 2*d*s + 2*e*t + f)
         elif (0 <= s) and (s <= 1) and (1 < t): # DA + sAB
             distance = tf.linalg.norm(pA-pD + s*vAB)           
         elif (1 < s) and (t < 0):  # BC
             distance = tf.linalg.norm(pC-pB)
         elif (1 < s) and (0 <= t) and (t <= 1): # BC + tCD
             distance = tf.linalg.norm(pC-pB + t*vCD)            
         elif (1 < s) and (1 < t):  # BD
             distance = tf.linalg.norm(pD-pB)
     else:
         vAC = pC - pA
         vAD = pD - pA
         vCB = pB - pC        
         s1 = tf.tensordot(vAC, vAB, axes=1)/(tf.linalg.norm(vAB)*tf.linalg.norm(vAB))
         s2 = tf.tensordot(vAD, vAB, axes=1)/(tf.linalg.norm(vAB)*tf.linalg.norm(vAB))
         t1 = tf.tensordot(vCA, vCD, axes=1)/(tf.linalg.norm(vCD)*tf.linalg.norm(vCD))
         t2 = tf.tensordot(vCB, vCD, axes=1)/(tf.linalg.norm(vCD)*tf.linalg.norm(vCD))   
         distance_planar = tf.constant([999, 999, 999, 999]) # s1 s2 t1 t2
         distance_Temp = 999
         
         if (0 <= s1) and (s1 <= 1):
             distance_planar[0] = tf.linalg.norm(vCA + s1*vAB)   # CA + s1*AB        
         if (0 <= s2) and (s2 <= 1):
             distance_planar[1] = tf.linalg.norm(-vAD + s2*vAB)  # DA + s2*AB        
         if (0 <= t1) and (t1 <= 1):
             distance_planar[2] = tf.linalg.norm(vAC + t1*vCD)   # AC + t1*CD        
         if (0 <= t2) and (t2 <= 1):
             distance_planar[3] = tf.linalg.norm(pC-pB + t2*vCD) # BC + t2*CD
         
         if (s1 < 0) and (s2 < 0) and (t1 < 0) and (t2 < 0):   # AC
             distance_planar[0] = tf.linalg.norm(vAC)
         elif (s1 > 1) and (s2 > 1) and (t1 < 0) and (t2 < 0): # BC
             distance_planar[1] = tf.linalg.norm(pC - pB)
         elif (s1 < 0) and (s2 < 0) and (t1 > 1) and (t2 > 1): # AD
             distance_planar[2] = tf.linalg.norm(vAD)
         elif (s1 > 1) and (s2 > 1) and (t1 > 1) and (t2 > 1): # BD
             distance_planar[3] = tf.linalg.norm(pD - pB)
         
         for cnt in range(1,4):
             if distance_Temp > distance_planar[cnt]:
                 distance_Temp =  distance_planar[cnt]
                 # cnt_Temp = cnt
         distance = distance_Temp
     return distance
def cross(*args, **kwargs):
    """ See https://www.tensorflow.org/api_docs/python/tf/cross .
    """
    return tensorflow.cross(*args, **kwargs)
Beispiel #15
0
    def get_ver_norm_bk(ver_xyz, tri):
        """
        Compute vertex normals and vertex contour mask(for 2d lmk loss).

        :param:
            ver_xyz: [batch, N, 3], vertex geometry
            tri: [M, 3], mesh triangles definition

        :return:
            ver_normals: [batch, N, 3], vertex normals
            ver_contour_mask: [batch, N, 1], vertex contour mask, indicating whether the vertex is on the contour
        """
        n_tri = tri.get_shape().as_list()[0]
        v1_idx, v2_idx, v3_idx = tf.unstack(tri, 3, axis=-1)
        v1 = tf.gather(ver_xyz, v1_idx, axis=1, name="v1_tri")
        v2 = tf.gather(ver_xyz, v2_idx, axis=1, name="v2_tri")
        v3 = tf.gather(ver_xyz, v3_idx, axis=1, name="v3_tri")

        EPS = 1e-8
        tri_normals = tf.cross(v2 - v1, v3 - v1)
        #tri_normals = tf.div(
        #    tri_normals,
        #    (tf.norm(tri_normals, axis=-1, keep_dims=True) + EPS),
        #    name="norm_tri",
        #)
        #tri_normals = tf.nn.l2_normalize(tri_normals, dim=-1)
        tmp = tf.tile(tf.expand_dims(tri_normals, 2), [1, 1, 3, 1],
                      name="tri_normals_tile")  # per vertex attribute
        # per_vertex: information for each vertex in triangle
        tri_normals_per_vertex = tf.reshape(tmp, [-1, 3],
                                            name="tri_normals_reshape")
        tri_visible_per_vertex = tf.cast(
            tf.greater(tri_normals_per_vertex[:, 2:], float(EPS)), tf.float32)
        tri_one_per_vertex = tf.ones_like(tri_visible_per_vertex,
                                          name="tri_cnts")

        B = v1.get_shape().as_list()[0]  # batch size
        batch_indices = tf.reshape(
            tf.tile(tf.expand_dims(tf.range(B), axis=1), [1, n_tri * 3]),
            [-1],
            name="batch_indices",
        )
        tri_inds = tf.stack(
            [
                batch_indices,
                tf.concat([tf.reshape(tri, [n_tri * 3])] * B, axis=0)
            ],
            axis=1,
            name="tri_inds",
        )

        ver_shape = ver_xyz.get_shape().as_list()

        ver_normals = tf.get_variable(
            shape=ver_shape,
            dtype=tf.float32,
            initializer=tf.zeros_initializer(),
            name="ver_norm",
            trainable=False,
        )

        # refresh normal per iteration
        init_normals = tf.zeros(shape=ver_shape,
                                dtype=tf.float32,
                                name="init_normals")
        assign_op = tf.assign(ver_normals,
                              init_normals,
                              name="ver_normal_assign")
        with tf.control_dependencies([assign_op]):
            ver_normals = tf.scatter_nd_add(ver_normals,
                                            tri_inds,
                                            tri_normals_per_vertex,
                                            name="ver_normal_scatter")
            #ver_normals = ver_normals / (
            #    tf.norm(ver_normals, axis=2, keep_dims=True, name="ver_normal_norm")
            #    + EPS
            #)
            ver_normals = tf.nn.l2_normalize(ver_normals, dim=-1)

        ver_visible = tf.get_variable(
            shape=ver_shape[:-1] + [1],
            dtype=tf.float32,
            initializer=tf.zeros_initializer(),
            name="ver_vote",
            trainable=False,
        )
        ver_tri_cnt = tf.get_variable(
            shape=ver_shape[:-1] + [1],
            dtype=tf.float32,
            initializer=tf.zeros_initializer(),
            name="ver_cnt",
            trainable=False,
        )
        init_values = tf.zeros(shape=ver_shape[:-1] + [1],
                               dtype=tf.float32,
                               name="init_votes")
        # find the visible boundary
        assign_op2 = tf.assign(ver_visible,
                               init_values,
                               name="ver_votes_assign")
        assign_op3 = tf.assign(ver_tri_cnt,
                               init_values,
                               name="ver_cnts_assign")
        with tf.control_dependencies([assign_op2, assign_op3]):
            ver_visible = tf.scatter_nd_add(ver_visible,
                                            tri_inds,
                                            tri_visible_per_vertex,
                                            name="ver_vote_scatter")
            ver_tri_cnt = tf.scatter_nd_add(ver_tri_cnt,
                                            tri_inds,
                                            tri_one_per_vertex,
                                            name="ver_cnt_scatter")
            ver_visible_ratio = ver_visible / (ver_tri_cnt + EPS)

            cond1 = tf.less(ver_visible_ratio, float(1.0))
            cond2 = tf.greater(ver_visible_ratio, float(0.0))
            ver_contour_mask = tf.cast(
                tf.logical_and(cond1, cond2),
                tf.float32,
                name="ver_votes_final",
            )

        return ver_normals, ver_contour_mask
Beispiel #16
0
def cross(a, b):
    return tf.cross(a, b)
Beispiel #17
0
def TriNormalsScaled(v, f):
    edge_vec1 = tf.reshape(TriEdges(v, f, 1, 0), (-1, 3))
    edge_vec2 = tf.reshape(TriEdges(v, f, 2, 0), (-1, 3))
    return tf.cross(edge_vec1, edge_vec2)
Beispiel #18
0
def tf_prior_get_normal(u, v, w):
    get_wu = lambda: tf.cross(w, u)
    get_vw = lambda: tf.cross(v, w)
    cond = tf.logical_and(tf.norm(w - v) < eps, tf.norm(w + v) < eps)
    normal = tf.cond(cond, get_wu, get_vw)
    return tf_unit_norm(normal)
Beispiel #19
0
 def test_Cross(self):
     t = tf.cross(*self.random((4, 3), (4, 3)))
     self.check(t)
Beispiel #20
0
    def step_graph_construct(self, Jinv_=None, observation_provided=False):
        # import tensorflow as tf
        self.observation_provided = observation_provided
        with tf.variable_scope('MellingerControl'):

            if not observation_provided:
                #Here we will provide all components independently
                self.xyz_tf = tf.placeholder(name='xyz',
                                             dtype=tf.float32,
                                             shape=(None, 3))
                self.Vxyz_tf = tf.placeholder(name='Vxyz',
                                              dtype=tf.float32,
                                              shape=(None, 3))
                self.Omega_tf = tf.placeholder(name='Omega',
                                               dtype=tf.float32,
                                               shape=(None, 3))
                self.R_tf = tf.placeholder(name='R',
                                           dtype=tf.float32,
                                           shape=(None, 3, 3))
            else:
                #Here we will provide observations directly and split them
                self.observation = tf.placeholder(name='obs',
                                                  dtype=tf.float32,
                                                  shape=(None, 3 + 3 + 9 + 3))
                self.xyz_tf, self.Vxyz_tf, self.R_flat, self.Omega_tf = tf.split(
                    self.observation, [3, 3, 9, 3], axis=1)
                self.R_tf = tf.reshape(self.R_flat, shape=[-1, 3, 3], name='R')

            R = self.R_tf
            # R_flat = tf.placeholder(name='R_flat', type=tf.float32, shape=(None, 9))
            # R = tf.reshape(R_flat, shape=(-1, 3, 3), name='R')

            #GOAL = [x,y,z, Vx, Vy, Vz]
            self.goal_xyz_tf = tf.placeholder(name='goal_xyz',
                                              dtype=tf.float32,
                                              shape=(None, 3))
            # goal_Vxyz = tf.placeholder(name='goal_Vxyz', type=tf.float32, shape=(None, 3))

            # Learnable gains with static initialization
            kp_p = tf.get_variable('kp_p',
                                   shape=[],
                                   initializer=tf.constant_initializer(4.5),
                                   trainable=True)  # 4.5
            kd_p = tf.get_variable('kd_p',
                                   shape=[],
                                   initializer=tf.constant_initializer(3.5),
                                   trainable=True)  # 3.5
            kp_a = tf.get_variable('kp_a',
                                   shape=[],
                                   initializer=tf.constant_initializer(200.0),
                                   trainable=True)  # 200.
            kd_a = tf.get_variable('kd_a',
                                   shape=[],
                                   initializer=tf.constant_initializer(50.0),
                                   trainable=True)  # 50.

            ## IN case you want to optimize them from random values
            # kp_p = tf.get_variable('kp_p', initializer=tf.random_uniform(shape=[1], minval=0.0, maxval=10.0), trainable=True)  # 4.5
            # kd_p = tf.get_variable('kd_p', initializer=tf.random_uniform(shape=[1], minval=0.0, maxval=10.0), trainable=True)  # 3.5
            # kp_a = tf.get_variable('kp_a', initializer=tf.random_uniform(shape=[1], minval=0.0, maxval=100.0), trainable=True)  # 200.
            # kd_a = tf.get_variable('kd_a', initializer=tf.random_uniform(shape=[1], minval=0.0, maxval=100.0), trainable=True)  # 50.

            to_goal = self.goal_xyz_tf - self.xyz_tf
            e_p = -tf.clip_by_norm(to_goal, 4.0, name='e_p')
            e_v = self.Vxyz_tf
            acc_des = -kp_p * e_p - kd_p * e_v + tf.constant([0, 0, 9.81],
                                                             name='GRAV')
            print('acc_des shape: ', acc_des.get_shape().as_list())

            def project_xy(x, name='project_xy'):
                # print('x_shape:', x.get_shape().as_list())
                # x = tf.squeeze(x, axis=2)
                return tf.multiply(x, tf.constant([1., 1., 0.]), name=name)

            # goal_dist = tf.norm(to_goal, name='goal_xyz_dist')
            xc_des = project_xy(tf.squeeze(tf.slice(R,
                                                    begin=[0, 0, 2],
                                                    size=[-1, 3, 1]),
                                           axis=2),
                                name='xc_des')
            print('xc_des shape: ', xc_des.get_shape().as_list())
            # xc_des = project_xy(R[:, 0])

            # rotation towards the ideal thrust direction
            # see Mellinger and Kumar 2011
            zb_des = tf.nn.l2_normalize(acc_des, axis=1, name='zb_dex')
            yb_des = tf.nn.l2_normalize(tf.cross(zb_des, xc_des),
                                        axis=1,
                                        name='yb_des')
            xb_des = tf.cross(yb_des, zb_des, name='xb_des')
            R_des = tf.stack([xb_des, yb_des, zb_des], axis=2, name='R_des')

            print('zb_des shape: ', zb_des.get_shape().as_list())
            print('yb_des shape: ', yb_des.get_shape().as_list())
            print('xb_des shape: ', xb_des.get_shape().as_list())
            print('R_des shape: ', R_des.get_shape().as_list())

            def transpose(x):
                return tf.transpose(x, perm=[0, 2, 1])

            # Rotational difference
            Rdiff = tf.matmul(transpose(R_des), R) - tf.matmul(
                transpose(R), R_des, name='Rdiff')
            print('Rdiff shape: ', Rdiff.get_shape().as_list())

            def tf_vee(R, name='vee'):
                return tf.squeeze(tf.stack([
                    tf.squeeze(tf.slice(R, [0, 2, 1], [-1, 1, 1]), axis=2),
                    tf.squeeze(tf.slice(R, [0, 0, 2], [-1, 1, 1]), axis=2),
                    tf.squeeze(tf.slice(R, [0, 1, 0], [-1, 1, 1]), axis=2)
                ],
                                           axis=1,
                                           name=name),
                                  axis=2)

            # def vee(R):
            #     return np.array([R[2, 1], R[0, 2], R[1, 0]])

            e_R = 0.5 * tf_vee(Rdiff, name='e_R')
            print('e_R shape: ', e_R.get_shape().as_list())
            # e_R[2] *= 0.2  # slow down yaw dynamics
            e_w = self.Omega_tf

            # Control orientation
            dw_des = -kp_a * e_R - kd_a * e_w
            print('dw_des shape: ', dw_des.get_shape().as_list())

            # we want this acceleration, but we can only accelerate in one direction!
            # thrust_mag = np.dot(acc_des, R[:, 2])
            acc_cur = tf.squeeze(tf.slice(R, begin=[0, 0, 2], size=[-1, 3, 1]),
                                 axis=2)
            print('acc_cur shape: ', acc_cur.get_shape().as_list())

            acc_dot = tf.multiply(acc_des, acc_cur)
            print('acc_dot shape: ', acc_dot.get_shape().as_list())

            thrust_mag = tf.reduce_sum(acc_dot,
                                       axis=1,
                                       keepdims=True,
                                       name='thrust_mag')
            print('thrust_mag shape: ', thrust_mag.get_shape().as_list())

            # des = np.append(thrust_mag, dw_des)
            des = tf.concat([thrust_mag, dw_des], axis=1, name='des')
            print('des shape: ', des.get_shape().as_list())

            if Jinv_ is None:
                # Learn the jacobian inverse
                Jinv = tf.get_variable('Jinv',
                                       initializer=tf.random_normal(
                                           shape=[4, 4], mean=0.0, stddev=0.1),
                                       trainable=True)
            else:
                # Jacobian inverse is provided
                Jinv = tf.constant(Jinv_.astype(np.float32), name='Jinv')
                # Jinv = tf.get_variable('Jinv', shape=[4,4], initializer=tf.constant_initializer())

            print('Jinv shape: ', Jinv.get_shape().as_list())
            ## Jacobian inverse for our quadrotor
            # Jinv = np.array([[0.0509684, 0.0043685, -0.0043685, 0.02038736],
            #                 [0.0509684, -0.0043685, -0.0043685, -0.02038736],
            #                 [0.0509684, -0.0043685,  0.0043685,  0.02038736],
            #                 [0.0509684,  0.0043685,  0.0043685, -0.02038736]])

            # thrusts = np.matmul(self.Jinv, des)
            thrusts = tf.matmul(des, tf.transpose(Jinv), name='thrust')
            thrusts = tf.clip_by_value(thrusts,
                                       clip_value_min=0.0,
                                       clip_value_max=1.0,
                                       name='thrust_clipped')
            return thrusts
from tensorflow.python.framework import ops
ops.reset_default_graph()

# Open graph session
sess = tf.Session()

# div() vs truediv() vs floordiv()
print(sess.run(tf.div(3,4)))
print(sess.run(tf.truediv(3,4)))
print(sess.run(tf.floordiv(3.0,4.0)))

# Mod function
print(sess.run(tf.mod(22.0,5.0)))

# Cross Product
print(sess.run(tf.cross([1.,0.,0.],[0.,1.,0.])))

# Trig functions
print(sess.run(tf.sin(3.1416)))
print(sess.run(tf.cos(3.1416)))
# Tangemt
print(sess.run(tf.div(tf.sin(3.1416/4.), tf.cos(3.1416/4.))))

# Custom operation
test_nums = range(15)
#from tensorflow.python.ops import math_ops
#print(sess.run(tf.equal(test_num, 3)))
def custom_polynomial(x_val):
    # Return 3x^2 - x + 10
    return(tf.sub(3 * tf.square(x_val), x_val) + 10)
Beispiel #22
0
def getRotInvPatches(x, adj):

    # The trick here is to compute a rotation from any face normal to a fixed axis (say, (0,0,1))
    # We follow the procedure described here: https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d#476311
    # Using the Levi-Civita tensor, as explained here https://math.stackexchange.com/questions/258775/from-a-vector-to-a-skew-symmetric-matrix

    batch_size, num_points, in_channels = x.get_shape().as_list()
    batch_size, num_points, K = adj.get_shape().as_list()

    # Position
    xp = tf.slice(x, [0, 0, 3], [-1, -1, -1])
    # Normal
    xn = tf.slice(x, [0, 0, 0], [-1, -1, 3])

    ref_axis_t = tf.reshape(ref_axis, [1, 1, 3])
    # [batch, N, 3]

    #ref_axes = tf.tile(ref_axes,[batch_size,num_points,1])
    #ref_axes = broadcast(ref_axes,x.shape)

    ref_axes = tf.zeros_like(xn)
    ref_axes = ref_axes + ref_axis_t

    # [batch, N, 3]
    ref_cross = tf.cross(xn, ref_axes)
    # [batch, N, 1]
    ref_sin = tf.norm(ref_cross)
    # [batch, N, 1]
    ref_cos = tensorDotProduct(ref_axis, xn)

    # [batch, N, 3, 1]
    ref_cross = tf.expand_dims(ref_cross, -1)
    # [batch, N, 3, 3, 1]
    ref_cross = tf.tile(tf.expand_dims(ref_cross, 2), [1, 1, 3, 1, 1])
    # [1, 1, 3, 3, 3]
    LC = tf.reshape(LC_tensor, [1, 1, 3, 3, 3])
    # [batch, N, 3, 3, 1]
    temp_zero = tf.zeros_like(ref_cross)
    # [batch, N, 3, 3, 3]
    temp_zero = tf.tile(temp_zero, [1, 1, 1, 1, 3])

    # [batch, N, 3, 3, 3]
    LC = LC + temp_zero
    #LC = tf.tile(LC,[batch_size,num_points,1,1,1])

    # [batch, N, 3, 3, 1]
    ssm = tf.matmul(LC, ref_cross)
    # [batch, N, 3, 3]
    ssm = tf.squeeze(ssm)

    # [batch, N, 1]
    rot_coef = tf.divide(tf.subtract(1.0, ref_cos),
                         tf.multiply(ref_sin, ref_sin))
    # [batch, N, 3, 3]
    rot_coef = tf.tile(tf.reshape(rot_coef, [batch_size, -1, 1, 1]),
                       [1, 1, 3, 3])
    # [1, 1, 3, 3]
    Idmat = tf.reshape(Id_tensor, [1, 1, 3, 3])
    # [batch, N, 3, 3]
    Idmat = Idmat + tf.zeros_like(rot_coef)
    #Idmat = tf.tile(Idmat,[batch_size,num_points,1,1])

    # [batch, N, 3, 3]
    rot = Idmat + ssm + tf.multiply(tf.matmul(ssm, ssm), rot_coef)
    # [batch, N, K, 3, 3]
    rot = tf.tile(tf.expand_dims(rot, axis=2), [1, 1, K, 1, 1])
    # rot gives a (3,3) rotation matrix for every face

    # [batch, N, K, ch]
    patches = get_patches(x, adj)

    # Normals part of patches
    npatches = tf.slice(patches, [0, 0, 0, 0], [-1, -1, -1, 3])

    # [batch, N, K, ch, 1]
    npatches = tf.expand_dims(npatches, -1)
    # [batch, N, K, ch, 1]
    npatches = tf.matmul(rot, npatches)
    # [batch, N, K, ch]
    npatches = tf.reshape(npatches, [batch_size, -1, K, 3])

    # Position part of patches
    # [batch, N, K, 3]
    ppatches = tf.slice(patches, [0, 0, 0, 3], [-1, -1, -1, -1])

    # Compute displacement to current face
    ppatches = tf.subtract(ppatches, tf.expand_dims(xp, axis=2))

    # Rotate, just like the normals
    ppatches = tf.expand_dims(ppatches, -1)
    ppatches = tf.matmul(rot, ppatches)
    ppatches = tf.reshape(ppatches, [batch_size, -1, K, 3])

    # [batch, N, K, ch (6)]
    patches = tf.concat([npatches, ppatches], axis=-1)

    return patches