def loss(y_pred, y_true,
metric=SO3.bi_invariant_metric,
representation='vector'):
if representation == 'quaternion':
y_pred = SO3.rotation_vector_from_quaternion(y_pred)
y_true = SO3.rotation_vector_from_quaternion(y_true)
loss = lie_group.loss(y_pred, y_true, SO3, metric)
return loss
def forward(ctx, input, label):
# input_np = input.data.numpy()
# label_np = label.data.numpy()
# ctx.save_for_backward(input_np, label_np)
# loss = lie_group.grad(input_np, label_np, SE3_GROUP, metric)
# return loss
input, label = input.detach(), label.detach()
ctx.save_for_backward(input, label)
loss = lie_group.loss(input, label, SE3_GROUP, metric)
return torch.tensor(np.mean(loss))
def define_loss(self):
with tf.name_scope('Loss'):
print("Loss ===============================================Start")
with tf.variable_scope('Resize_for_VAE'):
y_hat_dm_resize = tf.image.resize_bilinear(
self.y_hat_dm[:, :, :, 3:5],
size=(self.hyper_params.vae_h, self.hyper_params.vae_w),
align_corners=False,
half_pixel_centers=False,
name='y_hat_dm_resize')
y_cam_resize = tf.image.resize_bilinear(
self.x_cam_train,
size=(self.hyper_params.vae_h, self.hyper_params.vae_w),
align_corners=False,
half_pixel_centers=False,
name='y_cam_resize')
vae_inputs = tf.concat([y_cam_resize, y_hat_dm_resize],
-1,
name='vae_input')
print(" [vae inputs] {}".format(vae_inputs))
self.tolerance_regularization, _ = tolerance_regularizer(
inputs=vae_inputs,
vae_latent_dim=self.hyper_params.vae_latent_dim,
is_training=self.is_training)
self.frozen_vars = tf.get_collection(
tf.GraphKeys.TRAINABLE_VARIABLES, 'Tolerance_Regularization')
with tf.variable_scope('SE3_Loss'):
print(
" [SE3 Loss] ##################################"
)
# TODO: Hard coded batch number!!! BAD!!!
se3_pred = tf.reshape(self.y_hat_se3param,
(self.batch_size, 6))
se3_true = tf.reshape(self.y_se3param, (self.batch_size, 6))
print(" [y_hat_se3param] {}".format(
self.y_hat_se3param))
print(" [y_se3param ] {}".format(
self.y_se3param))
print(" [se3_pred ] {}".format(se3_pred))
print(" [se3_true ] {}".format(se3_true))
# Consider implementing a native one instead of using 3rd party lib
SE3_GROUP = SpecialEuclideanGroup(3,
epsilon=np.finfo(
np.float32).eps)
metric = SE3_GROUP.left_canonical_metric
self.loss_geodesic = tf.reduce_mean(
lie_group.loss(se3_pred, se3_true, SE3_GROUP, metric))
with tf.name_scope('Loss'):
self.loss = tf.identity(self.loss_geodesic +
self.hyper_params.regularizer_factor *
self.tolerance_regularization,
name='loss')
def test_se3():
# rpy <--> xyz convention
# http://web.mit.edu/2.05/www/Handout/HO2.PDF
# examples:
# [r, p, y, x, y, z]
SE3_GROUP = SpecialEuclideanGroup(3, epsilon=np.finfo(np.float32).eps)
metric = SE3_GROUP.left_canonical_metric
identity = tf.constant(np.array([0., 0, 0, 0, 0, 0], dtype=np.float32), name='identity')
r90 = tf.constant(np.array([1.5707963, 0., 0, 0, 0, 0], dtype=np.float32), name='r90')
x10 =tf.constant(np.array([0., 0, 0, 10, 0, 0], dtype=np.float32), name='x10')
r90_x10 = tf.constant(np.array([1.5707963, 0, 0, 10, 0, 0], dtype=np.float32), name='r90_x10')
test_se3s = tf.constant(np.array([[1.5707963, 0, 0, 0, 0, 0], [0, 0, 0, 10, 0, 0], [1.5707963, 0, 0, 10, 0, 0]], dtype=np.float32), name='test_se3s')
test_identities = tf.constant(np.array([[0., 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0]], dtype=np.float32), name='test_se3s')
dist_r90_to_identity = lie_group.loss(r90, identity, SE3_GROUP, metric)
dist_x10_to_identity = lie_group.loss(x10, identity, SE3_GROUP, metric)
dist_x10_to_r90 = lie_group.loss(x10, r90, SE3_GROUP, metric)
psudo_riemannian_log_dist_x10_to_r90 = dist_r90_to_identity + dist_x10_to_identity # Decoupled
dist_r90_to_x10 = lie_group.loss(r90, x10, SE3_GROUP, metric)
dist_r90_x10_to_identity = lie_group.loss(r90_x10, identity, SE3_GROUP, metric)
dist_batch = lie_group.loss(test_se3s, test_identities, SE3_GROUP, metric)
mean_dist_batch = tf.reduce_mean(dist_batch)
test_dict = {
'dist_r90_to_identity': dist_r90_to_identity,
'dist_x10_to_identity': dist_x10_to_identity,
'dist_x10_to_r90': dist_x10_to_r90,
'psudo_riemannian_log_dist_x10_to_r90': psudo_riemannian_log_dist_x10_to_r90,
'dist_r90_to_x10': dist_r90_to_x10,
'dist_r90_x10_to_identity': dist_r90_x10_to_identity,
'dist_batch': dist_batch,
'mean_dist_batch': mean_dist_batch
}
_compare_dict = get_compares_dict()
for _k, _v in _compare_dict.items():
test_dict['dist_{}_to_identity'.format(_k)] = lie_group.loss(tf.constant(_v['se3'], name=_k), identity, SE3_GROUP, metric)
with tf.Session() as sess:
for _k, _v in test_dict.items():
print("{} : {}".format(_k, sess.run(_v)))
"""
def loss(y_pred, y_true,
metric=SE3.left_canonical_metric,
representation='vector'):
"""
Loss function given by a riemannian metric on a Lie group,
by default the left-invariant canonical metric.
"""
if gs.ndim(y_pred) == 1:
y_pred = gs.expand_dims(y_pred, axis=0)
if gs.ndim(y_true) == 1:
y_true = gs.expand_dims(y_true, axis=0)
if representation == 'quaternion':
y_pred_rot_vec = SO3.rotation_vector_from_quaternion(y_pred[:, :4])
y_pred = gs.hstack([y_pred_rot_vec, y_pred[:, 4:]])
y_true_rot_vec = SO3.rotation_vector_from_quaternion(y_true[:, :4])
y_true = gs.hstack([y_true_rot_vec, y_true[:, 4:]])
loss = lie_group.loss(y_pred, y_true, SE3, metric)
return loss
def main(args):
SE3_GROUP = SpecialEuclideanGroup(3)
metric = SE3_GROUP.left_canonical_metric
reader_train = PoseNetReader([FLAGS.dataset])
# Get Input Tensors
image, y_true = reader_train.read_and_decode()
# Construct model and encapsulating all ops into scopes, making
# Tensorboard's Graph visualization more convenient
print('Making Model')
with tf.name_scope('Model'):
py_x, _ = inception.inception_v1(tf.cast(image, tf.float32),
num_classes=6,
is_training=False)
# tanh(pred_angle) required to prevent infinite spins on rotation axis
y_pred = tf.concat((tf.nn.tanh(py_x[:, :3]), py_x[:, 3:]), axis=1)
loss = tf.reduce_mean(lie_group.loss(y_pred, y_true, SE3_GROUP,
metric))
print('Initializing Variables...')
init_op = tf.group(tf.global_variables_initializer(),
tf.local_variables_initializer())
# Main Testing Routine
with tf.Session() as sess:
# Run the initializer
sess.run(init_op)
# Start Queue Threads
coord = tf.train.Coordinator()
threads = tf.train.start_queue_runners(coord=coord)
# Load saved weights
print('Loading Trained Weights')
saver = tf.train.Saver()
latest_checkpoint = tf.train.latest_checkpoint(FLAGS.model_dir)
saver.restore(sess, latest_checkpoint)
i = 0
# Inference cycle
try:
while True:
_y_pred, _y_true, _loss = sess.run([y_pred, y_true, loss])
print('Iteration:', i, 'loss:', _loss)
print('_y_pred:', _y_pred)
print('_y_true:', _y_true)
print('\n')
i = i + 1
except tf.errors.OutOfRangeError:
print('End of Testing Data')
except KeyboardInterrupt:
print('KeyboardInterrupt!')
finally:
print('Stopping Threads')
coord.request_stop()
coord.join(threads)
def main(args):
SE3_GROUP = SpecialEuclideanGroup(3, epsilon=FLAGS.epsilon)
metric = SE3_GROUP.left_canonical_metric
reader_train = PoseNetReader([FLAGS.dataset])
# Get Input Tensors
image, y_true = reader_train.read_and_decode(FLAGS.batch_size)
# Construct model and encapsulating all ops into scopes, making
# Tensorboard's Graph visualization more convenient
print('Making Model')
with tf.name_scope('Model'):
py_x, _ = inception.inception_v1(tf.cast(image, tf.float32), num_classes=6)
# tanh(pred_angle) required to prevent infinite spins on rotation axis
y_pred = tf.concat((tf.nn.tanh(py_x[:, :3]), py_x[:, 3:]), axis=1)
loss = tf.reduce_mean(lie_group.loss(y_pred, y_true, SE3_GROUP, metric))
print('Making Optimizer')
with tf.name_scope('Adam'):
# Adam Optimizer
train_op = tf.train.AdamOptimizer(FLAGS.init_lr).minimize(loss)
# Initialize the variables (i.e. assign their default value)
print('Initializing Variables...')
init_op = tf.group(tf.global_variables_initializer(),
tf.local_variables_initializer())
# Create a summary to monitor cost tensor
tf.summary.scalar('loss', loss)
# Merge all summaries into a single op
merged_summary_op = tf.summary.merge_all()
# Main Training Routine
with tf.Session() as sess:
# Run the initializer
sess.run(init_op)
# Start Queue Threads
coord = tf.train.Coordinator()
threads = tf.train.start_queue_runners(coord=coord)
# op to write logs to Tensorboard
summary_writer = tf.summary.FileWriter(FLAGS.logs_path, graph=tf.get_default_graph())
saver = tf.train.Saver()
if FLAGS.resume:
print('Resuming training.')
latest_checkpoint = tf.train.latest_checkpoint(FLAGS.model_dir)
saver.restore(sess, latest_checkpoint)
# Training cycle
try:
train_range = tqdm(range(FLAGS.max_iter))
for i in train_range:
_, _cost, summary = sess.run([train_op, loss, merged_summary_op])
# Write logs at every iteration
train_range.set_description('Training: (loss=%g)' % _cost)
summary_writer.add_summary(summary, i)
if i % FLAGS.snapshot == 0:
save_path = saver.save(sess, '{}/chkpt{}.ckpt'.format(FLAGS.model_dir, i))
except KeyboardInterrupt:
print('KeyboardInterrupt!')
finally:
print('Stopping Threads')
coord.request_stop()
coord.join(threads)
print('Saving iter: ', i)
save_path = saver.save(sess, FLAGS.model_dir + str(i) + '.ckpt')
def cal_metrics(self, se3_pred, se3_true, group, metric):
se3_error = lie_group.loss(se3_pred, se3_true, group, metric)
return se3_error
def cal_se3_error(se3_pred, se3_true):
se3_error = lie_group.loss(se3_pred, se3_true, SE3_GROUP, metric)
return se3_error