Add planar trifocal tensor

gtsam/geometry/TrifocalTensor2.cpp

@@ -0,0 +1,98 @@
+/* ----------------------------------------------------------------------------
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    TrifocalTensor2.cpp
+ * @brief   A 2x2x2 trifocal tensor in a plane, for 1D cameras.
+ * @author  Zhaodong Yang
+ * @author  Akshay Krishnan
+ */
+
+#include <gtsam/geometry/TrifocalTensor2.h>
+
+#include <stdexcept>
+#include <vector>
+
+namespace gtsam {
+
+// Convert bearing measurements to projective coordinates.
+std::vector<Point2> convertToProjective(const std::vector<Rot2>& rotations) {
+  std::vector<Point2> projectives;
+  projectives.reserve(rotations.size());
+  for (const Rot2& rotation : rotations) {
+    projectives.emplace_back(rotation.c() / rotation.s(), 1.0);
+  }
+  return projectives;
+}
+
+// Construct from 8 bearing measurements.
+TrifocalTensor2 TrifocalTensor2::FromBearingMeasurements(
+    const std::vector<Rot2>& bearings_u, const std::vector<Rot2>& bearings_v,
+    const std::vector<Rot2>& bearings_w) {
+  return TrifocalTensor2::FromProjectiveBearingMeasurements(
+      convertToProjective(bearings_u), convertToProjective(bearings_v),
+      convertToProjective(bearings_w));
+}
+
+// Construct from 8 bearing measurements expressed in projective coordinates.
+TrifocalTensor2 TrifocalTensor2::FromProjectiveBearingMeasurements(
+    const std::vector<Point2>& u, const std::vector<Point2>& v,
+    const std::vector<Point2>& w) {
+  if (u.size() < 8) {
+    throw std::invalid_argument(
+        "Trifocal tensor computation requires at least 8 measurements");
+  }
+  if (u.size() != v.size() || v.size() != w.size()) {
+    throw std::invalid_argument(
+        "Number of input measurements in 3 cameras must be same");
+  }
+
+  // Create the system matrix A.
+  Matrix A(u.size() > 8 ? u.size() : 8, 8);
+  for (int row = 0; row < u.size(); row++) {
+    for (int i = 0; i < 2; i++) {
+      for (int j = 0; j < 2; j++) {
+        for (int k = 0; k < 2; k++) {
+          A(row, 4 * i + 2 * j + k) = u[row](i) * v[row](j) * w[row](k);
+        }
+      }
+    }
+  }
+  for (int row = u.size() - 8; row < 0; row++) {
+    for (int col = 0; col < 8; col++) {
+      A(row, col) = 0;
+    }
+  }
+
+  // Eigen vector of smallest singular value is the trifocal tensor.
+  Matrix U, V;
+  Vector S;
+  svd(A, U, S, V);
+
+  Matrix2 matrix0, matrix1;
+  for (int i = 0; i < 2; i++) {
+    for (int j = 0; j < 2; j++) {
+      matrix0(i, j) = V(2 * i + j, V.cols() - 1);
+      matrix1(i, j) = V(2 * i + j + 4, V.cols() - 1);
+    }
+  }
+  return TrifocalTensor2(matrix0, matrix1);
+}
+
+// Finds a measurement in the first view using measurements from second and
+// third views.
+Rot2 TrifocalTensor2::transform(const Rot2& vZp, const Rot2& wZp) const {
+  Rot2 uZp;
+  Vector2 v_measurement, w_measurement;
+  v_measurement << vZp.c(), vZp.s();
+  w_measurement << wZp.c(), wZp.s();
+  return Rot2::atan2(dot(matrix0_ * w_measurement, v_measurement),
+                     -dot(matrix1_ * w_measurement, v_measurement));
+}
+
+}  // namespace gtsam

gtsam/geometry/TrifocalTensor2.h

@@ -0,0 +1,95 @@
+/* ----------------------------------------------------------------------------
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    TrifocalTensor2.h
+ * @brief   A 2x2x2 trifocal tensor in a plane, for 1D cameras.
+ * @author  Zhaodong Yang
+ * @author  Akshay Krishnan
+ */
+// \callgraph
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Rot2.h>
+
+namespace gtsam {
+
+/**
+ * @brief A trifocal tensor for 1D cameras in a plane. It encodes the
+ * relationship between bearing measurements of a point in the plane observed in
+ * 3 1D cameras.
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class TrifocalTensor2 {
+ private:
+  // The trifocal tensor has 2 matrices.
+  Matrix2 matrix0_, matrix1_;
+
+ public:
+  TrifocalTensor2() {}
+
+  // Construct from the two 2x2 matrices that form the tensor.
+  TrifocalTensor2(const Matrix2& matrix0, const Matrix2& matrix1)
+      : matrix0_(matrix0), matrix1_(matrix1) {}
+
+  /**
+   * @brief Estimates a tensor from 8 bearing measurements in 3 cameras. Throws
+   * a runtime error if the size of inputs are unequal or less than 8.
+   *
+   * @param u bearing measurement in camera u.
+   * @param v bearing measurement in camera v.
+   * @param w bearing measurement in camera w.
+   * @return Tensor estimated from the measurements.
+   */
+  static TrifocalTensor2 FromBearingMeasurements(
+      const std::vector<Rot2>& bearings_u, const std::vector<Rot2>& bearings_v,
+      const std::vector<Rot2>& bearings_w);
+
+  /**
+   * @brief Estimates a tensor from 8 projective measurements in 3 cameras.
+   * Throws a runtime error if the size of inputs are unequal or less than 8.
+   *
+   * @param u projective 1D bearing measurement in camera u.
+   * @param v projective 1D bearing measurement in camera v.
+   * @param w projective 1D bearing measurement in camera w.
+   * @return tensor estimated from the measurements.
+   */
+  static TrifocalTensor2 FromProjectiveBearingMeasurements(
+      const std::vector<Point2>& u, const std::vector<Point2>& v,
+      const std::vector<Point2>& w);
+
+  /**
+   * @brief Computes the bearing in camera 'u' given bearing measurements in
+   * cameras 'v' and 'w'.
+   *
+   * @param vZp bearing measurement in camera v
+   * @param wZp bearing measurement in camera w
+   * @return bearing measurement in camera u
+   */
+  Rot2 transform(const Rot2& vZp, const Rot2& wZp) const;
+
+  /**
+   * @brief Computes the bearing in camera 'u' from that of cameras 'v' and 'w',
+   * in projective coordinates.
+   *
+   * @param vZp projective bearing measurement in camera v
+   * @param wZp projective bearing measurement in camera w
+   * @return projective bearing measurement in camera u
+   */
+  Point2 transform(const Point2& vZp, const Point2& wZp) const;
+
+  // Accessors for the two matrices that comprise the trifocal tensor.
+  Matrix2 mat0() const { return matrix0_; }
+  Matrix2 mat1() const { return matrix1_; }
+};
+
+}  // namespace gtsam

gtsam/geometry/tests/testTrifocalTensor2.cpp

@@ -0,0 +1,139 @@
+/* ----------------------------------------------------------------------------
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testTrifocalTensor2.cpp
+ * @brief   Tests for the trifocal tensor class.
+ * @author  Zhaodong Yang
+ * @author  Akshay Krishnan
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/geometry/BearingRange.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/TrifocalTensor2.h>
+
+#include <vector>
+
+using namespace std::placeholders;
+using namespace std;
+using namespace gtsam;
+
+namespace trifocal {
+
+struct TrifocalTestData {
+  vector<Pose2> gt_poses;
+  vector<Point2> gt_landmarks;
+
+  // Outer vector over poses.
+  std::vector<vector<Rot2>> measurements;
+};
+
+TrifocalTestData getTestData() {
+  TrifocalTestData data;
+
+  // Poses
+  data.gt_poses.emplace_back(0, 0, 0);
+  data.gt_poses.emplace_back(-1.9, 4, -2 * acos(0.0) / 8);
+  data.gt_poses.emplace_back(2.1, -2.1, 2 * acos(0.0) / 3);
+
+  // Landmarks
+  data.gt_landmarks.emplace_back(1.2, 1.0);
+  data.gt_landmarks.emplace_back(2.4, 3.5);
+  data.gt_landmarks.emplace_back(-1.0, 0.5);
+  data.gt_landmarks.emplace_back(3.4, -1.5);
+  data.gt_landmarks.emplace_back(5.1, 0.6);
+  data.gt_landmarks.emplace_back(-0.1, -0.7);
+  data.gt_landmarks.emplace_back(3.1, 1.9);
+
+  // Measurements
+  for (const Pose2& pose : data.gt_poses) {
+    std::vector<Rot2> measurements;
+    for (const Point2& landmark : data.gt_landmarks) {
+      measurements.push_back(pose.bearing(landmark));
+    }
+    data.measurements.push_back(measurements);
+  }
+  return data;
+}
+
+}  // namespace trifocal
+
+// Check transform() correctly transforms measurements from 2 views to third.
+TEST(TrifocalTensor2, transform) {
+  trifocal::TrifocalTestData data = trifocal::getTestData();
+
+  // calculate trifocal tensor
+  TrifocalTensor2 T = TrifocalTensor2::FromBearingMeasurements(
+      data.measurements[0], data.measurements[1], data.measurements[2]);
+
+  // estimate measurement of a robot from the measurements of the other two
+  // robots
+  for (unsigned int i = 0; i < data.measurements[0].size(); i++) {
+    const Rot2 actual_measurement =
+        T.transform(data.measurements[1][i], data.measurements[2][i]);
+
+    // there might be two solutions for u1 and u2, comparing the ratio instead
+    // of both cos and sin
+    EXPECT(assert_equal(actual_measurement.c() * data.measurements[0][i].s(),
+                        actual_measurement.s() * data.measurements[0][i].c(),
+                        1e-8));
+  }
+}
+
+// Check the correct tensor is computed from measurements (catch regressions).
+TEST(TrifocalTensor2, tensorRegression) {
+  trifocal::TrifocalTestData data = trifocal::getTestData();
+
+  // calculate trifocal tensor
+  TrifocalTensor2 T = TrifocalTensor2::FromBearingMeasurements(
+      data.measurements[0], data.measurements[1], data.measurements[2]);
+
+  Matrix2 expected_tensor_mat0, expected_tensor_mat1;
+  // These values were obtained from a numpy-based python implementation.
+  expected_tensor_mat0 << -0.16301732 -0.1968196, -0.6082839  -0.10324949;
+  expected_tensor_mat1 << 0.45758469 -0.36310941, 0.30334159 -0.34751881;
+
+  EXPECT(assert_equal(T.mat0(), expected_tensor_mat0, 1e-2));
+  EXPECT(assert_equal(T.mat1(), expected_tensor_mat1, 1e-2));
+}
+
+// Check the calculation of Jacobian (Ground-true Jacobian comes from Auto-Grad
+// result of Pytorch)
+TEST(TrifocalTensor2, Jacobian) {
+  trifocal::TrifocalTestData data = trifocal::getTestData();
+
+  // Construct trifocal tensor using 2 rotations and 3 bearing measurements in 3
+  // cameras.
+  std::vector<Rot2> trifocal_in_angle;
+  trifocal_in_angle.insert(
+      trifocal_in_angle.end(),
+      {-0.39269908169872414, 1.0471975511965976, 2.014244663214635,
+       -0.7853981633974483, -0.5976990577022983});
+
+  // calculate trifocal tensor
+  TrifocalTensor2 T(trifocal_in_angle);
+
+  // Calculate Jacobian matrix
+  Matrix jacobian_of_trifocal = T.Jacobian(
+      data.measurements[0], data.measurements[1], data.measurements[2]);
+  // These values were obtained from a Pytorch-based python implementation.
+  Matrix expected_jacobian(7, 5) << -2.2003, 0.7050, 0.9689, 0.6296, -3.1280,
+      -4.6886, 1.1274, 2.7912, 1.6121, -5.1817, -0.7223, -0.6841, 0.5387,
+      0.7208, -0.5677, -0.8645, 0.1767, 0.5967, 0.9383, -2.2041, -3.0437,
+      0.5239, 2.0144, 1.6368, -4.0335, -1.9855, -0.2741, 1.4741, 0.6783,
+      -0.9262, -4.6600, 0.7275, 2.8182, 1.9639, -5.5489;
+
+  EXPECT(assert_equal(jacobian_of_trifocal, expected_jacobian, 1e-8));
+}
+
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}