mirror of https://github.com/tensorflow/models.git
239 lines
10 KiB
Python
239 lines
10 KiB
Python
# Copyright 2017 The TensorFlow Authors All Rights Reserved.
|
|
#
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
# you may not use this file except in compliance with the License.
|
|
# You may obtain a copy of the License at
|
|
#
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
#
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
# See the License for the specific language governing permissions and
|
|
# limitations under the License.
|
|
# ==============================================================================
|
|
|
|
"""Tests for icp op."""
|
|
|
|
from __future__ import absolute_import
|
|
from __future__ import division
|
|
from __future__ import print_function
|
|
|
|
import math
|
|
import os
|
|
|
|
from absl import flags
|
|
from absl import logging
|
|
from icp_op import icp
|
|
import icp_util
|
|
import numpy as np
|
|
import tensorflow as tf
|
|
|
|
FLAGS = flags.FLAGS
|
|
|
|
PRINT_CAP = 6
|
|
|
|
|
|
class IcpOpTestBase(tf.test.TestCase):
|
|
"""Classed used by IcpOpTest, IcpOpGradTest."""
|
|
|
|
def setUp(self):
|
|
self.small_cloud = tf.constant([[[0.352222, -0.151883, -0.106395],
|
|
[-0.397406, -0.473106, 0.292602],
|
|
[-0.731898, 0.667105, 0.441304],
|
|
[-0.734766, 0.854581, -0.0361733],
|
|
[-0.4607, -0.277468, -0.916762]]],
|
|
dtype=tf.float32)
|
|
self.random_cloud = self._generate_random_cloud()
|
|
self.organized_cloud = self._generate_organized_cloud()
|
|
self.lidar_cloud = self._load_lidar_cloud()
|
|
self.identity_transform = tf.constant([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],
|
|
dtype=tf.float32)
|
|
self.index_translation = 0
|
|
self.index_rotation = 3
|
|
|
|
def _run_icp(self, cloud_source, ego_motion, cloud_target):
|
|
transform, residual = icp(cloud_source, ego_motion, cloud_target)
|
|
logging.info('Running ICP:')
|
|
logging.info('ego_motion: %s\n%s', ego_motion, ego_motion.eval())
|
|
logging.info('transform: %s\n%s', transform, transform.eval())
|
|
logging.info('residual: %s\n%s', residual,
|
|
residual[0, :PRINT_CAP, :].eval())
|
|
return transform, residual
|
|
|
|
def _generate_random_cloud(self):
|
|
self.random_cloud_size = 50
|
|
tf.set_random_seed(11)
|
|
return tf.truncated_normal(
|
|
[1, self.random_cloud_size, 3], mean=0.0, stddev=1.0, dtype=tf.float32)
|
|
|
|
def _generate_organized_cloud(self):
|
|
res = 10
|
|
scale = 7
|
|
# [B, 10, 10, 3]
|
|
cloud = np.zeros(shape=(1, res, res, 3))
|
|
for i in range(res):
|
|
for j in range(res):
|
|
# For scale == 1.0, x and y range from -0.5 to 0.4.
|
|
y = scale / 2 - scale * (res - i) / res
|
|
x = scale / 2 - scale * (res - j) / res
|
|
z = math.sin(x * x + y * y)
|
|
cloud[0, i, j, :] = (x, y, z)
|
|
return tf.constant(cloud, dtype=tf.float32)
|
|
|
|
def _load_lidar_cloud(self):
|
|
lidar_cloud_path = os.path.join(FLAGS.test_srcdir,
|
|
icp_util.LIDAR_CLOUD_PATH)
|
|
lidar_cloud = np.load(lidar_cloud_path)
|
|
lidar_cloud = tf.expand_dims(lidar_cloud, axis=0) # Add batch.
|
|
logging.info('lidar_cloud.shape: %s', lidar_cloud.shape)
|
|
return lidar_cloud
|
|
|
|
|
|
class IcpOpTest(IcpOpTestBase):
|
|
|
|
def test_translate_small_cloud(self):
|
|
with self.test_session():
|
|
tx = 0.1
|
|
cloud_source = self.small_cloud
|
|
cloud_target = cloud_source + [tx, 0, 0]
|
|
transform, residual = self._run_icp(cloud_source, self.identity_transform,
|
|
cloud_target)
|
|
self.assertAlmostEqual(transform.eval()[0, self.index_translation], tx,
|
|
places=6)
|
|
self.assertAllClose(residual.eval(), tf.zeros_like(residual).eval(),
|
|
atol=1e-4)
|
|
|
|
def test_translate_random_cloud(self):
|
|
with self.test_session():
|
|
tx = 0.1
|
|
cloud_source = self.random_cloud
|
|
cloud_target = cloud_source + [tx, 0, 0]
|
|
transform, residual = self._run_icp(cloud_source, self.identity_transform,
|
|
cloud_target)
|
|
self.assertAlmostEqual(transform.eval()[0, self.index_translation], tx,
|
|
places=4)
|
|
self.assertAllClose(residual.eval(), tf.zeros_like(residual).eval(),
|
|
atol=1e-4)
|
|
|
|
def test_rotate_random_cloud(self):
|
|
with self.test_session():
|
|
ego_motion = tf.constant([[0.0, 0.0, 0.0,
|
|
np.pi / 32, np.pi / 64, np.pi / 24]],
|
|
dtype=tf.float32)
|
|
cloud_source = self.random_cloud
|
|
cloud_target = icp_util.batch_transform_cloud_xyz(cloud_source,
|
|
ego_motion)
|
|
unused_transform, residual = self._run_icp(cloud_source,
|
|
self.identity_transform,
|
|
cloud_target)
|
|
self.assertAllClose(residual.eval(), tf.zeros_like(residual).eval(),
|
|
atol=1e-4)
|
|
|
|
def test_translate_organized_cloud(self):
|
|
with self.test_session():
|
|
tx = 0.1
|
|
cloud_source = self.organized_cloud
|
|
cloud_target = cloud_source + [tx, 0, 0]
|
|
transform, residual = self._run_icp(cloud_source, self.identity_transform,
|
|
cloud_target)
|
|
self.assertAlmostEqual(transform.eval()[0, self.index_translation], tx,
|
|
places=4)
|
|
self.assertAllClose(residual.eval(), tf.zeros_like(residual).eval(),
|
|
atol=1e-4)
|
|
|
|
def test_rotate_organized_cloud(self):
|
|
with self.test_session():
|
|
ego_motion = tf.constant([[0.0, 0.0, 0.0,
|
|
np.pi / 16, np.pi / 32, np.pi / 12]],
|
|
dtype=tf.float32)
|
|
cloud_source = self.organized_cloud
|
|
cloud_shape = cloud_source.shape.as_list()
|
|
flat_shape = (cloud_shape[0],
|
|
cloud_shape[1] * cloud_shape[2],
|
|
cloud_shape[3])
|
|
cloud_source = tf.reshape(cloud_source, shape=flat_shape)
|
|
cloud_target = icp_util.batch_transform_cloud_xyz(cloud_source,
|
|
ego_motion)
|
|
cloud_source = tf.reshape(cloud_source, cloud_shape)
|
|
cloud_target = tf.reshape(cloud_target, cloud_shape)
|
|
|
|
unused_transform, residual = self._run_icp(cloud_source,
|
|
self.identity_transform,
|
|
cloud_target)
|
|
self.assertAllClose(residual.eval(), tf.zeros_like(residual).eval(),
|
|
atol=1e-4)
|
|
|
|
def test_translate_lidar_cloud(self):
|
|
with self.test_session():
|
|
tx = 0.1
|
|
cloud_source = self.lidar_cloud
|
|
cloud_target = cloud_source + [tx, 0, 0]
|
|
transform, residual = self._run_icp(cloud_source, self.identity_transform,
|
|
cloud_target)
|
|
self.assertAlmostEqual(transform.eval()[0, self.index_translation], tx,
|
|
places=4)
|
|
self.assertAllClose(residual.eval(), tf.zeros_like(residual).eval(),
|
|
atol=1e-4)
|
|
|
|
def test_translate_lidar_cloud_ego_motion(self):
|
|
with self.test_session():
|
|
tx = 0.2
|
|
ego_motion = tf.constant([[tx, 0.0, 0.0,
|
|
0.0, 0.0, 0.0]], dtype=tf.float32)
|
|
cloud_source = self.lidar_cloud
|
|
cloud_target = cloud_source + [tx, 0, 0]
|
|
transform, residual = self._run_icp(cloud_source, ego_motion,
|
|
cloud_target)
|
|
self.assertAllClose(transform.eval(), tf.zeros_like(transform).eval(),
|
|
atol=1e-4)
|
|
self.assertAllClose(residual.eval(), tf.zeros_like(residual).eval(),
|
|
atol=1e-4)
|
|
|
|
def test_rotate_lidar_cloud_ego_motion(self):
|
|
with self.test_session():
|
|
transform = [0.0, 0.0, 0.0, np.pi / 16, np.pi / 32, np.pi / 12]
|
|
ego_motion = tf.constant([transform], dtype=tf.float32)
|
|
cloud_source = self.lidar_cloud
|
|
cloud_target = icp_util.batch_transform_cloud_xyz(cloud_source,
|
|
ego_motion)
|
|
transform, residual = self._run_icp(cloud_source, ego_motion,
|
|
cloud_target)
|
|
self.assertAllClose(transform.eval(), tf.zeros_like(transform).eval(),
|
|
atol=1e-4)
|
|
self.assertAllClose(residual.eval(), tf.zeros_like(residual).eval(),
|
|
atol=1e-3)
|
|
|
|
def test_no_change_lidar_cloud(self):
|
|
with self.test_session():
|
|
cloud_source = self.lidar_cloud
|
|
transform, residual = self._run_icp(cloud_source, self.identity_transform,
|
|
cloud_source)
|
|
self.assertAlmostEqual(transform.eval()[0, self.index_translation], 0,
|
|
places=4)
|
|
self.assertAllClose(residual.eval(), tf.zeros_like(residual).eval(),
|
|
atol=1e-4)
|
|
|
|
def test_translate_lidar_cloud_batch_size_2(self):
|
|
with self.test_session():
|
|
batch_size = 2
|
|
tx = 0.1
|
|
self.assertEqual(len(self.lidar_cloud.shape), 3)
|
|
cloud_source = tf.tile(self.lidar_cloud, [batch_size, 1, 1])
|
|
cloud_target = cloud_source + [tx, 0, 0]
|
|
self.assertEqual(len(self.identity_transform.shape), 2)
|
|
ego_motion = tf.tile(self.identity_transform, [batch_size, 1])
|
|
logging.info('cloud_source.shape: %s', cloud_source.shape)
|
|
logging.info('cloud_target.shape: %s', cloud_target.shape)
|
|
transform, residual = self._run_icp(cloud_source, ego_motion,
|
|
cloud_target)
|
|
for b in range(batch_size):
|
|
self.assertAlmostEqual(transform.eval()[b, self.index_translation], tx,
|
|
places=4)
|
|
self.assertAllClose(residual.eval(), tf.zeros_like(residual).eval(),
|
|
atol=1e-4)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
tf.test.main()
|