diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp new file mode 100644 index 0000000000..854569670b --- /dev/null +++ b/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 + +#include +#include + +namespace gtsam { + +// Convert bearing measurements to projective coordinates. +std::vector convertToProjective(const std::vector& rotations) { + std::vector 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& bearings_u, const std::vector& bearings_v, + const std::vector& 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& u, const std::vector& v, + const std::vector& 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 \ No newline at end of file diff --git a/gtsam/geometry/TrifocalTensor2.h b/gtsam/geometry/TrifocalTensor2.h new file mode 100644 index 0000000000..1c4e947454 --- /dev/null +++ b/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 +#include +#include + +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& bearings_u, const std::vector& bearings_v, + const std::vector& 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& u, const std::vector& v, + const std::vector& 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 diff --git a/gtsam/geometry/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp new file mode 100644 index 0000000000..ed7cd252e3 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#include + +using namespace std::placeholders; +using namespace std; +using namespace gtsam; + +namespace trifocal { + +struct TrifocalTestData { + vector gt_poses; + vector gt_landmarks; + + // Outer vector over poses. + std::vector> 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 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 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); +}