From 049e5d736e20826aa189931f67beaf5597bab405 Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang Date: Tue, 1 Feb 2022 02:09:08 -0500 Subject: [PATCH 01/25] Planar Trifocal Tensor --- .../tests/testCal2DTrifocalTensor.cpp | 56 +++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 gtsam/geometry/tests/testCal2DTrifocalTensor.cpp diff --git a/gtsam/geometry/tests/testCal2DTrifocalTensor.cpp b/gtsam/geometry/tests/testCal2DTrifocalTensor.cpp new file mode 100644 index 0000000000..3fe7b29ba1 --- /dev/null +++ b/gtsam/geometry/tests/testCal2DTrifocalTensor.cpp @@ -0,0 +1,56 @@ +#include +#include +#include +#include + +#include + +using namespace std::placeholders; +using namespace std; +using namespace gtsam; + +typedef BearingRange BearingRange2D; + +GTSAM_CONCEPT_TESTABLE_INST(TrifocalTensor2D) +GTSAM_CONCEPT_MANIFOLD_INST(TrifocalTensor2D) + +TEST(TrifocalTensor2D, get_estimate_measurement) { + // 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 / 10); + + // 2D landmarks + vector 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)); + + /* + * the example + * D:\Gatech\research\3DReconstruction\GTSAM\gtsam\examples\Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp + * uses class graph. I don't have enough time to read code about that now. + * Maybe I'll do it later. + */ + + // getting bearing measurement from landmarks + vector 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 + TrifocalTensor2D T(measurement_u, measurement_v, measurement_w); + + // estimate measurement of a robot from the measurements of the other two + // robots + vector exp_measurement_u; + exp_measurement_u = T.get_estimate_measurement(measurement_v, measurement_w); + EXPECT(assert_equal(exp_measurement_u, measurement_u, 1e-8)); +} \ No newline at end of file From a0b4dab65cf3a3bdf8ab91141320a18d4e6a1d6b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Fri, 4 Feb 2022 00:26:39 -0800 Subject: [PATCH 02/25] Add header file, API --- gtsam/geometry/TrifocalTensor2.h | 42 ++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 gtsam/geometry/TrifocalTensor2.h diff --git a/gtsam/geometry/TrifocalTensor2.h b/gtsam/geometry/TrifocalTensor2.h new file mode 100644 index 0000000000..9af50827b6 --- /dev/null +++ b/gtsam/geometry/TrifocalTensor2.h @@ -0,0 +1,42 @@ +/* ---------------------------------------------------------------------------- + * 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 + +namespace gtsam { + +class TrifocalTensor2 { + private: + Matrix2 matrix0_, matrix1_; + + public: + TrifocalTensor2() {} + + TrifocalTensor2(const Matrix2& matrix0, const Matrix2& matrix1); + + TrifocalTensor2(const std::vector& bearings_u, + const std::vector& bearings_v, + const std::vector& bearings_w) const; + + Rot2 transform(const Rot2& vZp, const Rot2& wZp) const; + + Point2 transform(const Point2& vZp, const Point2& wZp) const; +}; + +} // namespace gtsam \ No newline at end of file From e2e3e40b6c02cb64fa915daa2272fcddb0ef1013 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Fri, 4 Feb 2022 00:27:42 -0800 Subject: [PATCH 03/25] rename test file --- .../{testCal2DTrifocalTensor.cpp => testTrifocalTensor2.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename gtsam/geometry/tests/{testCal2DTrifocalTensor.cpp => testTrifocalTensor2.cpp} (100%) diff --git a/gtsam/geometry/tests/testCal2DTrifocalTensor.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp similarity index 100% rename from gtsam/geometry/tests/testCal2DTrifocalTensor.cpp rename to gtsam/geometry/tests/testTrifocalTensor2.cpp From 719135ce538384788c6cf5d3b230562c7ef2e882 Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang <93161408+Hal-Zhaodong-Yang@users.noreply.github.com> Date: Fri, 4 Feb 2022 19:08:33 -0500 Subject: [PATCH 04/25] add trifocal tensor class cpp file add trifocal tensor constructor from bearing measurements --- gtsam/geometry/TrifocalTensor2.cpp | 34 ++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 gtsam/geometry/TrifocalTensor2.cpp diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp new file mode 100644 index 0000000000..aa0588bc23 --- /dev/null +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -0,0 +1,34 @@ + + +#include + +#include + +namespace gtsam { +TrifocalTensor2::TrifocalTensor2(const std::vector& bearings_u, + const std::vector& bearings_v, + const std::vector& bearings_w) { + int i = 0; + gtsam::Matrix(bearing_u.size(), 8) system_matrix; + for (int i = 0; i < bearing_u.size(); i++) { + system_matrix(i, 0) = bearings_u.c() * bearings_v.c() * bearings_w.c(); + system_matrix(i, 1) = bearings_u.c() * bearings_v.c() * bearings_w.s(); + system_matrix(i, 2) = bearings_u.c() * bearings_v.s() * bearings_w.c(); + system_matrix(i, 3) = bearings_u.c() * bearings_v.s() * bearings_w.s(); + system_matrix(i, 4) = bearings_u.s() * bearings_v.c() * bearings_w.c(); + system_matrix(i, 5) = bearings_u.s() * bearings_v.c() * bearings_w.s(); + system_matrix(i, 6) = bearings_u.s() * bearings_v.s() * bearings_w.c(); + system_matrix(i, 7) = bearings_u.s() * bearings_v.s() * bearings_w.s(); + } + + gtsam::Matrix U, S, V; + svd(system_matrix, U, S, V); + gtsam::Vector8 V_last_column = column(V, V.cols() - 1); + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + matrix0_(i, j) = V_last_column(2 * i + j); + matrix1_(i, j) = V_last_column(2 * i + j + 4); + } + } +} +} // namespace gtsam \ No newline at end of file From b97fa7aaf4d3ed5e4a591a0a321b43012ba69b93 Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang <93161408+Hal-Zhaodong-Yang@users.noreply.github.com> Date: Fri, 4 Feb 2022 22:53:31 -0500 Subject: [PATCH 05/25] add transform --- gtsam/geometry/TrifocalTensor2.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp index aa0588bc23..370aee81ed 100644 --- a/gtsam/geometry/TrifocalTensor2.cpp +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -31,4 +31,14 @@ TrifocalTensor2::TrifocalTensor2(const std::vector& bearings_u, } } } + +Rot2 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(); + uZp = Rot2.atan(trans(v_measurement) * matrix0_ * w_measurement, - trans(v_measurement) * matrix1_ * w_measurement); + return uZp; + +} } // namespace gtsam \ No newline at end of file From 87bc39cf943953fca6a06199b1fd38a0916c06cf Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang Date: Fri, 4 Feb 2022 23:52:47 -0500 Subject: [PATCH 06/25] change constructor from const to non-const --- gtsam/geometry/TrifocalTensor2.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/TrifocalTensor2.h b/gtsam/geometry/TrifocalTensor2.h index 9af50827b6..8973303f5e 100644 --- a/gtsam/geometry/TrifocalTensor2.h +++ b/gtsam/geometry/TrifocalTensor2.h @@ -32,11 +32,11 @@ class TrifocalTensor2 { TrifocalTensor2(const std::vector& bearings_u, const std::vector& bearings_v, - const std::vector& bearings_w) const; + const std::vector& bearings_w); Rot2 transform(const Rot2& vZp, const Rot2& wZp) const; Point2 transform(const Point2& vZp, const Point2& wZp) const; }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam From 92d60a0aafdea34155a836c31e9c95dc88ce8520 Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang Date: Sat, 5 Feb 2022 00:21:55 -0500 Subject: [PATCH 07/25] fixed some bugs --- gtsam/geometry/TrifocalTensor2.cpp | 36 +++++++++++++++--------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp index 370aee81ed..6154ec265f 100644 --- a/gtsam/geometry/TrifocalTensor2.cpp +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -9,16 +9,16 @@ TrifocalTensor2::TrifocalTensor2(const std::vector& bearings_u, const std::vector& bearings_v, const std::vector& bearings_w) { int i = 0; - gtsam::Matrix(bearing_u.size(), 8) system_matrix; - for (int i = 0; i < bearing_u.size(); i++) { - system_matrix(i, 0) = bearings_u.c() * bearings_v.c() * bearings_w.c(); - system_matrix(i, 1) = bearings_u.c() * bearings_v.c() * bearings_w.s(); - system_matrix(i, 2) = bearings_u.c() * bearings_v.s() * bearings_w.c(); - system_matrix(i, 3) = bearings_u.c() * bearings_v.s() * bearings_w.s(); - system_matrix(i, 4) = bearings_u.s() * bearings_v.c() * bearings_w.c(); - system_matrix(i, 5) = bearings_u.s() * bearings_v.c() * bearings_w.s(); - system_matrix(i, 6) = bearings_u.s() * bearings_v.s() * bearings_w.c(); - system_matrix(i, 7) = bearings_u.s() * bearings_v.s() * bearings_w.s(); + Matrix system_matrix(bearings_u.size(), 8); + for (int i = 0; i < bearings_u.size(); i++) { + system_matrix(i, 0) = bearings_u[i].c() * bearings_v[i].c() * bearings_w[i].c(); + system_matrix(i, 1) = bearings_u[i].c() * bearings_v[i].c() * bearings_w[i].s(); + system_matrix(i, 2) = bearings_u[i].c() * bearings_v[i].s() * bearings_w[i].c(); + system_matrix(i, 3) = bearings_u[i].c() * bearings_v[i].s() * bearings_w[i].s(); + system_matrix(i, 4) = bearings_u[i].s() * bearings_v[i].c() * bearings_w[i].c(); + system_matrix(i, 5) = bearings_u[i].s() * bearings_v[i].c() * bearings_w[i].s(); + system_matrix(i, 6) = bearings_u[i].s() * bearings_v[i].s() * bearings_w[i].c(); + system_matrix(i, 7) = bearings_u[i].s() * bearings_v[i].s() * bearings_w[i].s(); } gtsam::Matrix U, S, V; @@ -32,13 +32,13 @@ TrifocalTensor2::TrifocalTensor2(const std::vector& bearings_u, } } -Rot2 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(); - uZp = Rot2.atan(trans(v_measurement) * matrix0_ * w_measurement, - trans(v_measurement) * matrix1_ * w_measurement); - return uZp; - +Rot2 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(); + uZp = Rot2.atan(trans(v_measurement) * matrix0_ * w_measurement, + -trans(v_measurement) * matrix1_ * w_measurement); + return uZp; } } // namespace gtsam \ No newline at end of file From e69490edbf4433f5bfa26798c982c12bdf8a27f9 Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang Date: Sat, 5 Feb 2022 00:45:27 -0500 Subject: [PATCH 08/25] fixed some bugs --- gtsam/geometry/TrifocalTensor2.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp index 6154ec265f..d200a52609 100644 --- a/gtsam/geometry/TrifocalTensor2.cpp +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -32,13 +32,13 @@ TrifocalTensor2::TrifocalTensor2(const std::vector& bearings_u, } } -Rot2 transform(const Rot2& vZp, const Rot2& wZp) const { +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(); - uZp = Rot2.atan(trans(v_measurement) * matrix0_ * w_measurement, - -trans(v_measurement) * matrix1_ * w_measurement); + uZp = Rot2.atan(trans(v_measurement) * this->matrix0_ * w_measurement, + -trans(v_measurement) * this->matrix1_ * w_measurement); return uZp; } } // namespace gtsam \ No newline at end of file From 8aaebb4c8938b55dbfa118aa458c4d7f41e44655 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 5 Feb 2022 00:57:44 -0800 Subject: [PATCH 09/25] Update implementations, builds now --- gtsam/geometry/TrifocalTensor2.cpp | 61 +++++++++++++++++++----------- gtsam/geometry/TrifocalTensor2.h | 12 +++++- 2 files changed, 49 insertions(+), 24 deletions(-) diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp index d200a52609..37648cbb6e 100644 --- a/gtsam/geometry/TrifocalTensor2.cpp +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -1,33 +1,48 @@ - - #include #include namespace gtsam { + +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; +} + TrifocalTensor2::TrifocalTensor2(const std::vector& bearings_u, const std::vector& bearings_v, - const std::vector& bearings_w) { - int i = 0; - Matrix system_matrix(bearings_u.size(), 8); - for (int i = 0; i < bearings_u.size(); i++) { - system_matrix(i, 0) = bearings_u[i].c() * bearings_v[i].c() * bearings_w[i].c(); - system_matrix(i, 1) = bearings_u[i].c() * bearings_v[i].c() * bearings_w[i].s(); - system_matrix(i, 2) = bearings_u[i].c() * bearings_v[i].s() * bearings_w[i].c(); - system_matrix(i, 3) = bearings_u[i].c() * bearings_v[i].s() * bearings_w[i].s(); - system_matrix(i, 4) = bearings_u[i].s() * bearings_v[i].c() * bearings_w[i].c(); - system_matrix(i, 5) = bearings_u[i].s() * bearings_v[i].c() * bearings_w[i].s(); - system_matrix(i, 6) = bearings_u[i].s() * bearings_v[i].s() * bearings_w[i].c(); - system_matrix(i, 7) = bearings_u[i].s() * bearings_v[i].s() * bearings_w[i].s(); - } + const std::vector& bearings_w) + : TrifocalTensor2(convertToProjective(bearings_u), + convertToProjective(bearings_v), + convertToProjective(bearings_w)) {} - gtsam::Matrix U, S, V; - svd(system_matrix, U, S, V); - gtsam::Vector8 V_last_column = column(V, V.cols() - 1); +TrifocalTensor2::TrifocalTensor2(const std::vector& u, + const std::vector& v, + const std::vector& w) { + if (u.size() != v.size() || v.size() != w.size()) { + // throw error here + } + Matrix A(u.size(), 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); + } + } + } + } + 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_last_column(2 * i + j); - matrix1_(i, j) = V_last_column(2 * i + j + 4); + matrix0_(i, j) = V(2 * i + j, V.cols() - 1); + matrix1_(i, j) = V(2 * i + j + 4, V.cols() - 1); } } } @@ -37,8 +52,8 @@ Rot2 TrifocalTensor2::transform(const Rot2& vZp, const Rot2& wZp) const { Vector2 v_measurement, w_measurement; v_measurement << vZp.c(), vZp.s(); w_measurement << wZp.c(), wZp.s(); - uZp = Rot2.atan(trans(v_measurement) * this->matrix0_ * w_measurement, - -trans(v_measurement) * this->matrix1_ * w_measurement); - return uZp; + return Rot2::atan2(-dot(matrix1_ * w_measurement, v_measurement), + dot(matrix0_ * w_measurement, v_measurement)); } + } // namespace gtsam \ No newline at end of file diff --git a/gtsam/geometry/TrifocalTensor2.h b/gtsam/geometry/TrifocalTensor2.h index 8973303f5e..91f57dd073 100644 --- a/gtsam/geometry/TrifocalTensor2.h +++ b/gtsam/geometry/TrifocalTensor2.h @@ -17,6 +17,7 @@ #pragma once #include +#include #include namespace gtsam { @@ -28,14 +29,23 @@ class TrifocalTensor2 { public: TrifocalTensor2() {} + // Construct from the two 2x2 matrices that form the tensor. TrifocalTensor2(const Matrix2& matrix0, const Matrix2& matrix1); + // Construct from 8 bearing measurements. TrifocalTensor2(const std::vector& bearings_u, const std::vector& bearings_v, const std::vector& bearings_w); - + + // Construct from 8 bearing measurements expressed in projective coordinates. + TrifocalTensor2(const std::vector& u, const std::vector& v, + const std::vector& w); + + // Finds a measurement in the first view using measurements from second and + // third views. Rot2 transform(const Rot2& vZp, const Rot2& wZp) const; + // Same as above, but using projective measurements. Point2 transform(const Point2& vZp, const Point2& wZp) const; }; From df359c0899d18962cae65ec1c7865de638c6e0a8 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 5 Feb 2022 01:33:27 -0800 Subject: [PATCH 10/25] update test --- gtsam/geometry/tests/testTrifocalTensor2.cpp | 22 ++++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp index 3fe7b29ba1..eeed3e0511 100644 --- a/gtsam/geometry/tests/testTrifocalTensor2.cpp +++ b/gtsam/geometry/tests/testTrifocalTensor2.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include @@ -11,10 +12,7 @@ using namespace gtsam; typedef BearingRange BearingRange2D; -GTSAM_CONCEPT_TESTABLE_INST(TrifocalTensor2D) -GTSAM_CONCEPT_MANIFOLD_INST(TrifocalTensor2D) - -TEST(TrifocalTensor2D, get_estimate_measurement) { +TEST(TrifocalTensor2, transform) { // 2D robots poses Pose2 u = Pose2(0, 0, 0); Pose2 v = Pose2(1.0, 2.0, 3.1415926 / 6); @@ -46,11 +44,17 @@ TEST(TrifocalTensor2D, get_estimate_measurement) { } // calculate trifocal tensor - TrifocalTensor2D T(measurement_u, measurement_v, measurement_w); + TrifocalTensor2 T(measurement_u, measurement_v, measurement_w); // estimate measurement of a robot from the measurements of the other two // robots - vector exp_measurement_u; - exp_measurement_u = T.get_estimate_measurement(measurement_v, measurement_w); - EXPECT(assert_equal(exp_measurement_u, measurement_u, 1e-8)); -} \ No newline at end of file + for (int i = 0; i < measurement_u.size(); i++) { + const Rot2 actual_measurement_u = T.transform(measurement_v[i], measurement_w[i]); + EXPECT(assert_equal(actual_measurement_u, measurement_u[i], 1e-8)); + } +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} From b073fcbe1c32332f04cba0ed5ed040b58e71a6f5 Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang Date: Sat, 5 Feb 2022 14:11:25 -0500 Subject: [PATCH 11/25] fixed some bugs --- gtsam/geometry/TrifocalTensor2.cpp | 4 ++-- gtsam/geometry/tests/testTrifocalTensor2.cpp | 13 +++++++++++-- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp index 37648cbb6e..85f45bf150 100644 --- a/gtsam/geometry/TrifocalTensor2.cpp +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -52,8 +52,8 @@ Rot2 TrifocalTensor2::transform(const Rot2& vZp, const Rot2& wZp) const { Vector2 v_measurement, w_measurement; v_measurement << vZp.c(), vZp.s(); w_measurement << wZp.c(), wZp.s(); - return Rot2::atan2(-dot(matrix1_ * w_measurement, v_measurement), - dot(matrix0_ * w_measurement, v_measurement)); + 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/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp index eeed3e0511..378c3da12b 100644 --- a/gtsam/geometry/tests/testTrifocalTensor2.cpp +++ b/gtsam/geometry/tests/testTrifocalTensor2.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include @@ -49,8 +50,16 @@ TEST(TrifocalTensor2, transform) { // estimate measurement of a robot from the measurements of the other two // robots for (int i = 0; i < measurement_u.size(); i++) { - const Rot2 actual_measurement_u = T.transform(measurement_v[i], measurement_w[i]); - EXPECT(assert_equal(actual_measurement_u, measurement_u[i], 1e-8)); + const Rot2 actual_measurement_u = + T.transform(measurement_v[i], measurement_w[i]); + cout << "the ground truth: " << measurement_u[i].c() << " " + << measurement_u[i].s() + << "; the estimate: " << actual_measurement_u.c() << " " + << actual_measurement_u.s() << "\n"; + + // 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-2)); } } From f6d8cf5cb515cc4785b7b9ba54af85dbe8dec941 Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang Date: Sat, 5 Feb 2022 22:17:57 -0500 Subject: [PATCH 12/25] Fixed the bugs and now the tests for trifocal tensor passed --- gtsam/geometry/TrifocalTensor2.cpp | 28 +++++- gtsam/geometry/TrifocalTensor2.h | 4 + gtsam/geometry/tests/testTrifocalTensor2.cpp | 93 +++++++++++++++++++- 3 files changed, 121 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp index 85f45bf150..c52cc2a8ab 100644 --- a/gtsam/geometry/TrifocalTensor2.cpp +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -26,19 +26,42 @@ TrifocalTensor2::TrifocalTensor2(const std::vector& u, if (u.size() != v.size() || v.size() != w.size()) { // throw error here } - Matrix A(u.size(), 8); + /* + std::cout << "u, v, w:" << std::endl; + for (int row = 0; row < u.size(); row++) { + std::cout << u[row][0] << " " << u[row][1] << " " << v[row][0] << " " + << v[row][1] << " " << w[row][0] << " " << w[row][1] << std::endl; + }*/ + Matrix A(u.size() > 8? u.size():8, 8); + std::cout << "system matrix:" << std::endl; 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); + std::cout << A(row, 4 * i + 2 * j + k) << " "; } } } + std::cout << std::endl; + } + for (int row = u.size() - 8; row < 0; row ++){ + for (int col = 0; col < 8; col ++){ + A(row, col) = 0; + std::cout << A(row, col) << " "; + } + std::cout << std::endl; } Matrix U, V; Vector S; svd(A, U, S, V); + std::cout << "V:" << std::endl; + for (int i = 0; i < V.rows(); i++) { + for (int j = 0; j < V.cols(); j++) { + std::cout << V(i, j) << " "; + } + std::cout << std::endl; + } for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { matrix0_(i, j) = V(2 * i + j, V.cols() - 1); @@ -56,4 +79,7 @@ Rot2 TrifocalTensor2::transform(const Rot2& vZp, const Rot2& wZp) const { -dot(matrix1_ * w_measurement, v_measurement)); } +Matrix2 TrifocalTensor2::mat0() const { return matrix0_; } +Matrix2 TrifocalTensor2::mat1() const { return matrix1_; } + } // namespace gtsam \ No newline at end of file diff --git a/gtsam/geometry/TrifocalTensor2.h b/gtsam/geometry/TrifocalTensor2.h index 91f57dd073..7617e5b136 100644 --- a/gtsam/geometry/TrifocalTensor2.h +++ b/gtsam/geometry/TrifocalTensor2.h @@ -47,6 +47,10 @@ class TrifocalTensor2 { // Same as above, but using projective measurements. Point2 transform(const Point2& vZp, const Point2& wZp) const; + + // Get trifocal tensor matrix from class + Matrix2 mat0() const; + Matrix2 mat1() const; }; } // namespace gtsam diff --git a/gtsam/geometry/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp index 378c3da12b..352629d0d0 100644 --- a/gtsam/geometry/tests/testTrifocalTensor2.cpp +++ b/gtsam/geometry/tests/testTrifocalTensor2.cpp @@ -17,7 +17,7 @@ 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 / 10); + Pose2 w = Pose2(-0.5, 0.5, -3.1415926 / 18); // 2D landmarks vector landmarks; @@ -44,6 +44,8 @@ TEST(TrifocalTensor2, transform) { measurement_w.push_back(w.bearing(landmarks[i])); } + + // calculate trifocal tensor TrifocalTensor2 T(measurement_u, measurement_v, measurement_w); @@ -57,10 +59,95 @@ TEST(TrifocalTensor2, transform) { << "; the estimate: " << actual_measurement_u.c() << " " << actual_measurement_u.s() << "\n"; - // there might be two solutions for u1 and u2, comparing the ratio instead of both cos and sin + // 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-2)); + 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 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)); + + /* + * the example + * D:\Gatech\research\3DReconstruction\GTSAM\gtsam\examples\Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp + * uses class graph. I don't have enough time to read code about that now. + * Maybe I'll do it later. + */ + + // getting bearing measurement from landmarks + vector 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); + + 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 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)); + + /* + * the example + * D:\Gatech\research\3DReconstruction\GTSAM\gtsam\examples\Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp + * uses class graph. I don't have enough time to read code about that now. + * Maybe I'll do it later. + */ + + // getting bearing measurement from landmarks + vector 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); + + 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() { From 98bdb493ea762b851f7d39f94a507a91a54cadd6 Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang Date: Tue, 8 Feb 2022 23:22:48 -0500 Subject: [PATCH 13/25] Changed some format and added some comments --- gtsam/geometry/TrifocalTensor2.cpp | 35 ++++++++++++++++++++---------- 1 file changed, 24 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp index c52cc2a8ab..6206fa0b32 100644 --- a/gtsam/geometry/TrifocalTensor2.cpp +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -1,9 +1,23 @@ -#include +/* ---------------------------------------------------------------------------- + * 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 namespace gtsam { - +// Convert bearing measurements to projective form std::vector convertToProjective(const std::vector& rotations) { std::vector projectives; projectives.reserve(rotations.size()); @@ -13,6 +27,7 @@ std::vector convertToProjective(const std::vector& rotations) { return projectives; } +// Construct from 8 bearing measurements. TrifocalTensor2::TrifocalTensor2(const std::vector& bearings_u, const std::vector& bearings_v, const std::vector& bearings_w) @@ -20,19 +35,15 @@ TrifocalTensor2::TrifocalTensor2(const std::vector& bearings_u, convertToProjective(bearings_v), convertToProjective(bearings_w)) {} +// Construct from 8 bearing measurements expressed in projective coordinates. TrifocalTensor2::TrifocalTensor2(const std::vector& u, const std::vector& v, const std::vector& w) { if (u.size() != v.size() || v.size() != w.size()) { // throw error here } - /* - std::cout << "u, v, w:" << std::endl; - for (int row = 0; row < u.size(); row++) { - std::cout << u[row][0] << " " << u[row][1] << " " << v[row][0] << " " - << v[row][1] << " " << w[row][0] << " " << w[row][1] << std::endl; - }*/ - Matrix A(u.size() > 8? u.size():8, 8); + + Matrix A(u.size() > 8 ? u.size() : 8, 8); std::cout << "system matrix:" << std::endl; for (int row = 0; row < u.size(); row++) { for (int i = 0; i < 2; i++) { @@ -45,8 +56,8 @@ TrifocalTensor2::TrifocalTensor2(const std::vector& u, } std::cout << std::endl; } - for (int row = u.size() - 8; row < 0; row ++){ - for (int col = 0; col < 8; col ++){ + for (int row = u.size() - 8; row < 0; row++) { + for (int col = 0; col < 8; col++) { A(row, col) = 0; std::cout << A(row, col) << " "; } @@ -70,6 +81,8 @@ TrifocalTensor2::TrifocalTensor2(const std::vector& u, } } +// 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; From 3d26c56ba86e1919043702a5c2900c546059985e Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang Date: Wed, 9 Feb 2022 00:03:20 -0500 Subject: [PATCH 14/25] Changed some format and added some comments --- gtsam/geometry/tests/testTrifocalTensor2.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/gtsam/geometry/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp index 352629d0d0..9bf4e17611 100644 --- a/gtsam/geometry/tests/testTrifocalTensor2.cpp +++ b/gtsam/geometry/tests/testTrifocalTensor2.cpp @@ -11,8 +11,6 @@ using namespace std::placeholders; using namespace std; using namespace gtsam; -typedef BearingRange BearingRange2D; - TEST(TrifocalTensor2, transform) { // 2D robots poses Pose2 u = Pose2(0, 0, 0); @@ -44,8 +42,6 @@ TEST(TrifocalTensor2, transform) { measurement_w.push_back(w.bearing(landmarks[i])); } - - // calculate trifocal tensor TrifocalTensor2 T(measurement_u, measurement_v, measurement_w); From 27430313fbe56b591815d5805da4ec477280a4dc Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang Date: Wed, 9 Feb 2022 01:13:39 -0500 Subject: [PATCH 15/25] Changed some comments --- gtsam/geometry/tests/testTrifocalTensor2.cpp | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp index 9bf4e17611..b82c659d3c 100644 --- a/gtsam/geometry/tests/testTrifocalTensor2.cpp +++ b/gtsam/geometry/tests/testTrifocalTensor2.cpp @@ -27,13 +27,6 @@ TEST(TrifocalTensor2, transform) { landmarks.push_back(Point2(-3.2, -1.0)); landmarks.push_back(Point2(1.5, 2.0)); - /* - * the example - * D:\Gatech\research\3DReconstruction\GTSAM\gtsam\examples\Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp - * uses class graph. I don't have enough time to read code about that now. - * Maybe I'll do it later. - */ - // getting bearing measurement from landmarks vector measurement_u, measurement_v, measurement_w; for (int i = 0; i < landmarks.size(); ++i) { @@ -47,7 +40,7 @@ TEST(TrifocalTensor2, transform) { // estimate measurement of a robot from the measurements of the other two // robots - for (int i = 0; i < measurement_u.size(); i++) { + for (unsigned int i = 0; i < measurement_u.size(); i++) { const Rot2 actual_measurement_u = T.transform(measurement_v[i], measurement_w[i]); cout << "the ground truth: " << measurement_u[i].c() << " " @@ -86,7 +79,7 @@ TEST(TrifocalTensor2, mat0) { // getting bearing measurement from landmarks vector measurement_u, measurement_v, measurement_w; - for (int i = 0; i < landmarks.size(); ++i) { + 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])); @@ -128,7 +121,7 @@ TEST(TrifocalTensor2, mat1) { // getting bearing measurement from landmarks vector measurement_u, measurement_v, measurement_w; - for (int i = 0; i < landmarks.size(); ++i) { + 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])); From 9eaaa0cdc63940bd3fd26df0f709205e2a088b76 Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang Date: Wed, 9 Feb 2022 01:17:30 -0500 Subject: [PATCH 16/25] Changed some comments --- gtsam/geometry/tests/testTrifocalTensor2.cpp | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/gtsam/geometry/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp index b82c659d3c..3a348f73e2 100644 --- a/gtsam/geometry/tests/testTrifocalTensor2.cpp +++ b/gtsam/geometry/tests/testTrifocalTensor2.cpp @@ -70,13 +70,6 @@ TEST(TrifocalTensor2, mat0) { landmarks.push_back(Point2(-3.2, -1.0)); landmarks.push_back(Point2(1.5, 2.0)); - /* - * the example - * D:\Gatech\research\3DReconstruction\GTSAM\gtsam\examples\Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp - * uses class graph. I don't have enough time to read code about that now. - * Maybe I'll do it later. - */ - // getting bearing measurement from landmarks vector measurement_u, measurement_v, measurement_w; for (unsigned int i = 0; i < landmarks.size(); ++i) { @@ -112,13 +105,6 @@ TEST(TrifocalTensor2, mat1) { landmarks.push_back(Point2(-3.2, -1.0)); landmarks.push_back(Point2(1.5, 2.0)); - /* - * the example - * D:\Gatech\research\3DReconstruction\GTSAM\gtsam\examples\Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp - * uses class graph. I don't have enough time to read code about that now. - * Maybe I'll do it later. - */ - // getting bearing measurement from landmarks vector measurement_u, measurement_v, measurement_w; for (unsigned int i = 0; i < landmarks.size(); ++i) { From 0f0096da754827a0ef167ee9c34b72ef2511eb02 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 8 Feb 2022 22:41:14 -0800 Subject: [PATCH 17/25] update documentation, move accessor definitions to header --- gtsam/geometry/TrifocalTensor2.cpp | 7 ++-- gtsam/geometry/TrifocalTensor2.h | 51 +++++++++++++++++++++++++----- 2 files changed, 46 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp index 6206fa0b32..508f54655f 100644 --- a/gtsam/geometry/TrifocalTensor2.cpp +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -14,10 +14,12 @@ */ #include + #include namespace gtsam { -// Convert bearing measurements to projective form + +// Convert bearing measurements to projective coordinates. std::vector convertToProjective(const std::vector& rotations) { std::vector projectives; projectives.reserve(rotations.size()); @@ -92,7 +94,4 @@ Rot2 TrifocalTensor2::transform(const Rot2& vZp, const Rot2& wZp) const { -dot(matrix1_ * w_measurement, v_measurement)); } -Matrix2 TrifocalTensor2::mat0() const { return matrix0_; } -Matrix2 TrifocalTensor2::mat1() const { return matrix1_; } - } // namespace gtsam \ No newline at end of file diff --git a/gtsam/geometry/TrifocalTensor2.h b/gtsam/geometry/TrifocalTensor2.h index 7617e5b136..a6f7a464c3 100644 --- a/gtsam/geometry/TrifocalTensor2.h +++ b/gtsam/geometry/TrifocalTensor2.h @@ -22,8 +22,16 @@ 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: @@ -32,25 +40,52 @@ class TrifocalTensor2 { // Construct from the two 2x2 matrices that form the tensor. TrifocalTensor2(const Matrix2& matrix0, const Matrix2& matrix1); - // Construct from 8 bearing measurements. + /** + * @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& bearings_u, const std::vector& bearings_v, const std::vector& bearings_w); - // Construct from 8 bearing measurements expressed in projective coordinates. + /** + * @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& u, const std::vector& v, const std::vector& w); - // Finds a measurement in the first view using measurements from second and - // third views. + /** + * @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; - // Same as above, but using projective measurements. + /** + * @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; - // Get trifocal tensor matrix from class - Matrix2 mat0() const; - Matrix2 mat1() const; + // Accessors for the two matrices that comprise the trifocal tensor. + Matrix2 mat0() const { return matrix0_; } + Matrix2 mat1() const { return matrix1_; } }; } // namespace gtsam From 0df983674ed5b95938ab4cf31e2bc89fdb143d1a Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 8 Feb 2022 22:58:07 -0800 Subject: [PATCH 18/25] throw invalid arg exception --- gtsam/geometry/TrifocalTensor2.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp index 508f54655f..255d621d83 100644 --- a/gtsam/geometry/TrifocalTensor2.cpp +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -15,7 +15,7 @@ #include -#include +#include namespace gtsam { @@ -41,10 +41,15 @@ TrifocalTensor2::TrifocalTensor2(const std::vector& bearings_u, TrifocalTensor2::TrifocalTensor2(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 error here + throw std::invalid_argument( + "Number of input measurements in 3 cameras must be same"); } - + Matrix A(u.size() > 8 ? u.size() : 8, 8); std::cout << "system matrix:" << std::endl; for (int row = 0; row < u.size(); row++) { From a516b5f401b508539f10050a927ee3adca574aea Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 8 Feb 2022 23:02:42 -0800 Subject: [PATCH 19/25] remove debug print statements --- gtsam/geometry/TrifocalTensor2.cpp | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp index 255d621d83..dcf48fb504 100644 --- a/gtsam/geometry/TrifocalTensor2.cpp +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -50,36 +50,28 @@ TrifocalTensor2::TrifocalTensor2(const std::vector& u, "Number of input measurements in 3 cameras must be same"); } + // Create the system matrix A. Matrix A(u.size() > 8 ? u.size() : 8, 8); - std::cout << "system matrix:" << std::endl; 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); - std::cout << A(row, 4 * i + 2 * j + k) << " "; } } } - std::cout << std::endl; } for (int row = u.size() - 8; row < 0; row++) { for (int col = 0; col < 8; col++) { A(row, col) = 0; - std::cout << A(row, col) << " "; } - std::cout << std::endl; } + + // Eigen vector of smallest singular value is the trifocal tensor. Matrix U, V; Vector S; svd(A, U, S, V); - std::cout << "V:" << std::endl; - for (int i = 0; i < V.rows(); i++) { - for (int j = 0; j < V.cols(); j++) { - std::cout << V(i, j) << " "; - } - std::cout << std::endl; - } + for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { matrix0_(i, j) = V(2 * i + j, V.cols() - 1); From 62b4df4a5ecfa3df3e7366df32e8d095695062c6 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 8 Feb 2022 23:06:39 -0800 Subject: [PATCH 20/25] remove debug statements in test --- gtsam/geometry/tests/testTrifocalTensor2.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp index 3a348f73e2..a02820ae84 100644 --- a/gtsam/geometry/tests/testTrifocalTensor2.cpp +++ b/gtsam/geometry/tests/testTrifocalTensor2.cpp @@ -1,11 +1,12 @@ +#include +#include + #include + #include #include #include #include -#include - -#include using namespace std::placeholders; using namespace std; @@ -43,10 +44,6 @@ TEST(TrifocalTensor2, transform) { for (unsigned int i = 0; i < measurement_u.size(); i++) { const Rot2 actual_measurement_u = T.transform(measurement_v[i], measurement_w[i]); - cout << "the ground truth: " << measurement_u[i].c() << " " - << measurement_u[i].s() - << "; the estimate: " << actual_measurement_u.c() << " " - << actual_measurement_u.s() << "\n"; // there might be two solutions for u1 and u2, comparing the ratio instead // of both cos and sin From b5aade6dabd2432bcae8440509e752f6fe0681e1 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Thu, 24 Feb 2022 23:28:31 -0800 Subject: [PATCH 21/25] add file level comment in test --- gtsam/geometry/tests/testTrifocalTensor2.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam/geometry/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp index a02820ae84..61edc903d5 100644 --- a/gtsam/geometry/tests/testTrifocalTensor2.cpp +++ b/gtsam/geometry/tests/testTrifocalTensor2.cpp @@ -1,3 +1,18 @@ +/* ---------------------------------------------------------------------------- + * 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 From 2bf85c5845ed2f5889a9ac08a05b3ce69429a39d Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Fri, 25 Feb 2022 00:14:35 -0800 Subject: [PATCH 22/25] create test data, update test doc --- gtsam/geometry/tests/testTrifocalTensor2.cpp | 157 +++++++------------ 1 file changed, 61 insertions(+), 96 deletions(-) diff --git a/gtsam/geometry/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp index 61edc903d5..0f36d1968c 100644 --- a/gtsam/geometry/tests/testTrifocalTensor2.cpp +++ b/gtsam/geometry/tests/testTrifocalTensor2.cpp @@ -13,128 +13,93 @@ * @author Akshay Krishnan */ -#include -#include - #include - #include #include #include #include +#include + 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 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 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])); +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(0, 0, 0); + data.gt_poses.emplace_back(0, 0, 0); + + // Landmarks + data.gt_landmarks.emplace_back(2.0, 0.5); + data.gt_landmarks.emplace_back(-0.8, 2.4); + data.gt_landmarks.emplace_back(1.9, -0.4); + data.gt_landmarks.emplace_back(2.3, 1.0); + data.gt_landmarks.emplace_back(-0.4, -0.4); + data.gt_landmarks.emplace_back(-3.2, -1.0); + data.gt_landmarks.emplace_back(1.5, 2.0); + + // 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 + +TEST(TrifocalTensor2, transform) { + trifocal::TrifocalTestData data = trifocal::getTestData(); // calculate trifocal tensor - TrifocalTensor2 T(measurement_u, measurement_v, measurement_w); + TrifocalTensor2 T(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 < measurement_u.size(); i++) { - const Rot2 actual_measurement_u = - T.transform(measurement_v[i], measurement_w[i]); + 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_u.c() * measurement_u[i].s(), - actual_measurement_u.s() * measurement_u[i].c(), 1e-8)); + EXPECT(assert_equal(actual_measurement.c() * data.measurements[0][i].s(), + actual_measurement.s() * data.measurements[0][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 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 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 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 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])); - } +TEST(TrifocalTensor2, tensorRegression) { + trifocal::TrifocalTestData data = trifocal::getTestData(); // calculate trifocal tensor - TrifocalTensor2 T(measurement_u, measurement_v, measurement_w); - - Matrix2 actual_trifocal_tensor_mat0 = T.mat1(); + TrifocalTensor2 T(data.measurements[0], data.measurements[1], + data.measurements[2]); - Matrix2 exp_trifocal_tensor_mat0; - exp_trifocal_tensor_mat0 << -0.27261704, 0.09097327, 0.51699647, 0.0108839; + Matrix2 expected_tensor_mat0, expected_tensor_mat1; + // These values were obtained from a numpy-based python implementation. + expected_tensor_mat0 << -0.13178263, 0.29210566, -0.00860471, -0.73975238; + expected_tensor_mat1 << -0.27261704, 0.09097327, 0.51699647, 0.0108839; - EXPECT(assert_equal(actual_trifocal_tensor_mat0, exp_trifocal_tensor_mat0, - 1e-2)); + EXPECT(assert_equal(T.mat0(), expected_tensor_mat0, 1e-2)); + EXPECT(assert_equal(T.mat1(), expected_tensor_mat1, 1e-2)); } int main() { From e2423e7090183a11ff4c890e70014138ae9007f4 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Fri, 25 Feb 2022 19:49:04 -0800 Subject: [PATCH 23/25] change measurement constructor to static method --- gtsam/geometry/TrifocalTensor2.cpp | 32 +++++++++++--------- gtsam/geometry/TrifocalTensor2.h | 21 +++++++------ gtsam/geometry/tests/testTrifocalTensor2.cpp | 10 +++--- 3 files changed, 36 insertions(+), 27 deletions(-) diff --git a/gtsam/geometry/TrifocalTensor2.cpp b/gtsam/geometry/TrifocalTensor2.cpp index dcf48fb504..854569670b 100644 --- a/gtsam/geometry/TrifocalTensor2.cpp +++ b/gtsam/geometry/TrifocalTensor2.cpp @@ -16,6 +16,7 @@ #include #include +#include namespace gtsam { @@ -30,26 +31,27 @@ std::vector convertToProjective(const std::vector& rotations) { } // Construct from 8 bearing measurements. -TrifocalTensor2::TrifocalTensor2(const std::vector& bearings_u, - const std::vector& bearings_v, - const std::vector& bearings_w) - : TrifocalTensor2(convertToProjective(bearings_u), - convertToProjective(bearings_v), - convertToProjective(bearings_w)) {} +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(const std::vector& u, - const std::vector& v, - const std::vector& w) { +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") + "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++) { @@ -66,18 +68,20 @@ TrifocalTensor2::TrifocalTensor2(const std::vector& u, 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); + 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 diff --git a/gtsam/geometry/TrifocalTensor2.h b/gtsam/geometry/TrifocalTensor2.h index a6f7a464c3..7d771b9b39 100644 --- a/gtsam/geometry/TrifocalTensor2.h +++ b/gtsam/geometry/TrifocalTensor2.h @@ -41,27 +41,30 @@ class TrifocalTensor2 { 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. + * @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. */ - TrifocalTensor2(const std::vector& bearings_u, - const std::vector& bearings_v, - const std::vector& bearings_w); + static TrifocalTensor2 FromBearingMeasurements( + const std::vector& bearings_u, const std::vector& bearings_v, + const std::vector& 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. + * @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. */ - TrifocalTensor2(const std::vector& u, const std::vector& v, - const std::vector& w); + 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 diff --git a/gtsam/geometry/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp index 0f36d1968c..7f66f855ca 100644 --- a/gtsam/geometry/tests/testTrifocalTensor2.cpp +++ b/gtsam/geometry/tests/testTrifocalTensor2.cpp @@ -65,12 +65,13 @@ TrifocalTestData getTestData() { } // 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(data.measurements[0], data.measurements[1], - data.measurements[2]); + 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 @@ -86,12 +87,13 @@ TEST(TrifocalTensor2, transform) { } } +// Check the correct tensor is computed from measurements (catch regressions). TEST(TrifocalTensor2, tensorRegression) { trifocal::TrifocalTestData data = trifocal::getTestData(); // calculate trifocal tensor - TrifocalTensor2 T(data.measurements[0], data.measurements[1], - data.measurements[2]); + 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. From 5d2a3c2bd9a59bb2f7aa577455e397f254818e3a Mon Sep 17 00:00:00 2001 From: Hal-Zhaodong-Yang Date: Sat, 26 Feb 2022 14:04:56 -0500 Subject: [PATCH 24/25] add unit test for Jacobian of trifocal tensor --- gtsam/geometry/tests/testTrifocalTensor2.cpp | 51 +++++++++++++++----- 1 file changed, 40 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/tests/testTrifocalTensor2.cpp b/gtsam/geometry/tests/testTrifocalTensor2.cpp index 7f66f855ca..ed7cd252e3 100644 --- a/gtsam/geometry/tests/testTrifocalTensor2.cpp +++ b/gtsam/geometry/tests/testTrifocalTensor2.cpp @@ -40,17 +40,17 @@ TrifocalTestData getTestData() { // Poses data.gt_poses.emplace_back(0, 0, 0); - data.gt_poses.emplace_back(0, 0, 0); - 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(2.0, 0.5); - data.gt_landmarks.emplace_back(-0.8, 2.4); - data.gt_landmarks.emplace_back(1.9, -0.4); - data.gt_landmarks.emplace_back(2.3, 1.0); - data.gt_landmarks.emplace_back(-0.4, -0.4); - data.gt_landmarks.emplace_back(-3.2, -1.0); - data.gt_landmarks.emplace_back(1.5, 2.0); + 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) { @@ -97,13 +97,42 @@ TEST(TrifocalTensor2, tensorRegression) { Matrix2 expected_tensor_mat0, expected_tensor_mat1; // These values were obtained from a numpy-based python implementation. - expected_tensor_mat0 << -0.13178263, 0.29210566, -0.00860471, -0.73975238; - expected_tensor_mat1 << -0.27261704, 0.09097327, 0.51699647, 0.0108839; + 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); From 6cbde539a3fa10569b701bdbc70beaa9ee5c7ad7 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 26 Feb 2022 11:42:14 -0800 Subject: [PATCH 25/25] define constructor from matrices --- gtsam/geometry/TrifocalTensor2.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/TrifocalTensor2.h b/gtsam/geometry/TrifocalTensor2.h index 7d771b9b39..1c4e947454 100644 --- a/gtsam/geometry/TrifocalTensor2.h +++ b/gtsam/geometry/TrifocalTensor2.h @@ -38,7 +38,8 @@ class TrifocalTensor2 { TrifocalTensor2() {} // Construct from the two 2x2 matrices that form the tensor. - TrifocalTensor2(const Matrix2& matrix0, const Matrix2& matrix1); + TrifocalTensor2(const Matrix2& matrix0, const Matrix2& matrix1) + : matrix0_(matrix0), matrix1_(matrix1) {} /** * @brief Estimates a tensor from 8 bearing measurements in 3 cameras. Throws