Add planar trifocal tensor

gtsam/geometry/TrifocalTensor2.cpp

@@ -0,0 +1,94 @@
+/* ----------------------------------------------------------------------------
+ * 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>
+
+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(const std::vector<Rot2>& bearings_u,
+                                 const std::vector<Rot2>& bearings_v,
+                                 const std::vector<Rot2>& bearings_w)
+    : TrifocalTensor2(convertToProjective(bearings_u),
+                      convertToProjective(bearings_v),
+                      convertToProjective(bearings_w)) {}
+
+// Construct from 8 bearing measurements expressed in projective coordinates.
+TrifocalTensor2::TrifocalTensor2(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);
+
+  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);
+    }
+  }
+}
+
+// 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,91 @@
+/* ----------------------------------------------------------------------------
+ * 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);
+
+  /**
+   * @brief Constructor using 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.
+   */
+  TrifocalTensor2(const std::vector<Rot2>& bearings_u,
+                  const std::vector<Rot2>& bearings_v,
+                  const std::vector<Rot2>& bearings_w);
+
+  /**
+   * @brief Constructor using 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.
+   */
+  TrifocalTensor2(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,128 @@
+#include <iostream>
+#include <vector>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/geometry/BearingRange.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/TrifocalTensor2.h>
+
+using namespace std::placeholders;
+using namespace std;
+using namespace gtsam;
+
+TEST(TrifocalTensor2, transform) {
+  // 2D robots poses
+  Pose2 u = Pose2(0, 0, 0);
+  Pose2 v = Pose2(1.0, 2.0, 3.1415926 / 6);
+  Pose2 w = Pose2(-0.5, 0.5, -3.1415926 / 18);
+
+  // 2D landmarks
+  vector<Point2> landmarks;
+  landmarks.push_back(Point2(2.0, 0.5));
+  landmarks.push_back(Point2(-0.8, 2.4));
+  landmarks.push_back(Point2(1.9, -0.4));
+  landmarks.push_back(Point2(2.3, 1.0));
+  landmarks.push_back(Point2(-0.4, -0.4));
+  landmarks.push_back(Point2(-3.2, -1.0));
+  landmarks.push_back(Point2(1.5, 2.0));
+
+  // getting bearing measurement from landmarks
+  vector<Rot2> measurement_u, measurement_v, measurement_w;
+  for (int i = 0; i < landmarks.size(); ++i) {
+    measurement_u.push_back(u.bearing(landmarks[i]));
+    measurement_v.push_back(v.bearing(landmarks[i]));
+    measurement_w.push_back(w.bearing(landmarks[i]));
+  }
+
+  // calculate trifocal tensor
+  TrifocalTensor2 T(measurement_u, measurement_v, measurement_w);
+
+  // estimate measurement of a robot from the measurements of the other two
+  // robots
+  for (unsigned int i = 0; i < measurement_u.size(); i++) {
+    const Rot2 actual_measurement_u =
+        T.transform(measurement_v[i], measurement_w[i]);
+
+    // there might be two solutions for u1 and u2, comparing the ratio instead
+    // of both cos and sin
+    EXPECT(assert_equal(actual_measurement_u.c() * measurement_u[i].s(),
+                        actual_measurement_u.s() * measurement_u[i].c(), 1e-8));
+  }
+}
+
+TEST(TrifocalTensor2, mat0) {
+  Pose2 u = Pose2(0, 0, 0);
+  Pose2 v = Pose2(1.0, 2.0, 3.1415926 / 6);
+  Pose2 w = Pose2(-0.5, 0.5, -3.1415926 / 18);
+
+  // 2D landmarks
+  vector<Point2> landmarks;
+  landmarks.push_back(Point2(2.0, 0.5));
+  landmarks.push_back(Point2(-0.8, 2.4));
+  landmarks.push_back(Point2(1.9, -0.4));
+  landmarks.push_back(Point2(2.3, 1.0));
+  landmarks.push_back(Point2(-0.4, -0.4));
+  landmarks.push_back(Point2(-3.2, -1.0));
+  landmarks.push_back(Point2(1.5, 2.0));
+
+  // getting bearing measurement from landmarks
+  vector<Rot2> measurement_u, measurement_v, measurement_w;
+  for (unsigned int i = 0; i < landmarks.size(); ++i) {
+    measurement_u.push_back(u.bearing(landmarks[i]));
+    measurement_v.push_back(v.bearing(landmarks[i]));
+    measurement_w.push_back(w.bearing(landmarks[i]));
+  }
+
+  // calculate trifocal tensor
+  TrifocalTensor2 T(measurement_u, measurement_v, measurement_w);
+
+  Matrix2 actual_trifocal_tensor_mat0 = T.mat0();
+
+  Matrix2 exp_trifocal_tensor_mat0;
+  exp_trifocal_tensor_mat0 << -0.13178263, 0.29210566, -0.00860471, -0.73975238;
+
+  EXPECT(assert_equal(actual_trifocal_tensor_mat0, exp_trifocal_tensor_mat0,
+                      1e-2));
+}
+
+TEST(TrifocalTensor2, mat1) {
+  Pose2 u = Pose2(0, 0, 0);
+  Pose2 v = Pose2(1.0, 2.0, 3.1415926 / 6);
+  Pose2 w = Pose2(-0.5, 0.5, -3.1415926 / 18);
+
+  // 2D landmarks
+  vector<Point2> landmarks;
+  landmarks.push_back(Point2(2.0, 0.5));
+  landmarks.push_back(Point2(-0.8, 2.4));
+  landmarks.push_back(Point2(1.9, -0.4));
+  landmarks.push_back(Point2(2.3, 1.0));
+  landmarks.push_back(Point2(-0.4, -0.4));
+  landmarks.push_back(Point2(-3.2, -1.0));
+  landmarks.push_back(Point2(1.5, 2.0));
+
+  // getting bearing measurement from landmarks
+  vector<Rot2> measurement_u, measurement_v, measurement_w;
+  for (unsigned int i = 0; i < landmarks.size(); ++i) {
+    measurement_u.push_back(u.bearing(landmarks[i]));
+    measurement_v.push_back(v.bearing(landmarks[i]));
+    measurement_w.push_back(w.bearing(landmarks[i]));
+  }
+
+  // calculate trifocal tensor
+  TrifocalTensor2 T(measurement_u, measurement_v, measurement_w);
+
+  Matrix2 actual_trifocal_tensor_mat0 = T.mat1();
+
+  Matrix2 exp_trifocal_tensor_mat0;
+  exp_trifocal_tensor_mat0 << -0.27261704, 0.09097327, 0.51699647, 0.0108839;
+
+  EXPECT(assert_equal(actual_trifocal_tensor_mat0, exp_trifocal_tensor_mat0,
+                      1e-2));
+}
+
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}