def setUp(self):
warnings.simplefilter('ignore', category=ImportWarning)
gs.random.seed(1234)
n = 3
group = SpecialEuclideanGroup(n=n)
# Diagonal left and right invariant metrics
diag_mat_at_identity = gs.eye(group.dimension)
left_diag_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=diag_mat_at_identity,
left_or_right='left')
right_diag_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=diag_mat_at_identity,
left_or_right='right')
# General left and right invariant metrics
# TODO(nina): Replace the matrix below by a general SPD matrix.
sym_mat_at_identity = gs.eye(group.dimension)
left_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=sym_mat_at_identity,
left_or_right='left')
right_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=sym_mat_at_identity,
left_or_right='right')
metrics = {
'left_diag': left_diag_metric,
'right_diag_metric': right_diag_metric,
'left': left_metric,
'right': right_metric
}
# General case for the point
point_1 = gs.array([[-0.2, 0.9, 0.5, 5., 5., 5.]])
point_2 = gs.array([[0., 2., -0.1, 30., 400., 2.]])
# Edge case for the point, angle < epsilon,
point_small = gs.array([[-1e-7, 0., -7 * 1e-8, 6., 5., 9.]])
self.group = group
self.metrics = metrics
self.left_diag_metric = left_diag_metric
self.right_diag_metric = right_diag_metric
self.left_metric = left_metric
self.right_metric = right_metric
self.point_1 = point_1
self.point_2 = point_2
self.point_small = point_small
def setUp(self):
gs.random.seed(1234)
n = 3
group = SpecialEuclideanGroup(n=n)
# Diagonal left and right invariant metrics
diag_mat_at_identity = gs.zeros([group.dimension, group.dimension])
diag_mat_at_identity[0:3, 0:3] = 1 * gs.eye(3)
diag_mat_at_identity[3:6, 3:6] = 1 * gs.eye(3)
left_diag_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=diag_mat_at_identity,
left_or_right='left')
right_diag_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=diag_mat_at_identity,
left_or_right='right')
# General left and right invariant metrics
# TODO(xxx): replace by general SPD matrix
sym_mat_at_identity = gs.eye(group.dimension)
left_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=sym_mat_at_identity,
left_or_right='left')
right_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=sym_mat_at_identity,
left_or_right='right')
metrics = {
'left_diag': left_diag_metric,
'right_diag_metric': right_diag_metric,
'left': left_metric,
'right': right_metric
}
# General case for the point
point_1 = gs.array([-0.2, 0.9, 0.5, 5., 5., 5.])
point_2 = gs.array([0., 2., -0.1, 30., 400., 2.])
# Edge case for the point, angle < epsilon,
point_small = gs.array([-1e-7, 0., -7 * 1e-8, 6., 5., 9.])
self.group = group
self.metrics = metrics
self.left_diag_metric = left_diag_metric
self.right_diag_metric = right_diag_metric
self.left_metric = left_metric
self.right_metric = right_metric
self.point_1 = point_1
self.point_2 = point_2
self.point_small = point_small
class SE3GeodesicLoss(Function):
"""
Geodesic Loss on the Special Euclidean Group SE(3), of 3D rotations
and translations, computed as the square geodesic distance with respect
to a left-invariant Riemannian metric.
"""
def __init__(self, weight):
assert weight.shape != SE3_DIM, 'Weight vector must be of shape [6]'
self.weight = weight
self.SE3_GROUP = SpecialEuclideanGroup(N)
self.metric = InvariantMetric(
group=self.SE3_GROUP,
inner_product_mat_at_identity=np.eye(SE3_DIM) * self.weight,
left_or_right='left')
def forward(self, inputs, targets):
"""
PyTorch Custom Forward Function
:param inputs: Custom Function
:param targets: Function Inputs
:return:
"""
self.y_pred = inputs.numpy()
self.y_true = targets.numpy()
sq_geodesic_dist = self.metric.squared_dist(self.y_pred, self.y_true)
batch_loss = np.sum(sq_geodesic_dist)
return torch.FloatTensor([batch_loss])
def backward(self, grad_output):
"""
PyTorch Custom Backward Function
:param grad_output: Gradients for equation prime
:return: gradient w.r.t. input
"""
tangent_vec = self.metric.log(
base_point=self.y_pred,
point=self.y_true)
grad_point = - 2. * tangent_vec
inner_prod_mat = self.metric.inner_product_matrix(
base_point=self.y_pred)
riemannian_grad = np.einsum('ijk,ik->ij', inner_prod_mat, grad_point)
sqrt_weight = np.sqrt(self.weight)
riemannian_grad = np.multiply(riemannian_grad, sqrt_weight)
return grad_output * torch.FloatTensor(riemannian_grad), None
def __init__(self, weight):
assert weight.shape != SE3_DIM, 'Weight vector must be of shape [6]'
self.weight = weight
self.SE3_GROUP = SpecialEuclideanGroup(N)
self.metric = InvariantMetric(
group=self.SE3_GROUP,
inner_product_mat_at_identity=np.eye(SE3_DIM) * self.weight,
left_or_right='left')
def setUp(self):
gs.random.seed(1234)
n = 3
group = SpecialOrthogonalGroup(n=n)
# Diagonal left and right invariant metrics
diag_mat_at_identity = gs.eye(group.dimension)
left_diag_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=diag_mat_at_identity,
left_or_right='left')
right_diag_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=diag_mat_at_identity,
left_or_right='right')
# General left and right invariant metrics
# TODO(nina): replace by general SPD matrix
sym_mat_at_identity = gs.eye(group.dimension)
left_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=sym_mat_at_identity,
left_or_right='left')
right_metric = InvariantMetric(
group=group,
inner_product_mat_at_identity=sym_mat_at_identity,
left_or_right='right')
metrics = {
'left_diag': left_diag_metric,
'right_diag_metric': right_diag_metric,
'left': left_metric,
'right': right_metric
}
# General case for the point
point_1 = tf.convert_to_tensor([-0.2, 0.9, 0.5])
point_2 = tf.convert_to_tensor([0., 2., -0.1])
# Edge case for the point, angle < epsilon,
point_small = tf.convert_to_tensor([[-1e-7, 0., -7 * 1e-8]])
self.group = group
self.metrics = metrics
self.left_diag_metric = left_diag_metric
self.right_diag_metric = right_diag_metric
self.left_metric = left_metric
self.right_metric = right_metric
self.point_1 = point_1
self.point_2 = point_2
self.point_small = point_small
def __init__(self, dimension):
assert dimension > 0
Manifold.__init__(self, dimension)
self.left_canonical_metric = InvariantMetric(
group=self,
inner_product_mat_at_identity=gs.eye(self.dimension),
left_or_right='left')
self.right_canonical_metric = InvariantMetric(
group=self,
inner_product_mat_at_identity=gs.eye(self.dimension),
left_or_right='right')
self.metrics = []
def __init__(self, dimension, identity):
super(LieGroup, self).__init__(dimension)
self.identity = identity
self.left_canonical_metric = InvariantMetric(
group=self,
inner_product_mat_at_identity=np.eye(self.dimension),
left_or_right='left')
self.right_canonical_metric = InvariantMetric(
group=self,
inner_product_mat_at_identity=np.eye(self.dimension),
left_or_right='right')
self.metrics = []
def regularize_tangent_vec(self, tangent_vec, base_point, metric=None):
if metric is None:
metric = self.left_canonical_metric
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
base_point = gs.to_ndarray(base_point, to_ndim=2)
rotations = self.rotations
dim_rotations = rotations.dimension
rot_tangent_vec = tangent_vec[:, :dim_rotations]
rot_base_point = base_point[:, :dim_rotations]
metric_mat = metric.inner_product_mat_at_identity
rot_metric_mat = metric_mat[:, :dim_rotations, :dim_rotations]
rot_metric = InvariantMetric(
group=rotations,
inner_product_mat_at_identity=rot_metric_mat,
left_or_right=metric.left_or_right)
regularized_vec = gs.zeros_like(tangent_vec)
regularized_vec[:, :dim_rotations] = rotations.regularize_tangent_vec(
tangent_vec=rot_tangent_vec,
base_point=rot_base_point,
metric=rot_metric)
regularized_vec[:, dim_rotations:] = tangent_vec[:, dim_rotations:]
return regularized_vec
def regularize_tangent_vec(self,
tangent_vec,
base_point,
metric=None,
point_type=None):
if point_type is None:
point_type = self.default_point_type
if metric is None:
metric = self.left_canonical_metric
if point_type == 'vector':
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
base_point = gs.to_ndarray(base_point, to_ndim=2)
rotations = self.rotations
dim_rotations = rotations.dimension
rot_tangent_vec = tangent_vec[:, :dim_rotations]
rot_base_point = base_point[:, :dim_rotations]
metric_mat = metric.inner_product_mat_at_identity
rot_metric_mat = metric_mat[:, :dim_rotations, :dim_rotations]
rot_metric = InvariantMetric(
group=rotations,
inner_product_mat_at_identity=rot_metric_mat,
left_or_right=metric.left_or_right)
regularized_vec = gs.zeros_like(tangent_vec)
rotations_vec = rotations.regularize_tangent_vec(
tangent_vec=rot_tangent_vec,
base_point=rot_base_point,
metric=rot_metric,
point_type=point_type)
regularized_vec = gs.concatenate(
[rotations_vec, tangent_vec[:, dim_rotations:]], axis=1)
elif point_type == 'matrix':
# TODO(nina): regularization in terms
# of skew-symmetric matrices?
regularized_vec = tangent_vec
return regularized_vec
def regularize_tangent_vec(self, tangent_vec, base_point, metric=None):
"""
Regularize an element of the group SE(3),
by extracting the rotation vector r from the input [r t]
and using self.rotations.regularize.
:param point: 6d vector, element in SE(3) represented as [r t].
:returns self.regularized_point: 6d vector, element in SE(3)
with self.regularized rotation.
"""
if metric is None:
metric = self.left_canonical_metric
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
base_point = gs.to_ndarray(base_point, to_ndim=2)
rotations = self.rotations
dim_rotations = rotations.dimension
rot_tangent_vec = tangent_vec[:, :dim_rotations]
rot_base_point = base_point[:, :dim_rotations]
metric_mat = metric.inner_product_mat_at_identity
rot_metric_mat = metric_mat[:, :dim_rotations, :dim_rotations]
rot_metric = InvariantMetric(
group=rotations,
inner_product_mat_at_identity=rot_metric_mat,
left_or_right=metric.left_or_right)
regularized_vec = gs.zeros_like(tangent_vec)
regularized_vec[:, :dim_rotations] = rotations.regularize_tangent_vec(
tangent_vec=rot_tangent_vec,
base_point=rot_base_point,
metric=rot_metric)
regularized_vec[:, dim_rotations:] = tangent_vec[:, dim_rotations:]
return regularized_vec
class SE3GeodesicLoss(object):
"""
Geodesic Loss on the Special Euclidean Group SE(3), of 3D rotations
and translations, computed as the square geodesic distance with respect
to a left-invariant Riemannian metric.
"""
def __init__(self, weight, op_name='SE3GeodesicLoss'):
assert weight.shape != SE3_DIM, 'Weight vector must be of shape [6]'
self.op_name = op_name
self.weight = weight
self.SE3_GROUP = SpecialEuclideanGroup(N)
self.metric = InvariantMetric(
group=self.SE3_GROUP,
inner_product_mat_at_identity=np.eye(SE3_DIM) * self.weight,
left_or_right='left')
# Python Custom Op Tensorflow Wrapper
def py_func(self, func, inp, Tout, stateful=True, name=None, grad=None):
"""
PyFunc defined as given by Tensorflow
:param func: Custom Function
:param inp: Function Inputs
:param Tout: Ouput Type of out Custom Function
:param stateful: Calculate Gradients when stateful is True
:param name: Name of the PyFunction
:param grad: Custom Gradient Function
:return:
"""
# Generate Random Gradient name to avoid conflicts with inbuilt names
rnd_name = 'PyFuncGrad' + str(np.random.randint(0, 2**32 - 1))
# Register Tensorflow Gradient
tf.RegisterGradient(rnd_name)(grad)
# Get current graph
g = tf.get_default_graph()
# Add gradient override map
with g.gradient_override_map({
'PyFunc': rnd_name,
'PyFuncStateless': rnd_name
}):
return tf.py_func(func, inp, Tout, stateful=stateful, name=name)
# Python Custom Op
def geodesic_loss(self, y_pred, y_true, name=None):
"""
Custom Function which defines pyfunc and gradient override
:param x: y_pred - predicted se(3) pose
:param y: y_true - ground truth se(3) pose
:param name: Function name
:return: dist - geodesic distance between y_pred and y_true
"""
with ops.name_scope(name, self.op_name, [y_pred, y_true]) as name:
"""
Our pyfunc op accepts 2 input parameters and returns 1 outputs
Input Parameters: y_pred, y_true
Output Parameters: geodesic distance
"""
dist, grad = self.py_func(self.riemannian_dist_grad,
[y_pred, y_true],
[tf.float32, tf.float32],
name=name,
grad=self.riemannian_grad_op)
return dist
# Geodesic Loss Core Function
def riemannian_dist_grad(self, y_pred, y_true):
"""
Geodesic Loss Core Function
:param y_pred: y_pred
:param y_true: y_true
:return: dist, grad
"""
# Geodesic Distance
sq_geodesic_dist = self.metric.squared_dist(y_pred, y_true)
batch_loss = np.sum(sq_geodesic_dist).astype('float32')
# Computation of Riemannian Gradient
tangent_vec = self.metric.log(base_point=y_pred, point=y_true)
grad_point = -2. * tangent_vec
inner_prod_mat = self.metric.inner_product_matrix(base_point=y_pred)
riemannian_grad = np.einsum('ijk,ik->ij', inner_prod_mat, grad_point)
sqrt_weight = np.sqrt(self.weight)
riemannian_grad = np.multiply(riemannian_grad,
sqrt_weight).astype('float32')
return batch_loss, riemannian_grad
# Geodesic Loss Gradient Function
def riemannian_grad_op(self, op, grads, grad_glob):
"""
Geodesic Loss Gradient Function
:param op: Operation - operation.inputs = [y_pred, y_true],
operation.outputs = [dist, grad]
:param grads: Gradients for equation prime
:param grad_glob: No real use of it, but the gradient function
parameter size should match op.inputs
:return: grads * d/d_y_pred , vector of ones
"""
# Only gradient w.r.t. y_pred is returned.
return grads * op.outputs[1], tf.ones_like(op.outputs[1])
def main(argv):
# TF Record
dataset = tf.data.TFRecordDataset(FLAGS.data_dir +
'/dataset_test.tfrecords')
dataset = dataset.map(_parse_function_kingscollege)
# dataset = dataset.repeat()
# dataset = dataset.shuffle(FLAGS.queue_buffer)
dataset = dataset.batch(1)
image, vec, pose_q, pose_x = dataset.make_one_shot_iterator().get_next()
# Network Definition
image_resized = tf.image.resize_images(image, size=[224, 224])
if FLAGS.loss == 'PoseNet':
y_pred, _ = inception.inception_v3(image_resized,
num_classes=7,
is_training=False)
quaternion_pred, translation_pred = tf.split(y_pred, [4, 3], axis=1)
sess = tf.Session()
ckpt_file = tf.train.latest_checkpoint(FLAGS.model_dir)
tf.train.Saver().restore(sess, ckpt_file)
print('restoring parameters from', ckpt_file)
i = 0
results = []
try:
while True:
_image, _quaternion_true, _translation_true, _quaternion_pred, _translation_pred = \
sess.run([image, pose_q, pose_x, quaternion_pred, translation_pred])
# Compute Individual Sample Error
q1 = _quaternion_true / np.linalg.norm(_quaternion_true)
q2 = _quaternion_pred / np.linalg.norm(_quaternion_pred)
d = abs(np.sum(np.multiply(q1, q2)))
theta = 2. * np.arccos(d) * 180. / np.pi
error_x = np.linalg.norm(_translation_true - _translation_pred)
results.append([error_x, theta])
print('Iteration:', i, 'Error XYZ (m):', error_x,
'Error Q (degrees):', theta)
i = i + 1
except tf.errors.OutOfRangeError:
print('End of Test Data')
results = np.stack(results)
results = np.median(results, axis=0)
print('Error XYZ (m):', results[0], 'Error Q (degrees):', results[1])
elif FLAGS.loss == 'SE3':
y_pred, _ = inception.inception_v3(image_resized,
num_classes=6,
is_training=False)
sess = tf.Session()
ckpt_file = tf.train.latest_checkpoint(FLAGS.model_dir)
tf.train.Saver().restore(sess, ckpt_file)
print('restoring parameters from', ckpt_file)
SO3_GROUP = SpecialOrthogonalGroup(3)
SE3_GROUP = SpecialEuclideanGroup(3)
metric = InvariantMetric(group=SE3_GROUP,
inner_product_mat_at_identity=np.eye(6),
left_or_right='left')
i = 0
results = []
_y_pred_i = []
_y_true_i = []
_se3_err_i = []
try:
while True:
_image, _rvec, _qvec, _tvec, _y_pred = \
sess.run([image, vec, pose_q, pose_x, y_pred])
_quaternion_true = _qvec
_quaternion_pred = SO3_GROUP.quaternion_from_rotation_vector(
_y_pred[0, :3])[0]
# Compute Individual Sample Error
q1 = _quaternion_true / np.linalg.norm(_quaternion_true)
q2 = _quaternion_pred / np.linalg.norm(_quaternion_pred)
d = abs(np.sum(np.multiply(q1, q2)))
theta = 2. * np.arccos(d) * 180. / np.pi
error_x = np.linalg.norm(_tvec - _y_pred[0, 3:])
results.append([error_x, theta])
# SE3 compute
_y_true = np.concatenate((_rvec, _tvec), axis=-1)
se3_dist = metric.squared_dist(_y_pred, _y_true)[0]
_y_pred_i.append(_y_pred)
_y_true_i.append(_y_true)
_se3_err_i.append(
SE3_GROUP.compose(SE3_GROUP.inverse(_y_true), _y_pred))
print('Iteration:', i, 'Error XYZ (m):', error_x,
'Error Q (degrees):', theta, 'SE3 dist:', se3_dist)
i = i + 1
except tf.errors.OutOfRangeError:
print('End of Test Data')
# Calculate SE3 Error Weights
err_vec = np.vstack(_se3_err_i)
err_weights = np.diag(np.linalg.inv(np.cov(err_vec.T)))
err_weights = err_weights / np.linalg.norm(err_weights)
print(err_weights)
results = np.stack(results)
results = np.median(results, axis=0)
print('Error XYZ (m):', results[0], 'Error Q (degrees):', results[1])
else:
print('Invalid Option:', FLAGS.loss)
raise SystemExit
