Add vid2depth model.

research/vid2depth/ops/icp_grad_test.py

+# 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 grad."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import icp_grad  # pylint: disable=unused-import
+import icp_test
+import tensorflow as tf
+from tensorflow.python.ops import gradient_checker
+
+
+class IcpOpGradTest(icp_test.IcpOpTestBase):
+
+  def test_grad_transform(self):
+    with self.test_session():
+      cloud_source = self.small_cloud
+      cloud_target = cloud_source + [0.05, 0, 0]
+      ego_motion = self.identity_transform
+      transform, unused_residual = self._run_icp(cloud_source, ego_motion,
+                                                 cloud_target)
+      err = gradient_checker.compute_gradient_error(ego_motion,
+                                                    ego_motion.shape.as_list(),
+                                                    transform,
+                                                    transform.shape.as_list())
+    # Since our gradient is an approximation, it doesn't pass a numerical check.
+    # Nonetheless, this test verifies that icp_grad computes a gradient.
+    self.assertGreater(err, 1e-3)
+
+  def test_grad_transform_same_ego_motion(self):
+    with self.test_session():
+      cloud_source = self.small_cloud
+      cloud_target = cloud_source + [0.1, 0, 0]
+      ego_motion = tf.constant([[0.1, 0.0, 0.0, 0.0, 0.0, 0.0]],
+                               dtype=tf.float32)
+      transform, unused_residual = self._run_icp(cloud_source, ego_motion,
+                                                 cloud_target)
+      err = gradient_checker.compute_gradient_error(ego_motion,
+                                                    ego_motion.shape.as_list(),
+                                                    transform,
+                                                    transform.shape.as_list())
+    # Since our gradient is an approximation, it doesn't pass a numerical check.
+    # Nonetheless, this test verifies that icp_grad computes a gradient.
+    self.assertGreater(err, 1e-3)
+
+  def test_grad_residual(self):
+    with self.test_session():
+      cloud_source = self.small_cloud
+      cloud_target = cloud_source + [0.05, 0, 0]
+      ego_motion = self.identity_transform
+      unused_transform, residual = self._run_icp(cloud_source, ego_motion,
+                                                 cloud_target)
+      err = gradient_checker.compute_gradient_error(
+          cloud_source, cloud_source.shape.as_list(), residual,
+          residual.shape.as_list())
+    # Since our gradient is an approximation, it doesn't pass a numerical check.
+    # Nonetheless, this test verifies that icp_grad computes a gradient.
+    self.assertGreater(err, 1e-3)
+
+
+if __name__ == '__main__':
+  tf.test.main()

research/vid2depth/ops/icp_op.py

+# 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.
+# ==============================================================================
+
+"""Loads icp op."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from absl import logging
+import tensorflow as tf
+
+try:
+  icp_op_module = tf.load_op_library('./ops/icp_op.so')
+  icp = icp_op_module.icp
+except Exception:  # pylint: disable=broad-except
+  logging.error('Could not load object file for ICP op.')
+  icp = None

research/vid2depth/ops/icp_op_kernel.cc

+/* 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.
+==============================================================================*/
+
+#include <pcl/point_types.h>
+#include <pcl/registration/icp.h>
+#include <pcl/registration/lum.h>
+#include "tensorflow/core/framework/common_shape_fns.h"
+#include "tensorflow/core/framework/op_kernel.h"
+#include "tensorflow/core/framework/register_types.h"
+#include "tensorflow/core/framework/shape_inference.h"
+#include "tensorflow/core/util/work_sharder.h"
+
+using ::pcl::PointCloud;
+using ::pcl::PointXYZ;
+using ::tensorflow::DEVICE_CPU;
+using ::tensorflow::OpKernel;
+using ::tensorflow::OpKernelConstruction;
+using ::tensorflow::OpKernelContext;
+using ::tensorflow::Shard;
+using ::tensorflow::Status;
+using ::tensorflow::Tensor;
+using ::tensorflow::TensorShape;
+using ::tensorflow::shape_inference::InferenceContext;
+using ::tensorflow::shape_inference::ShapeHandle;
+
+#define kTransformSize 6               // tx, ty, tz, rx, ry, rz.
+#define kMaxCorrespondenceDistance -1  // -1 = no limit.
+
+// Extend IterativeClosestPoint to expose this->correspondences_.
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class IterativeClosestPointExposed
+    : public pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar> {
+ public:
+  pcl::CorrespondencesPtr getCorrespondencesPtr() {
+    return this->correspondences_;
+  }
+};
+
+REGISTER_OP("Icp")
+    .Attr("T: realnumbertype")
+    .Input("p: T")
+    .Input("ego_motion: T")
+    .Input("q: T")
+    .Output("transform: T")
+    .Output("residual: T")
+    .SetShapeFn([](InferenceContext* c) {
+      // TODO(rezama): Add shape checks to ensure:
+      // p and q have the same rank.
+      // The last dimension for p and q is always 3.
+      // ego_motion has shape [B, kTransformSize].
+      ShapeHandle p_shape;
+      ShapeHandle q_shape;
+      ShapeHandle ego_motion_shape;
+      TF_RETURN_IF_ERROR(c->WithRankAtLeast(c->input(0), 3, &p_shape));
+      TF_RETURN_IF_ERROR(c->WithRankAtLeast(c->input(2), 3, &q_shape));
+      TF_RETURN_IF_ERROR(c->WithRank(c->input(1), 2, &ego_motion_shape));
+      if (c->RankKnown(p_shape)) {
+        c->set_output(0, c->MakeShape({c->Dim(p_shape, 0), kTransformSize}));
+      } else {
+        c->set_output(0, c->UnknownShape());
+      }
+      c->set_output(1, c->input(0));
+      return Status::OK();
+    })
+    .Doc(R"doc(
+Aligns two point clouds and returns the alignment transformation and residuals.
+
+p: A [B, k, 3] or [B, m, n, 3] Tensor containing x, y, z coordinates of source
+  point cloud before being transformed using ego_motion.
+ego_motion: A [B, 6] Tensor containing [rx, ry, rz, tx, ty, tz].
+q: A [B, k', 3] or [B, m', n', 3] Tensor containing x, y, z coordinates of
+  target point cloud.
+
+transform: A [B, 6] Tensor representing the alignment transformation containing
+  [rx, ry, rz, tx, ty, tz].
+residual: A Tensor with the same shape as p containing the residual
+  q_c_i - transformation(p_i) for all points in p, where c_i denotes the index
+  of the matched point for p_i in q.
+)doc");
+
+template <typename T>
+class IcpOp : public OpKernel {
+ public:
+  explicit IcpOp(OpKernelConstruction* context) : OpKernel(context) {}
+
+  void Compute(OpKernelContext* context) override {
+    // TODO(rezama): Add shape checks to ensure:
+    // p and q have the same rank.
+    // The last dimension for p and q is always 3.
+    // ego_motion has shape [B, kTransformSize].
+
+    const Tensor& P_tensor = context->input(0);
+    const Tensor& Q_tensor = context->input(2);
+    const Tensor& ego_motion_tensor = context->input(1);
+    const TensorShape P_shape(P_tensor.shape());
+    const TensorShape Q_shape(Q_tensor.shape());
+    const TensorShape ego_motion_shape(ego_motion_tensor.shape());
+    const int batch_size = P_shape.dim_size(0);
+    const int P_rank = P_shape.dims();
+    const bool is_organized = P_rank == 4;
+    // P_cloud_size = k * 3 or m * n * 3.
+    int P_cloud_size, Q_cloud_size;
+    if (is_organized) {
+      P_cloud_size =
+          P_shape.dim_size(1) * P_shape.dim_size(2) * P_shape.dim_size(3);
+      Q_cloud_size =
+          Q_shape.dim_size(1) * Q_shape.dim_size(2) * Q_shape.dim_size(3);
+    } else {
+      P_cloud_size = P_shape.dim_size(1) * P_shape.dim_size(2);
+      Q_cloud_size = Q_shape.dim_size(1) * Q_shape.dim_size(2);
+    }
+    auto P_flat = P_tensor.flat<T>();
+    auto Q_flat = Q_tensor.flat<T>();
+    #define PIDX(b, i, j) b * P_cloud_size + i * 3 + j
+    #define QIDX(b, i, j) b * Q_cloud_size + i * 3 + j
+    auto ego_motion_flat = ego_motion_tensor.flat<T>();
+
+    // Create output tensors.
+    Tensor* transform_tensor = nullptr;
+    Tensor* residual_tensor = nullptr;
+    OP_REQUIRES_OK(context, context->allocate_output(
+                                0, TensorShape({batch_size, kTransformSize}),
+                                &transform_tensor));
+    OP_REQUIRES_OK(context,
+                   context->allocate_output(1, P_shape, &residual_tensor));
+    auto output_transform = transform_tensor->template flat<T>();
+    auto output_residual = residual_tensor->template flat<T>();
+
+    auto worker_threads = *(context->device()->tensorflow_cpu_worker_threads());
+
+    auto do_work = [context, &P_shape, &Q_shape, is_organized, &P_flat, &Q_flat,
+                    &output_transform, &output_residual, P_cloud_size,
+                    Q_cloud_size, &ego_motion_flat,
+                    this](int64 start_row, int64 limit_row) {
+      for (size_t b = start_row; b < limit_row; ++b) {
+        PointCloud<PointXYZ>::Ptr cloud_in(new PointCloud<PointXYZ>);
+        PointCloud<PointXYZ>::Ptr cloud_target(new PointCloud<PointXYZ>);
+
+        // Fill in the source cloud data.
+        if (is_organized) {
+          cloud_in->width = P_shape.dim_size(2);
+          cloud_in->height = P_shape.dim_size(1);
+        } else {
+          cloud_in->width = P_shape.dim_size(1);
+          cloud_in->height = 1;
+        }
+        cloud_in->is_dense = false;
+        cloud_in->points.resize(cloud_in->width * cloud_in->height);
+        for (size_t i = 0; i < cloud_in->points.size(); ++i) {
+          cloud_in->points[i].x = P_flat(PIDX(b, i, 0));
+          cloud_in->points[i].y = P_flat(PIDX(b, i, 1));
+          cloud_in->points[i].z = P_flat(PIDX(b, i, 2));
+        }
+        // Fill in the target cloud data.
+        if (is_organized) {
+          cloud_target->width = Q_shape.dim_size(2);
+          cloud_target->height = Q_shape.dim_size(1);
+        } else {
+          cloud_target->width = Q_shape.dim_size(1);
+          cloud_target->height = 1;
+        }
+        cloud_target->is_dense = false;
+        cloud_target->points.resize(cloud_target->width * cloud_target->height);
+        for (size_t i = 0; i < cloud_target->points.size(); ++i) {
+          cloud_target->points[i].x = Q_flat(QIDX(b, i, 0));
+          cloud_target->points[i].y = Q_flat(QIDX(b, i, 1));
+          cloud_target->points[i].z = Q_flat(QIDX(b, i, 2));
+        }
+        // Apply ego-motion.
+        Eigen::Vector6f ego_motion_vector;
+        int s = b * kTransformSize;
+        // TODO(rezama): Find out how to use slicing here.
+        ego_motion_vector << ego_motion_flat(s),
+                             ego_motion_flat(s + 1),
+                             ego_motion_flat(s + 2),
+                             ego_motion_flat(s + 3),
+                             ego_motion_flat(s + 4),
+                             ego_motion_flat(s + 5);
+        Eigen::Matrix4f ego_motion_mat =
+            ComposeTransformationMatrix(ego_motion_vector);
+        PointCloud<PointXYZ>::Ptr cloud_in_moved(new PointCloud<PointXYZ>());
+        pcl::transformPointCloud(*cloud_in, *cloud_in_moved, ego_motion_mat);
+
+        // Run ICP.
+        IterativeClosestPointExposed<PointXYZ, PointXYZ> icp;
+        if (kMaxCorrespondenceDistance > 0) {
+          icp.setMaxCorrespondenceDistance(kMaxCorrespondenceDistance);
+        }
+        icp.setInputSource(cloud_in_moved);
+        icp.setInputTarget(cloud_target);
+        PointCloud<PointXYZ> Final;
+        icp.align(Final);
+        Eigen::Matrix4f transform = icp.getFinalTransformation();
+
+        // Execute the transformation.
+        PointCloud<PointXYZ>::Ptr transformed_cloud(new PointCloud<PointXYZ>());
+        pcl::transformPointCloud(*cloud_in_moved, *transformed_cloud,
+                                 transform);
+
+        // Compute residual.
+        pcl::CorrespondencesPtr correspondences = icp.getCorrespondencesPtr();
+        for (size_t i = 0; i < cloud_in->points.size(); i++) {
+          output_residual(PIDX(b, i, 0)) = 0;
+          output_residual(PIDX(b, i, 1)) = 0;
+          output_residual(PIDX(b, i, 2)) = 0;
+        }
+        for (size_t i = 0; i < correspondences->size(); i++) {
+          pcl::Correspondence corr = (*correspondences)[i];
+          PointXYZ p_i_trans = transformed_cloud->points[corr.index_query];
+          PointXYZ q_i = cloud_target->points[corr.index_match];
+          PointXYZ residual_i;
+          // Compute residual_i = q_i - p_i_trans
+          residual_i.getArray3fMap() =
+              q_i.getArray3fMap() - p_i_trans.getArray3fMap();
+          output_residual(PIDX(b, corr.index_query, 0)) = residual_i.x;
+          output_residual(PIDX(b, corr.index_query, 1)) = residual_i.y;
+          output_residual(PIDX(b, corr.index_query, 2)) = residual_i.z;
+        }
+
+        // Decompose transformation.
+        Eigen::Vector3f rotation = DecomposeRotationScale(transform);
+        output_transform(b * kTransformSize + 0) = transform(0, 3);  // tx.
+        output_transform(b * kTransformSize + 1) = transform(1, 3);  // ty.
+        output_transform(b * kTransformSize + 2) = transform(2, 3);  // tz.
+        output_transform(b * kTransformSize + 3) = rotation(0);      // rx.
+        output_transform(b * kTransformSize + 4) = rotation(1);      // ry.
+        output_transform(b * kTransformSize + 5) = rotation(2);      // rz.
+      }
+    };  // End of closure.
+
+    // Incredibly rough estimate of clock cycles for do_work
+    const int64 cost = 50 * P_cloud_size * P_cloud_size;
+    Shard(worker_threads.num_threads, worker_threads.workers, batch_size, cost,
+          do_work);
+  }
+
+  Eigen::Vector3f DecomposeRotationScale(const Eigen::Matrix4f& mat) {
+    // Get columns.
+    Eigen::Vector3f c0 = mat.col(0).head(3);
+    Eigen::Vector3f c1 = mat.col(1).head(3);
+    Eigen::Vector3f c2 = mat.col(2).head(3);
+
+    // Compute scale.
+    float sx = c0.norm();
+    float sy = c1.norm();
+    float sz = c2.norm();
+
+    // Normalize rotations.
+    c0 /= sx;
+    c1 /= sy;
+    c2 /= sz;
+
+    // Compute Euler angles.
+    float rx = atan2(c1(2), c2(2));
+    float ry = atan2(-c0(2), sqrt(c1(2) * c1(2) + c2(2) * c2(2)));
+    float rz = atan2(c0(1), c0(0));
+
+    // Return as a vector.
+    Eigen::Vector3f rotation;
+    rotation << rx, ry, rz;
+    return rotation;
+  }
+
+  Eigen::Matrix4f ComposeTransformationMatrix(const Eigen::VectorXf& v) {
+    float tx = v(0);
+    float ty = v(1);
+    float tz = v(2);
+    float rx = v(3);
+    float ry = v(4);
+    float rz = v(5);
+
+    Eigen::Transform<float, 3, Eigen::Affine> t;
+    t = Eigen::Translation<float, 3>(tx, ty, tz);
+    t *= Eigen::AngleAxisf(rx, Eigen::Vector3f::UnitX());
+    t *= Eigen::AngleAxisf(ry, Eigen::Vector3f::UnitY());
+    t *= Eigen::AngleAxisf(rz, Eigen::Vector3f::UnitZ());
+
+    Eigen::Matrix4f mat = t.matrix();
+    return mat;
+  }
+};
+
+REGISTER_KERNEL_BUILDER(Name("Icp")
+                            .Device(DEVICE_CPU)
+                            .TypeConstraint<float>("T"),
+                        IcpOp<float>);
+REGISTER_KERNEL_BUILDER(Name("Icp")
+                            .Device(DEVICE_CPU)
+                            .TypeConstraint<double>("T"),
+                        IcpOp<double>);

research/vid2depth/ops/icp_test.py

+# 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()

research/vid2depth/ops/icp_train_demo.py

+# 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.
+# ==============================================================================
+
+"""Verify the op's ability to discover a hidden transformation and residual."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import os.path
+import random
+import time
+import google3
+from absl import app
+from absl import flags
+from absl import logging
+import icp_grad  # pylint: disable=unused-import
+from icp_op import icp
+import icp_util
+import numpy as np
+import tensorflow as tf
+
+FLAGS = flags.FLAGS
+flags.DEFINE_integer('batch_size', 4, 'Batch size.')
+flags.DEFINE_float('learning_rate', 0.1, 'Learning rate.')
+flags.DEFINE_integer('max_steps', 2000, 'Number of steps to run trainer.')
+flags.DEFINE_string('train_dir', '/tmp/icp_train_demo',
+                    'Directory to save event files for TensorBoard.')
+
+# Every training step feeds the model two points clouds A, B, such that
+# A = random_transform . sample_cloud
+# B = (SECRET_EGO_MOTION . A) + cone(CENTER, RADIUS, SECRET_RES_HEIGHT).
+# The ICP op gradients should help the program discover the values for
+# SECRET_EGO_MOTION and SECRET_RES_HEIGHT to get the best alignment for A, B.
+SECRET_EGO_MOTION = [0.0, 0.0, 0.1, 0.0, 0.0, 0.0]
+RES_CENTER = [0.103, 1.954, 0]
+RES_RADIUS = 10.0
+SECRET_RES_HEIGHT = 0.1
+
+
+class DataProducer(object):
+  """Generates training data."""
+
+  def __init__(self):
+    pass
+
+  @classmethod
+  def setup(cls):
+    """Open a KITTI video and read its point clouds."""
+    lidar_cloud_path = os.path.join(FLAGS.test_srcdir,
+                                    icp_util.LIDAR_CLOUD_PATH)
+    cls.sample_cloud = np.load(lidar_cloud_path)
+    logging.info('sample_cloud: %s', cls.sample_cloud)
+    x_min = np.min(cls.sample_cloud[:, 0])
+    x_max = np.max(cls.sample_cloud[:, 0])
+    y_min = np.min(cls.sample_cloud[:, 1])
+    y_max = np.max(cls.sample_cloud[:, 1])
+    z_min = np.min(cls.sample_cloud[:, 2])
+    z_max = np.max(cls.sample_cloud[:, 2])
+    logging.info('x: %s - %s', x_min, x_max)
+    logging.info('y: %s - %s', y_min, y_max)
+    logging.info('z: %s - %s', z_min, z_max)
+
+  @classmethod
+  def random_transform(cls):
+    tx = random.uniform(-0.2, 0.2)
+    ty = random.uniform(-0.2, 0.2)
+    tz = random.uniform(-0.9, 0.9)
+    rx = random.uniform(-0.2, 0.2) * np.pi
+    ry = random.uniform(-0.2, 0.2) * np.pi
+    rz = random.uniform(-0.2, 0.2) * np.pi
+    transform = [tx, ty, tz, rx, ry, rz]
+    return transform
+
+  @classmethod
+  def next_batch(cls, batch_size):
+    """Returns a training batch."""
+    source_items = []
+    target_items = []
+    for _ in range(batch_size):
+      source_cloud = icp_util.np_transform_cloud_xyz(cls.sample_cloud,
+                                                     cls.random_transform())
+      source_items.append(source_cloud)
+      dist_to_center = np.linalg.norm((source_cloud - RES_CENTER)[:, :2],
+                                      axis=1, keepdims=True)
+      res = np.maximum(RES_RADIUS - dist_to_center, 0.0) / RES_RADIUS
+      res *= SECRET_RES_HEIGHT
+      # x = 0, y = 0, z = res.
+      res = np.concatenate((np.zeros_like(res), np.zeros_like(res), res),
+                           axis=1)
+      target_cloud = icp_util.np_transform_cloud_xyz(source_cloud + res,
+                                                     SECRET_EGO_MOTION)
+      target_items.append(target_cloud)
+    return np.stack(source_items), np.stack(target_items)
+
+
+def placeholder_inputs(batch_size):
+  cloud_shape = (batch_size, DataProducer.sample_cloud.shape[0], 3)
+  source_placeholder = tf.placeholder(tf.float32, shape=cloud_shape)
+  target_placeholder = tf.placeholder(tf.float32, shape=cloud_shape)
+  return source_placeholder, target_placeholder
+
+
+def fill_feed_dict(source_placeholder, target_placeholder):
+  # Create the feed_dict for the placeholders filled with the next
+  # `batch size` examples.
+  source_feed, target_feed = DataProducer.next_batch(FLAGS.batch_size)
+  feed_dict = {
+      source_placeholder: source_feed,
+      target_placeholder: target_feed,
+  }
+  return feed_dict
+
+
+def run_training():
+  """Train model for a number of steps."""
+  # Tell TensorFlow that the model will be built into the default Graph.
+  with tf.Graph().as_default():
+    DataProducer.setup()
+    source_placeholder, target_placeholder = placeholder_inputs(
+        FLAGS.batch_size)
+    transform, residual = inference(source_placeholder, target_placeholder)
+    loss = loss_func(transform, residual)
+    train_op = training(loss, FLAGS.learning_rate)
+    summary_op = tf.summary.merge_all()
+    init = tf.global_variables_initializer()
+
+    with tf.Session() as sess:
+      summary_writer = tf.summary.FileWriter(FLAGS.train_dir, sess.graph)
+      sess.run(init)
+      # Start the training loop.
+      for step in range(FLAGS.max_steps):
+        start_time = time.time()
+        feed_dict = fill_feed_dict(source_placeholder, target_placeholder)
+        _, loss_value = sess.run([train_op, loss], feed_dict=feed_dict)
+        duration = time.time() - start_time
+        # Print status to stdout.
+        print('Step %d: loss = %f (%.2f sec)' % (step, loss_value, duration))
+        # Update the events file.
+        summary_str = sess.run(summary_op, feed_dict=feed_dict)
+        summary_writer.add_summary(summary_str, step)
+        summary_writer.flush()
+
+
+def inference(source, target):
+  """Builds model."""
+  ego_motion = tf.Variable(tf.zeros([6]), name='ego_motion')
+  res_height = tf.Variable(tf.fill([1], 0.0), name='res_height')
+  tf.summary.scalar('tx', ego_motion[0])
+  tf.summary.scalar('ty', ego_motion[1])
+  tf.summary.scalar('tz', ego_motion[2])
+  tf.summary.scalar('rx', ego_motion[3])
+  tf.summary.scalar('ry', ego_motion[4])
+  tf.summary.scalar('rz', ego_motion[5])
+  tf.summary.scalar('res_height', res_height[0])
+
+  dist_to_center = tf.norm((source - RES_CENTER)[:, :, :2], axis=2,
+                           keep_dims=True)
+  res = tf.maximum(RES_RADIUS - dist_to_center, 0.0) / RES_RADIUS
+  res *= res_height
+  res = tf.concat([tf.zeros_like(res), tf.zeros_like(res), res], axis=2)
+
+  shifted_source = source + res
+  ego_motion = tf.stack([ego_motion] * FLAGS.batch_size)
+  transform, residual = icp(shifted_source, ego_motion, target)
+  return transform, residual
+
+
+def loss_func(transform, residual):
+  return (tf.reduce_mean(tf.square(transform), name='transform_mean') +
+          tf.reduce_mean(tf.square(residual), name='residual_mean'))
+
+
+def training(loss, learning_rate):
+  tf.summary.scalar('loss', loss)
+  optimizer = tf.train.GradientDescentOptimizer(learning_rate)
+  # Create a variable to track the global step.
+  global_step = tf.Variable(0, name='global_step', trainable=False)
+  # Use the optimizer to apply the gradients that minimize the loss
+  # (and also increment the global step counter) as a single training step.
+  train_op = optimizer.minimize(loss, global_step=global_step)
+  return train_op
+
+
+def main(_):
+  run_training()
+
+
+if __name__ == '__main__':
+  app.run(main)

research/vid2depth/ops/icp_util.py

+# 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.
+# ==============================================================================
+
+"""Utility functions for transformations."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import numpy as np
+import tensorflow as tf
+
+# Sample pointcloud with shape (1568, 3).
+LIDAR_CLOUD_PATH = 'ops/testdata/pointcloud.npy'
+
+
+def get_transformation_matrix(transform):
+  """Converts [tx, ty, tz, rx, ry, rz] to a transform matrix."""
+  rx = transform[3]
+  ry = transform[4]
+  rz = transform[5]
+
+  rz = tf.clip_by_value(rz, -np.pi, np.pi)
+  ry = tf.clip_by_value(ry, -np.pi, np.pi)
+  rx = tf.clip_by_value(rx, -np.pi, np.pi)
+
+  cos_rx = tf.cos(rx)
+  sin_rx = tf.sin(rx)
+  rotx_1 = tf.stack([1.0, 0.0, 0.0])
+  rotx_2 = tf.stack([0.0, cos_rx, -sin_rx])
+  rotx_3 = tf.stack([0.0, sin_rx, cos_rx])
+  xmat = tf.stack([rotx_1, rotx_2, rotx_3])
+
+  cos_ry = tf.cos(ry)
+  sin_ry = tf.sin(ry)
+  roty_1 = tf.stack([cos_ry, 0.0, sin_ry])
+  roty_2 = tf.stack([0.0, 1.0, 0.0])
+  roty_3 = tf.stack([-sin_ry, 0.0, cos_ry])
+  ymat = tf.stack([roty_1, roty_2, roty_3])
+
+  cos_rz = tf.cos(rz)
+  sin_rz = tf.sin(rz)
+  rotz_1 = tf.stack([cos_rz, -sin_rz, 0.0])
+  rotz_2 = tf.stack([sin_rz, cos_rz, 0.0])
+  rotz_3 = tf.stack([0.0, 0.0, 1.0])
+  zmat = tf.stack([rotz_1, rotz_2, rotz_3])
+
+  rotate = tf.matmul(tf.matmul(xmat, ymat), zmat)
+
+  translate = transform[:3]
+  mat = tf.concat([rotate, tf.expand_dims(translate, 1)], axis=1)
+
+  hom_filler = tf.constant([0.0, 0.0, 0.0, 1.0], shape=[1, 4], dtype=tf.float32)
+  mat = tf.concat([mat, hom_filler], axis=0)
+  return mat
+
+
+def np_get_transformation_matrix(transform):
+  """Converts [tx, ty, tz, rx, ry, rz] to a transform matrix."""
+  rx = transform[3]
+  ry = transform[4]
+  rz = transform[5]
+
+  rz = np.clip(rz, -np.pi, np.pi)
+  ry = np.clip(ry, -np.pi, np.pi)
+  rx = np.clip(rx, -np.pi, np.pi)
+
+  cos_rx = np.cos(rx)
+  sin_rx = np.sin(rx)
+  rotx_1 = np.stack([1.0, 0.0, 0.0])
+  rotx_2 = np.stack([0.0, cos_rx, -sin_rx])
+  rotx_3 = np.stack([0.0, sin_rx, cos_rx])
+  xmat = np.stack([rotx_1, rotx_2, rotx_3])
+
+  cos_ry = np.cos(ry)
+  sin_ry = np.sin(ry)
+  roty_1 = np.stack([cos_ry, 0.0, sin_ry])
+  roty_2 = np.stack([0.0, 1.0, 0.0])
+  roty_3 = np.stack([-sin_ry, 0.0, cos_ry])
+  ymat = np.stack([roty_1, roty_2, roty_3])
+
+  cos_rz = np.cos(rz)
+  sin_rz = np.sin(rz)
+  rotz_1 = np.stack([cos_rz, -sin_rz, 0.0])
+  rotz_2 = np.stack([sin_rz, cos_rz, 0.0])
+  rotz_3 = np.stack([0.0, 0.0, 1.0])
+  zmat = np.stack([rotz_1, rotz_2, rotz_3])
+
+  rotate = np.dot(np.dot(xmat, ymat), zmat)
+
+  translate = transform[:3]
+  mat = np.concatenate((rotate, np.expand_dims(translate, 1)), axis=1)
+
+  hom_filler = np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32)
+  mat = np.concatenate((mat, hom_filler), axis=0)
+  return mat
+
+
+def transform_cloud_xyz(cloud, transform):
+  num_points = cloud.shape.as_list()[0]
+  ones = tf.ones(shape=[num_points, 1], dtype=tf.float32)
+  hom_cloud = tf.concat([cloud, ones], axis=1)
+  hom_cloud_t = tf.transpose(hom_cloud)
+  mat = get_transformation_matrix(transform)
+  transformed_cloud = tf.matmul(mat, hom_cloud_t)
+  transformed_cloud = tf.transpose(transformed_cloud)
+  transformed_cloud = transformed_cloud[:, :3]
+  return transformed_cloud
+
+
+def np_transform_cloud_xyz(cloud, transform):
+  num_points = cloud.shape[0]
+  ones = np.ones(shape=[num_points, 1], dtype=np.float32)
+  hom_cloud = np.concatenate((cloud, ones), axis=1)
+  hom_cloud_t = np.transpose(hom_cloud)
+  mat = np_get_transformation_matrix(transform)
+  transformed_cloud = np.dot(mat, hom_cloud_t)
+  transformed_cloud = np.transpose(transformed_cloud)
+  transformed_cloud = transformed_cloud[:, :3]
+  return transformed_cloud
+
+
+def batch_transform_cloud_xyz(cloud, transform):
+  results = []
+  cloud_items = tf.unstack(cloud)
+  if len(transform.shape.as_list()) == 2:
+    transform_items = tf.unstack(transform)
+  else:
+    transform_items = [transform] * len(cloud_items)
+  for cloud_item, transform_item in zip(cloud_items, transform_items):
+    results.append(transform_cloud_xyz(cloud_item, transform_item))
+  return tf.stack(results)

research/vid2depth/ops/pcl_demo.cc

+/* 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.
+==============================================================================*/
+
+#include <math.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <pcl/point_types.h>
+#include <pcl/registration/icp.h>
+#include <pcl/registration/lum.h>
+
+#include <iostream>
+#include <memory>
+#include <vector>
+
+using pcl::PointCloud;
+using pcl::PointXYZ;
+
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class IterativeClosestPointExposed
+    : public pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar> {
+ public:
+  pcl::CorrespondencesPtr getCorrespondencesPtr() {
+    for (uint32_t i = 0; i < this->correspondences_->size(); i++) {
+      pcl::Correspondence currentCorrespondence = (*this->correspondences_)[i];
+      std::cout << "Index of the source point: "
+                << currentCorrespondence.index_query << std::endl;
+      std::cout << "Index of the matching target point: "
+                << currentCorrespondence.index_match << std::endl;
+      std::cout << "Distance between the corresponding points: "
+                << currentCorrespondence.distance << std::endl;
+      std::cout << "Weight of the confidence in the correspondence: "
+                << currentCorrespondence.weight << std::endl;
+    }
+    return this->correspondences_;
+  }
+};
+
+int main(int argc, char** argv) {
+  PointCloud<PointXYZ>::Ptr cloud_in(new PointCloud<PointXYZ>);
+  PointCloud<PointXYZ>::Ptr cloud_out(new PointCloud<PointXYZ>);
+
+  // Fill in the CloudIn data
+  cloud_in->width = 5;
+  cloud_in->height = 1;
+  cloud_in->is_dense = false;
+  cloud_in->points.resize(cloud_in->width * cloud_in->height);
+  for (size_t i = 0; i < cloud_in->points.size(); ++i) {
+    cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
+    cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
+    cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
+  }
+  std::cout << "Saved " << cloud_in->points.size()
+            << " data points to input:" << std::endl;
+  for (size_t i = 0; i < cloud_in->points.size(); ++i)
+    std::cout << "    " << cloud_in->points[i].x << " " << cloud_in->points[i].y
+              << " " << cloud_in->points[i].z << std::endl;
+  *cloud_out = *cloud_in;
+  std::cout << "size:" << cloud_out->points.size() << std::endl;
+  for (size_t i = 0; i < cloud_in->points.size(); ++i)
+    cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
+  std::cout << "Transformed " << cloud_in->points.size()
+            << " data points:" << std::endl;
+  for (size_t i = 0; i < cloud_out->points.size(); ++i)
+    std::cout << "    " << cloud_out->points[i].x << " "
+              << cloud_out->points[i].y << " " << cloud_out->points[i].z
+              << std::endl;
+
+  IterativeClosestPointExposed<PointXYZ, PointXYZ> icp;
+  icp.setInputSource(cloud_in);
+  icp.setInputTarget(cloud_out);
+  PointCloud<PointXYZ> Final;
+  icp.align(Final);
+  std::cout << "has converged:" << icp.hasConverged()
+            << " score: " << icp.getFitnessScore() << std::endl;
+  Eigen::Matrix4f transform = icp.getFinalTransformation();
+  std::cout << transform << std::endl;
+  icp.getCorrespondencesPtr();
+
+  Eigen::Matrix3f m;
+
+  m = Eigen::AngleAxisf(0.25 * M_PI, Eigen::Vector3f::UnitX()) *
+      Eigen::AngleAxisf(0.5 * M_PI, Eigen::Vector3f::UnitY()) *
+      Eigen::AngleAxisf(0.33 * M_PI, Eigen::Vector3f::UnitZ());
+
+  std::cout << "original rotation:" << std::endl;
+  std::cout << m << std::endl << std::endl;
+
+  Eigen::Vector3f ea = m.eulerAngles(0, 1, 2);
+  std::cout << "to Euler angles:" << std::endl;
+  std::cout << ea << std::endl << std::endl;
+
+  Eigen::Matrix3f n;
+  n = Eigen::AngleAxisf(ea[0], Eigen::Vector3f::UnitX()) *
+      Eigen::AngleAxisf(ea[1], Eigen::Vector3f::UnitY()) *
+      Eigen::AngleAxisf(ea[2], Eigen::Vector3f::UnitZ());
+  std::cout << "recalc original rotation:" << std::endl;
+  std::cout << n << std::endl << std::endl;
+
+  Eigen::Vector3f c0 = m.col(0);
+  Eigen::Vector3f c1 = m.col(1);
+  Eigen::Vector3f c2 = m.col(2);
+
+  float sx = c0.norm();
+  float sy = c1.norm();
+  float sz = c2.norm();
+
+  c0 /= sx;
+  c1 /= sy;
+  c2 /= sz;
+
+  float rx = atan2(c1(2), c2(2));
+  float ry = atan2(-c0(2), sqrt(c1(2) * c1(2) + c2(2) * c2(2)));
+  float rz = atan2(c0(1), c0(0));
+
+  std::cout << "Scales:" << std::endl;
+  std::cout << sx << std::endl
+            << sy << std::endl
+            << sz << std::endl
+            << std::endl;
+
+  std::cout << "Euler angles:" << std::endl;
+  std::cout << rx << std::endl
+            << ry << std::endl
+            << rz << std::endl
+            << std::endl;
+
+  Eigen::Vector6f v6f;
+  v6f << sx, sy, sz, rx, ry, rz;
+  std::cout << "Vector6f:" << std::endl;
+  std::cout << v6f << std::endl << std::endl;
+
+  return (0);
+}

research/vid2depth/project.py

+# 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.
+# ==============================================================================
+
+"""Geometry utilities for projecting frames based on depth and motion.
+
+Modified from Spatial Transformer Networks:
+https://github.com/tensorflow/models/blob/master/transformer/spatial_transformer.py
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from absl import logging
+import numpy as np
+import tensorflow as tf
+
+
+def inverse_warp(img, depth, egomotion, intrinsic_mat, intrinsic_mat_inv):
+  """Inverse warp a source image to the target image plane.
+
+  Args:
+    img: The source image (to sample pixels from) -- [B, H, W, 3].
+    depth: Depth map of the target image -- [B, H, W].
+    egomotion: 6DoF egomotion vector from target to source -- [B, 6].
+    intrinsic_mat: Camera intrinsic matrix -- [B, 3, 3].
+    intrinsic_mat_inv: Inverse of the intrinsic matrix -- [B, 3, 3].
+  Returns:
+    Projected source image
+  """
+  dims = tf.shape(img)
+  batch_size, img_height, img_width = dims[0], dims[1], dims[2]
+  depth = tf.reshape(depth, [batch_size, 1, img_height * img_width])
+  grid = _meshgrid_abs(img_height, img_width)
+  grid = tf.tile(tf.expand_dims(grid, 0), [batch_size, 1, 1])
+  cam_coords = _pixel2cam(depth, grid, intrinsic_mat_inv)
+  ones = tf.ones([batch_size, 1, img_height * img_width])
+  cam_coords_hom = tf.concat([cam_coords, ones], axis=1)
+  egomotion_mat = _egomotion_vec2mat(egomotion, batch_size)
+
+  # Get projection matrix for target camera frame to source pixel frame
+  hom_filler = tf.constant([0.0, 0.0, 0.0, 1.0], shape=[1, 1, 4])
+  hom_filler = tf.tile(hom_filler, [batch_size, 1, 1])
+  intrinsic_mat_hom = tf.concat(
+      [intrinsic_mat, tf.zeros([batch_size, 3, 1])], axis=2)
+  intrinsic_mat_hom = tf.concat([intrinsic_mat_hom, hom_filler], axis=1)
+  proj_target_cam_to_source_pixel = tf.matmul(intrinsic_mat_hom, egomotion_mat)
+  source_pixel_coords = _cam2pixel(cam_coords_hom,
+                                   proj_target_cam_to_source_pixel)
+  source_pixel_coords = tf.reshape(source_pixel_coords,
+                                   [batch_size, 2, img_height, img_width])
+  source_pixel_coords = tf.transpose(source_pixel_coords, perm=[0, 2, 3, 1])
+  projected_img, mask = _spatial_transformer(img, source_pixel_coords)
+  return projected_img, mask
+
+
+def _pixel2cam(depth, pixel_coords, intrinsic_mat_inv):
+  """Transform coordinates in the pixel frame to the camera frame."""
+  cam_coords = tf.matmul(intrinsic_mat_inv, pixel_coords) * depth
+  return cam_coords
+
+
+def _cam2pixel(cam_coords, proj_c2p):
+  """Transform coordinates in the camera frame to the pixel frame."""
+  pcoords = tf.matmul(proj_c2p, cam_coords)
+  x = tf.slice(pcoords, [0, 0, 0], [-1, 1, -1])
+  y = tf.slice(pcoords, [0, 1, 0], [-1, 1, -1])
+  z = tf.slice(pcoords, [0, 2, 0], [-1, 1, -1])
+  # Not tested if adding a small number is necessary
+  x_norm = x / (z + 1e-10)
+  y_norm = y / (z + 1e-10)
+  pixel_coords = tf.concat([x_norm, y_norm], axis=1)
+  return pixel_coords
+
+
+def _meshgrid_abs(height, width):
+  """Meshgrid in the absolute coordinates."""
+  x_t = tf.matmul(
+      tf.ones(shape=tf.stack([height, 1])),
+      tf.transpose(tf.expand_dims(tf.linspace(-1.0, 1.0, width), 1), [1, 0]))
+  y_t = tf.matmul(
+      tf.expand_dims(tf.linspace(-1.0, 1.0, height), 1),
+      tf.ones(shape=tf.stack([1, width])))
+  x_t = (x_t + 1.0) * 0.5 * tf.cast(width - 1, tf.float32)
+  y_t = (y_t + 1.0) * 0.5 * tf.cast(height - 1, tf.float32)
+  x_t_flat = tf.reshape(x_t, (1, -1))
+  y_t_flat = tf.reshape(y_t, (1, -1))
+  ones = tf.ones_like(x_t_flat)
+  grid = tf.concat([x_t_flat, y_t_flat, ones], axis=0)
+  return grid
+
+
+def _euler2mat(z, y, x):
+  """Converts euler angles to rotation matrix.
+
+   From:
+   https://github.com/pulkitag/pycaffe-utils/blob/master/rot_utils.py#L174
+
+   TODO: Remove the dimension for 'N' (deprecated for converting all source
+   poses altogether).
+
+  Args:
+    z: rotation angle along z axis (in radians) -- size = [B, n]
+    y: rotation angle along y axis (in radians) -- size = [B, n]
+    x: rotation angle along x axis (in radians) -- size = [B, n]
+
+  Returns:
+    Rotation matrix corresponding to the euler angles, with shape [B, n, 3, 3].
+  """
+  batch_size = tf.shape(z)[0]
+  n = 1
+  z = tf.clip_by_value(z, -np.pi, np.pi)
+  y = tf.clip_by_value(y, -np.pi, np.pi)
+  x = tf.clip_by_value(x, -np.pi, np.pi)
+
+  # Expand to B x N x 1 x 1
+  z = tf.expand_dims(tf.expand_dims(z, -1), -1)
+  y = tf.expand_dims(tf.expand_dims(y, -1), -1)
+  x = tf.expand_dims(tf.expand_dims(x, -1), -1)
+
+  zeros = tf.zeros([batch_size, n, 1, 1])
+  ones = tf.ones([batch_size, n, 1, 1])
+
+  cosz = tf.cos(z)
+  sinz = tf.sin(z)
+  rotz_1 = tf.concat([cosz, -sinz, zeros], axis=3)
+  rotz_2 = tf.concat([sinz, cosz, zeros], axis=3)
+  rotz_3 = tf.concat([zeros, zeros, ones], axis=3)
+  zmat = tf.concat([rotz_1, rotz_2, rotz_3], axis=2)
+
+  cosy = tf.cos(y)
+  siny = tf.sin(y)
+  roty_1 = tf.concat([cosy, zeros, siny], axis=3)
+  roty_2 = tf.concat([zeros, ones, zeros], axis=3)
+  roty_3 = tf.concat([-siny, zeros, cosy], axis=3)
+  ymat = tf.concat([roty_1, roty_2, roty_3], axis=2)
+
+  cosx = tf.cos(x)
+  sinx = tf.sin(x)
+  rotx_1 = tf.concat([ones, zeros, zeros], axis=3)
+  rotx_2 = tf.concat([zeros, cosx, -sinx], axis=3)
+  rotx_3 = tf.concat([zeros, sinx, cosx], axis=3)
+  xmat = tf.concat([rotx_1, rotx_2, rotx_3], axis=2)
+
+  return tf.matmul(tf.matmul(xmat, ymat), zmat)
+
+
+def _egomotion_vec2mat(vec, batch_size):
+  """Converts 6DoF transform vector to transformation matrix.
+
+  Args:
+    vec: 6DoF parameters [tx, ty, tz, rx, ry, rz] -- [B, 6].
+    batch_size: Batch size.
+
+  Returns:
+    A transformation matrix -- [B, 4, 4].
+  """
+  translation = tf.slice(vec, [0, 0], [-1, 3])
+  translation = tf.expand_dims(translation, -1)
+  rx = tf.slice(vec, [0, 3], [-1, 1])
+  ry = tf.slice(vec, [0, 4], [-1, 1])
+  rz = tf.slice(vec, [0, 5], [-1, 1])
+  rot_mat = _euler2mat(rz, ry, rx)
+  rot_mat = tf.squeeze(rot_mat, squeeze_dims=[1])
+  filler = tf.constant([0.0, 0.0, 0.0, 1.0], shape=[1, 1, 4])
+  filler = tf.tile(filler, [batch_size, 1, 1])
+  transform_mat = tf.concat([rot_mat, translation], axis=2)
+  transform_mat = tf.concat([transform_mat, filler], axis=1)
+  return transform_mat
+
+
+def _bilinear_sampler(im, x, y, name='blinear_sampler'):
+  """Perform bilinear sampling on im given list of x, y coordinates.
+
+  Implements the differentiable sampling mechanism with bilinear kernel
+  in https://arxiv.org/abs/1506.02025.
+
+  x,y are tensors specifying normalized coordinates [-1, 1] to be sampled on im.
+  For example, (-1, -1) in (x, y) corresponds to pixel location (0, 0) in im,
+  and (1, 1) in (x, y) corresponds to the bottom right pixel in im.
+
+  Args:
+    im: Batch of images with shape [B, h, w, channels].
+    x: Tensor of normalized x coordinates in [-1, 1], with shape [B, h, w, 1].
+    y: Tensor of normalized y coordinates in [-1, 1], with shape [B, h, w, 1].
+    name: Name scope for ops.
+
+  Returns:
+    Sampled image with shape [B, h, w, channels].
+    Principled mask with shape [B, h, w, 1], dtype:float32.  A value of 1.0
+      in the mask indicates that the corresponding coordinate in the sampled
+      image is valid.
+  """
+  with tf.variable_scope(name):
+    x = tf.reshape(x, [-1])
+    y = tf.reshape(y, [-1])
+
+    # Constants.
+    batch_size = tf.shape(im)[0]
+    _, height, width, channels = im.get_shape().as_list()
+
+    x = tf.to_float(x)
+    y = tf.to_float(y)
+    height_f = tf.cast(height, 'float32')
+    width_f = tf.cast(width, 'float32')
+    zero = tf.constant(0, dtype=tf.int32)
+    max_y = tf.cast(tf.shape(im)[1] - 1, 'int32')
+    max_x = tf.cast(tf.shape(im)[2] - 1, 'int32')
+
+    # Scale indices from [-1, 1] to [0, width - 1] or [0, height - 1].
+    x = (x + 1.0) * (width_f - 1.0) / 2.0
+    y = (y + 1.0) * (height_f - 1.0) / 2.0
+
+    # Compute the coordinates of the 4 pixels to sample from.
+    x0 = tf.cast(tf.floor(x), 'int32')
+    x1 = x0 + 1
+    y0 = tf.cast(tf.floor(y), 'int32')
+    y1 = y0 + 1
+
+    mask = tf.logical_and(
+        tf.logical_and(x0 >= zero, x1 <= max_x),
+        tf.logical_and(y0 >= zero, y1 <= max_y))
+    mask = tf.to_float(mask)
+
+    x0 = tf.clip_by_value(x0, zero, max_x)
+    x1 = tf.clip_by_value(x1, zero, max_x)
+    y0 = tf.clip_by_value(y0, zero, max_y)
+    y1 = tf.clip_by_value(y1, zero, max_y)
+    dim2 = width
+    dim1 = width * height
+
+    # Create base index.
+    base = tf.range(batch_size) * dim1
+    base = tf.reshape(base, [-1, 1])
+    base = tf.tile(base, [1, height * width])
+    base = tf.reshape(base, [-1])
+
+    base_y0 = base + y0 * dim2
+    base_y1 = base + y1 * dim2
+    idx_a = base_y0 + x0
+    idx_b = base_y1 + x0
+    idx_c = base_y0 + x1
+    idx_d = base_y1 + x1
+
+    # Use indices to lookup pixels in the flat image and restore channels dim.
+    im_flat = tf.reshape(im, tf.stack([-1, channels]))
+    im_flat = tf.to_float(im_flat)
+    pixel_a = tf.gather(im_flat, idx_a)
+    pixel_b = tf.gather(im_flat, idx_b)
+    pixel_c = tf.gather(im_flat, idx_c)
+    pixel_d = tf.gather(im_flat, idx_d)
+
+    x1_f = tf.to_float(x1)
+    y1_f = tf.to_float(y1)
+
+    # And finally calculate interpolated values.
+    wa = tf.expand_dims(((x1_f - x) * (y1_f - y)), 1)
+    wb = tf.expand_dims((x1_f - x) * (1.0 - (y1_f - y)), 1)
+    wc = tf.expand_dims(((1.0 - (x1_f - x)) * (y1_f - y)), 1)
+    wd = tf.expand_dims(((1.0 - (x1_f - x)) * (1.0 - (y1_f - y))), 1)
+
+    output = tf.add_n([wa * pixel_a, wb * pixel_b, wc * pixel_c, wd * pixel_d])
+    output = tf.reshape(output, tf.stack([batch_size, height, width, channels]))
+    mask = tf.reshape(mask, tf.stack([batch_size, height, width, 1]))
+    return output, mask
+
+
+def _spatial_transformer(img, coords):
+  """A wrapper over binlinear_sampler(), taking absolute coords as input."""
+  img_height = tf.cast(tf.shape(img)[1], tf.float32)
+  img_width = tf.cast(tf.shape(img)[2], tf.float32)
+  px = coords[:, :, :, :1]
+  py = coords[:, :, :, 1:]
+  # Normalize coordinates to [-1, 1] to send to _bilinear_sampler.
+  px = px / (img_width - 1) * 2.0 - 1.0
+  py = py / (img_height - 1) * 2.0 - 1.0
+  output_img, mask = _bilinear_sampler(img, px, py)
+  return output_img, mask
+
+
+def get_cloud(depth, intrinsics_inv, name=None):  # pylint: disable=unused-argument
+  """Convert depth map to 3D point cloud."""
+  with tf.name_scope(name):
+    dims = depth.shape.as_list()
+    batch_size, img_height, img_width = dims[0], dims[1], dims[2]
+    depth = tf.reshape(depth, [batch_size, 1, img_height * img_width])
+    grid = _meshgrid_abs(img_height, img_width)
+    grid = tf.tile(tf.expand_dims(grid, 0), [batch_size, 1, 1])
+    cam_coords = _pixel2cam(depth, grid, intrinsics_inv)
+    cam_coords = tf.transpose(cam_coords, [0, 2, 1])
+    cam_coords = tf.reshape(cam_coords, [batch_size, img_height, img_width, 3])
+    logging.info('depth -> cloud: %s', cam_coords)
+    return cam_coords

research/vid2depth/reader.py

+# 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.
+# ==============================================================================
+
+"""Reads data that is produced by dataset/gen_data.py."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import os
+import random
+from absl import logging
+import tensorflow as tf
+import util
+
+gfile = tf.gfile
+
+QUEUE_SIZE = 2000
+QUEUE_BUFFER = 3
+
+
+class DataReader(object):
+  """Reads stored sequences which are produced by dataset/gen_data.py."""
+
+  def __init__(self, data_dir, batch_size, img_height, img_width, seq_length,
+               num_scales):
+    self.data_dir = data_dir
+    self.batch_size = batch_size
+    self.img_height = img_height
+    self.img_width = img_width
+    self.seq_length = seq_length
+    self.num_scales = num_scales
+
+  def read_data(self):
+    """Provides images and camera intrinsics."""
+    with tf.name_scope('data_loading'):
+      with tf.name_scope('enqueue_paths'):
+        seed = random.randint(0, 2**31 - 1)
+        self.file_lists = self.compile_file_list(self.data_dir, 'train')
+        image_paths_queue = tf.train.string_input_producer(
+            self.file_lists['image_file_list'], seed=seed, shuffle=True)
+        cam_paths_queue = tf.train.string_input_producer(
+            self.file_lists['cam_file_list'], seed=seed, shuffle=True)
+        img_reader = tf.WholeFileReader()
+        _, image_contents = img_reader.read(image_paths_queue)
+        image_seq = tf.image.decode_jpeg(image_contents)
+
+      with tf.name_scope('load_intrinsics'):
+        cam_reader = tf.TextLineReader()
+        _, raw_cam_contents = cam_reader.read(cam_paths_queue)
+        rec_def = []
+        for _ in range(9):
+          rec_def.append([1.0])
+        raw_cam_vec = tf.decode_csv(raw_cam_contents, record_defaults=rec_def)
+        raw_cam_vec = tf.stack(raw_cam_vec)
+        intrinsics = tf.reshape(raw_cam_vec, [3, 3])
+
+      with tf.name_scope('convert_image'):
+        image_seq = self.preprocess_image(image_seq)  # Converts to float.
+
+      with tf.name_scope('image_augmentation'):
+        image_seq = self.augment_image_colorspace(image_seq)
+
+      image_stack = self.unpack_images(image_seq)
+
+      with tf.name_scope('image_augmentation_scale_crop'):
+        image_stack, intrinsics = self.augment_images_scale_crop(
+            image_stack, intrinsics, self.img_height, self.img_width)
+
+      with tf.name_scope('multi_scale_intrinsics'):
+        intrinsic_mat = self.get_multi_scale_intrinsics(intrinsics,
+                                                        self.num_scales)
+        intrinsic_mat.set_shape([self.num_scales, 3, 3])
+        intrinsic_mat_inv = tf.matrix_inverse(intrinsic_mat)
+        intrinsic_mat_inv.set_shape([self.num_scales, 3, 3])
+
+      with tf.name_scope('batching'):
+        image_stack, intrinsic_mat, intrinsic_mat_inv = (
+            tf.train.shuffle_batch(
+                [image_stack, intrinsic_mat, intrinsic_mat_inv],
+                batch_size=self.batch_size,
+                capacity=QUEUE_SIZE + QUEUE_BUFFER * self.batch_size,
+                min_after_dequeue=QUEUE_SIZE))
+        logging.info('image_stack: %s', util.info(image_stack))
+    return image_stack, intrinsic_mat, intrinsic_mat_inv
+
+  def unpack_images(self, image_seq):
+    """[h, w * seq_length, 3] -> [h, w, 3 * seq_length]."""
+    with tf.name_scope('unpack_images'):
+      image_list = [
+          image_seq[:, i * self.img_width:(i + 1) * self.img_width, :]
+          for i in range(self.seq_length)
+      ]
+      image_stack = tf.concat(image_list, axis=2)
+      image_stack.set_shape(
+          [self.img_height, self.img_width, self.seq_length * 3])
+    return image_stack
+
+  @classmethod
+  def preprocess_image(cls, image):
+    # Convert from uint8 to float.
+    return tf.image.convert_image_dtype(image, dtype=tf.float32)
+
+  # Source: https://github.com/mrharicot/monodepth.
+  @classmethod
+  def augment_image_colorspace(cls, image_seq):
+    """Apply data augmentation to inputs."""
+    # Randomly shift gamma.
+    random_gamma = tf.random_uniform([], 0.8, 1.2)
+    image_seq_aug = image_seq**random_gamma
+    # Randomly shift brightness.
+    random_brightness = tf.random_uniform([], 0.5, 2.0)
+    image_seq_aug *= random_brightness
+    # Randomly shift color.
+    random_colors = tf.random_uniform([3], 0.8, 1.2)
+    white = tf.ones([tf.shape(image_seq)[0], tf.shape(image_seq)[1]])
+    color_image = tf.stack([white * random_colors[i] for i in range(3)], axis=2)
+    image_seq_aug *= color_image
+    # Saturate.
+    image_seq_aug = tf.clip_by_value(image_seq_aug, 0, 1)
+    return image_seq_aug
+
+  @classmethod
+  def augment_images_scale_crop(cls, im, intrinsics, out_h, out_w):
+    """Randomly scales and crops image."""
+
+    def scale_randomly(im, intrinsics):
+      """Scales image and adjust intrinsics accordingly."""
+      in_h, in_w, _ = im.get_shape().as_list()
+      scaling = tf.random_uniform([2], 1, 1.15)
+      x_scaling = scaling[0]
+      y_scaling = scaling[1]
+      out_h = tf.cast(in_h * y_scaling, dtype=tf.int32)
+      out_w = tf.cast(in_w * x_scaling, dtype=tf.int32)
+      # Add batch.
+      im = tf.expand_dims(im, 0)
+      im = tf.image.resize_area(im, [out_h, out_w])
+      im = im[0]
+      fx = intrinsics[0, 0] * x_scaling
+      fy = intrinsics[1, 1] * y_scaling
+      cx = intrinsics[0, 2] * x_scaling
+      cy = intrinsics[1, 2] * y_scaling
+      intrinsics = cls.make_intrinsics_matrix(fx, fy, cx, cy)
+      return im, intrinsics
+
+    # Random cropping
+    def crop_randomly(im, intrinsics, out_h, out_w):
+      """Crops image and adjust intrinsics accordingly."""
+      # batch_size, in_h, in_w, _ = im.get_shape().as_list()
+      in_h, in_w, _ = tf.unstack(tf.shape(im))
+      offset_y = tf.random_uniform([1], 0, in_h - out_h + 1, dtype=tf.int32)[0]
+      offset_x = tf.random_uniform([1], 0, in_w - out_w + 1, dtype=tf.int32)[0]
+      im = tf.image.crop_to_bounding_box(im, offset_y, offset_x, out_h, out_w)
+      fx = intrinsics[0, 0]
+      fy = intrinsics[1, 1]
+      cx = intrinsics[0, 2] - tf.cast(offset_x, dtype=tf.float32)
+      cy = intrinsics[1, 2] - tf.cast(offset_y, dtype=tf.float32)
+      intrinsics = cls.make_intrinsics_matrix(fx, fy, cx, cy)
+      return im, intrinsics
+
+    im, intrinsics = scale_randomly(im, intrinsics)
+    im, intrinsics = crop_randomly(im, intrinsics, out_h, out_w)
+    return im, intrinsics
+
+  def compile_file_list(self, data_dir, split, load_pose=False):
+    """Creates a list of input files."""
+    logging.info('data_dir: %s', data_dir)
+    with gfile.Open(os.path.join(data_dir, '%s.txt' % split), 'r') as f:
+      frames = f.readlines()
+    subfolders = [x.split(' ')[0] for x in frames]
+    frame_ids = [x.split(' ')[1][:-1] for x in frames]
+    image_file_list = [
+        os.path.join(data_dir, subfolders[i], frame_ids[i] + '.jpg')
+        for i in range(len(frames))
+    ]
+    cam_file_list = [
+        os.path.join(data_dir, subfolders[i], frame_ids[i] + '_cam.txt')
+        for i in range(len(frames))
+    ]
+    file_lists = {}
+    file_lists['image_file_list'] = image_file_list
+    file_lists['cam_file_list'] = cam_file_list
+    if load_pose:
+      pose_file_list = [
+          os.path.join(data_dir, subfolders[i], frame_ids[i] + '_pose.txt')
+          for i in range(len(frames))
+      ]
+      file_lists['pose_file_list'] = pose_file_list
+    self.steps_per_epoch = len(image_file_list) // self.batch_size
+    return file_lists
+
+  @classmethod
+  def make_intrinsics_matrix(cls, fx, fy, cx, cy):
+    r1 = tf.stack([fx, 0, cx])
+    r2 = tf.stack([0, fy, cy])
+    r3 = tf.constant([0., 0., 1.])
+    intrinsics = tf.stack([r1, r2, r3])
+    return intrinsics
+
+  @classmethod
+  def get_multi_scale_intrinsics(cls, intrinsics, num_scales):
+    """Returns multiple intrinsic matrices for different scales."""
+    intrinsics_multi_scale = []
+    # Scale the intrinsics accordingly for each scale
+    for s in range(num_scales):
+      fx = intrinsics[0, 0] / (2**s)
+      fy = intrinsics[1, 1] / (2**s)
+      cx = intrinsics[0, 2] / (2**s)
+      cy = intrinsics[1, 2] / (2**s)
+      intrinsics_multi_scale.append(cls.make_intrinsics_matrix(fx, fy, cx, cy))
+    intrinsics_multi_scale = tf.stack(intrinsics_multi_scale)
+    return intrinsics_multi_scale

research/vid2depth/repo.bzl

+""" TensorFlow Http Archive
+
+Modified http_arhive that allows us to override the TensorFlow commit that is
+downloaded by setting an environment variable. This override is to be used for
+testing purposes.
+
+Add the following to your Bazel build command in order to override the
+TensorFlow revision.
+
+build: --action_env TF_REVISION="<git commit hash>"
+
+  * `TF_REVISION`: tensorflow revision override (git commit hash)
+"""
+
+_TF_REVISION = "TF_REVISION"
+
+def _tensorflow_http_archive(ctx):
+  git_commit = ctx.attr.git_commit
+  sha256 = ctx.attr.sha256
+
+  override_git_commit = ctx.os.environ.get(_TF_REVISION)
+  if override_git_commit:
+    sha256 = ""
+    git_commit = override_git_commit
+
+  strip_prefix = "tensorflow-%s" % git_commit
+  urls = [
+      "https://mirror.bazel.build/github.com/tensorflow/tensorflow/archive/%s.tar.gz" % git_commit,
+      "https://github.com/tensorflow/tensorflow/archive/%s.tar.gz" % git_commit,
+  ]
+  ctx.download_and_extract(
+      urls,
+      "",
+      sha256,
+      "",
+      strip_prefix)
+
+tensorflow_http_archive = repository_rule(
+    implementation=_tensorflow_http_archive,
+    attrs={
+        "git_commit": attr.string(mandatory=True),
+        "sha256": attr.string(mandatory=True),
+    })

research/vid2depth/third_party/BUILD

+licenses(["notice"])  # Apache 2.0

research/vid2depth/third_party/eigen.BUILD

+# Description:
+#   Eigen is a C++ template library for linear algebra: vectors,
+#   matrices, and related algorithms.
+
+licenses([
+    "reciprocal",  # MPL2
+    "notice",  # Portions BSD
+])
+
+exports_files(["COPYING.MPL2"])
+
+# Note: unsupported/Eigen is unsupported and might go away at any time.
+EIGEN_FILES = [
+    "Eigen/**",
+    "unsupported/**",
+]
+
+# Files known to be under MPL2 license.
+EIGEN_MPL2_HEADER_FILES = glob(
+    EIGEN_FILES,
+)
+
+cc_library(
+    name = "eigen",
+    hdrs = EIGEN_MPL2_HEADER_FILES,
+    defines = [
+        "EIGEN_MPL2_ONLY",
+    ],
+    includes = ["."],
+    visibility = ["//visibility:public"],
+)

research/vid2depth/third_party/flann.BUILD

+# Description:
+#    FLANN is a library for performing fast approximate nearest neighbor
+#    searches in high dimensional spaces. It contains a collection of
+#    algorithms we found to work best for nearest neighbor search and a system
+#    for automatically choosing the best algorithm and optimum parameters
+#    depending on the dataset.
+
+licenses(["notice"])  # BSD
+
+package(
+    default_visibility = ["//visibility:public"],
+    features = [
+        "-layering_check",
+        "-parse_headers",
+    ],
+)
+
+FLANN_COPTS = [
+    "-fopenmp",
+    "-fexceptions",
+    "-Wno-non-virtual-dtor",
+    "-Wno-unknown-pragmas",
+    "-Wno-conversion",
+]
+
+BOOST_TARGETS = [
+    "@boost//:algorithm",
+    "@boost//:align",
+    "@boost//:any",
+    "@boost//:archive",
+    "@boost//:array",
+    "@boost//:asio",
+    "@boost//:assert",
+    "@boost//:atomic",
+    "@boost//:beast",
+    "@boost//:bimap",
+    "@boost//:bind",
+    "@boost//:call_traits",
+    "@boost//:callable_traits",
+    "@boost//:cerrno",
+    "@boost//:checked_delete",
+    "@boost//:chrono",
+    "@boost//:circular_buffer",
+    # "@boost//:compute",
+    "@boost//:concept",
+    "@boost//:concept_archetype",
+    "@boost//:concept_check",
+    "@boost//:config",
+    "@boost//:container",
+    # "@boost//:context",
+    "@boost//:conversion",
+    "@boost//:core",
+    # "@boost//:coroutine",
+    "@boost//:cstdint",
+    "@boost//:current_function",
+    "@boost//:date_time",
+    "@boost//:detail",
+    "@boost//:dynamic_bitset",
+    "@boost//:enable_shared_from_this",
+    # "@boost//:endian",
+    "@boost//:exception",
+    "@boost//:exception_ptr",
+    # "@boost//:fiber",
+    "@boost//:filesystem",
+    "@boost//:foreach",
+    "@boost//:format",
+    "@boost//:function",
+    "@boost//:function_types",
+    "@boost//:functional",
+    "@boost//:fusion",
+    "@boost//:get_pointer",
+    "@boost//:heap",
+    # "@boost//:icl",
+    "@boost//:integer",
+    "@boost//:interprocess",
+    "@boost//:intrusive",
+    "@boost//:intrusive_ptr",
+    "@boost//:io",
+    "@boost//:iostreams",
+    "@boost//:is_placeholder",
+    "@boost//:iterator",
+    "@boost//:lexical_cast",
+    "@boost//:limits",
+    "@boost//:math",
+    "@boost//:mem_fn",
+    "@boost//:move",
+    "@boost//:mp11",
+    "@boost//:mpl",
+    "@boost//:multi_index",
+    # "@boost//:multiprecision",
+    "@boost//:noncopyable",
+    "@boost//:none",
+    "@boost//:numeric",
+    "@boost//:numeric_conversion",
+    "@boost//:numeric_ublas",
+    "@boost//:operators",
+    "@boost//:optional",
+    "@boost//:parameter",
+    # "@boost//:pool",
+    "@boost//:predef",
+    "@boost//:preprocessor",
+    "@boost//:process",
+    "@boost//:program_options",
+    "@boost//:property_tree",
+    "@boost//:ptr_container",
+    "@boost//:random",
+    "@boost//:range",
+    "@boost//:ratio",
+    # "@boost//:rational",
+    "@boost//:ref",
+    "@boost//:regex",
+    "@boost//:scope_exit",
+    "@boost//:scoped_array",
+    "@boost//:scoped_ptr",
+    "@boost//:serialization",
+    "@boost//:shared_array",
+    "@boost//:shared_ptr",
+    "@boost//:signals2",
+    "@boost//:smart_ptr",
+    "@boost//:spirit",
+    "@boost//:static_assert",
+    "@boost//:swap",
+    "@boost//:system",
+    "@boost//:thread",
+    "@boost//:throw_exception",
+    "@boost//:timer",
+    "@boost//:tokenizer",
+    # "@boost//:tribool",
+    "@boost//:tuple",
+    "@boost//:type",
+    "@boost//:type_index",
+    "@boost//:type_traits",
+    "@boost//:typeof",
+    "@boost//:unordered",
+    "@boost//:utility",
+    "@boost//:uuid",
+    "@boost//:variant",
+    "@boost//:version",
+    "@boost//:visit_each",
+]
+
+FLANN_DEPS = BOOST_TARGETS + [
+    "@hdf5//:hdf5",
+]
+
+cc_library(
+    name = "flann",
+    srcs = glob([
+        "src/**/*.cpp",
+    ]),
+    hdrs = glob([
+        "src/**/*.h",
+        "src/**/*.hpp",
+    ]),
+    copts = FLANN_COPTS,
+    includes = [
+        "src/cpp",
+    ],
+    linkopts = ["-lgomp"],
+    deps = FLANN_DEPS,
+)