def test_compute_range_image_cartesian(self): batch = 3 # The first entry in the channel is range. range_image = test_utils.generate_range_image( tf.constant([ [1, 1], [2, 2], ], dtype=tf.int32), tf.constant([ [5.0, 0.0, 1.5], [6.0, 0.0, 1.6], ], dtype=tf.float32), [3, 5, 3], batch) range_image = range_image[..., 0] extrinsic = test_utils.generate_extrinsic(3.14 / 4, 0.0, 0.0, [1.0, 0.0, 0.0], batch) inclination = tf.tile([[1.0, 0.0, -1.0]], [batch, 1]) range_image_polar = range_image_utils.compute_range_image_polar( range_image, extrinsic, inclination) range_image_cartesian = range_image_utils.compute_range_image_cartesian( range_image_polar, extrinsic) # Transform to sensor frame. range_image_cartesian_sensor_frame = _transform_range_image( range_image_cartesian, tf.linalg.inv(extrinsic)) with self.test_session() as sess: ri_polar, ri_cartesian = sess.run( [range_image_polar, range_image_cartesian_sensor_frame]) self.assertAllClose(ri_polar.shape, [3, 3, 5, 3]) self.assertAllClose(ri_cartesian.shape, [3, 3, 5, 3]) for i in range(3): self.assertAllClose(np.linalg.norm(ri_cartesian[i, 1, 1, :]), 5.0) self.assertAllClose(np.linalg.norm(ri_cartesian[i, 2, 2, :]), 6.0)
def test_build_range_image_from_point_cloud_from_points(self): """Builds range image from points directly.""" # Need large enough range to deal with numerical errors. batch = 1 num_cols = 4000 inclination = tf.range(1.0, -1.0, -0.001) num_rows = inclination.shape[0] points = tf.tile( tf.expand_dims( tf.constant([ [5.0, 0.0, 1.5], [20.0, 0.0, 1.5], ], dtype=tf.float32), axis=0), [batch, 1, 1]) num_points = tf.ones([batch], dtype=tf.int32) * 2 inclination = tf.tile(tf.expand_dims(inclination, axis=0), [batch, 1]) extrinsic = test_utils.generate_extrinsic(3.14 / 8, 0.0, 0.0, [1.0, 0.1, 0.1], batch) range_image, _, _ = range_image_utils.build_range_image_from_point_cloud( points, num_points, extrinsic, inclination, [num_rows, num_cols]) range_image_mask = tf.compat.v1.where(range_image > 1e-5) polar = range_image_utils.compute_range_image_polar(range_image, extrinsic, inclination) cartesian = range_image_utils.compute_range_image_cartesian( polar, extrinsic) points_recovered = tf.gather_nd(cartesian, range_image_mask) points_recovered = tf.reshape(points_recovered, [batch, -1, 3]) with self.test_session() as sess: points_recovered, points = sess.run([points_recovered, points]) self.assertAllClose(points_recovered, points, rtol=1e-02, atol=1e-2)
def test_build_range_image_from_point_cloud_from_range_image(self): """Builds range image from points extracted from range.""" batch = 2 width = 5 # The first entry in the channel is range. index = tf.constant([ [1, 1], [2, 2], ], dtype=tf.int32) range_image = test_utils.generate_range_image( index, tf.constant([ [5.0, 0.0, 1.5], [6.0, 0.0, 1.6], ], dtype=tf.float32), [5, width, 3], batch) range_image = range_image[..., 0] extrinsic = test_utils.generate_extrinsic(3.14 / 4, 0.0, 0.0, [1.0, 0.1, 0.1], batch) inclination = tf.tile([[1.0, 0.5, 0.0, -1.0, -2.0]], [batch, 1]) range_image_polar = range_image_utils.compute_range_image_polar( range_image, extrinsic, inclination) range_image_cartesian = range_image_utils.compute_range_image_cartesian( range_image_polar, extrinsic) indices = tf.constant([[0, 1, 1], [0, 2, 2], [1, 1, 1], [1, 2, 2]], dtype=tf.int32) # [4, 3] points = tf.gather_nd(range_image_cartesian, indices) points = tf.reshape(points, [batch, 2, 3]) # [batch, 5, 3] num_points = tf.constant([2, 2], dtype=tf.int32) range_images, range_image_indices, range_image_ranges = ( range_image_utils.build_range_image_from_point_cloud( points, num_points, extrinsic, inclination, point_features=points[..., 0:2] / 100.0, range_image_size=[5, width])) with self.test_session() as sess: ri, _, _ = sess.run( [range_images, range_image_indices, range_image_ranges]) self.assertAllClose(ri[..., 0], range_image, atol=0.01)
def test_compute_range_image_cartesian_from_raw_range_image(self): batch = 3 # The first entry in the channel is range. range_image = test_utils.generate_range_image( tf.constant([ [1, 1], [2, 2], ], dtype=tf.int32), tf.constant([ [5.0, 0.0, 1.5], [6.0, 0.0, 1.6], ], dtype=tf.float32), [3, 5, 3], batch) range_image = range_image[..., 0] extrinsic = test_utils.generate_extrinsic(3.14 / 4, 0.0, 0.0, [1.0, 0.0, 0.0], batch) frame_pose = test_utils.generate_extrinsic(3.14 / 4, 0.0, 0.0, [1.0, 0.0, 0.0], batch) pixel_pose = tf.tile( tf.expand_dims( tf.expand_dims( test_utils.generate_extrinsic(3.14 / 4, 0.0, 0.0, [1.0, 0.0, 0.0], batch), 1), 1), [1, 3, 5, 1, 1]) inclination = tf.tile([[1.0, 0.0, -1.0]], [batch, 1]) range_image_polar = range_image_utils.compute_range_image_polar( range_image, extrinsic, inclination) range_image_cartesian = range_image_utils.compute_range_image_cartesian( range_image_polar, extrinsic, pixel_pose=pixel_pose, frame_pose=frame_pose) # Transform to sensor frame. range_image_cartesian_sensor_frame = _transform_range_image( range_image_cartesian, tf.matrix_inverse(extrinsic)) with self.test_session() as sess: ri_polar, ri_cartesian = sess.run( [range_image_polar, range_image_cartesian_sensor_frame]) self.assertAllClose(ri_polar.shape, [3, 3, 5, 3]) self.assertAllClose(ri_cartesian.shape, [3, 3, 5, 3])