Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add planar trifocal tensor #1094

Draft
wants to merge 32 commits into
base: develop
Choose a base branch
from
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
32 commits
Select commit Hold shift + click to select a range
18b4036
Finish 4.2a1
dellaert Jan 5, 2022
d6f3468
Merge pull request #1024 from borglab/release/4.2a2
dellaert Jan 10, 2022
76c7419
Merge pull request #1041 from borglab/release/4.2a3
dellaert Jan 17, 2022
d6edcea
Merge pull request #1073 from borglab/release/4.2a4
dellaert Jan 28, 2022
5dc5845
Merge branch 'master' of https://github.com/Hal-Zhaodong-Yang/gtsam i…
Hal-Zhaodong-Yang Feb 1, 2022
049e5d7
Planar Trifocal Tensor
Hal-Zhaodong-Yang Feb 1, 2022
3eda4cd
Merge branch 'develop' of github.com:Hal-Zhaodong-Yang/gtsam into tri…
akshay-krishnan Feb 4, 2022
a0b4dab
Add header file, API
akshay-krishnan Feb 4, 2022
e2e3e40
rename test file
akshay-krishnan Feb 4, 2022
719135c
add trifocal tensor class cpp file
Hal-Zhaodong-Yang Feb 5, 2022
b97fa7a
add transform
Hal-Zhaodong-Yang Feb 5, 2022
87bc39c
change constructor from const to non-const
Hal-Zhaodong-Yang Feb 5, 2022
92d60a0
fixed some bugs
Hal-Zhaodong-Yang Feb 5, 2022
e69490e
fixed some bugs
Hal-Zhaodong-Yang Feb 5, 2022
8aaebb4
Update implementations, builds now
akshay-krishnan Feb 5, 2022
df359c0
update test
akshay-krishnan Feb 5, 2022
b073fcb
fixed some bugs
Hal-Zhaodong-Yang Feb 5, 2022
f6d8cf5
Fixed the bugs and now the tests for trifocal tensor passed
Hal-Zhaodong-Yang Feb 6, 2022
98bdb49
Changed some format and added some comments
Hal-Zhaodong-Yang Feb 9, 2022
3d26c56
Changed some format and added some comments
Hal-Zhaodong-Yang Feb 9, 2022
2743031
Changed some comments
Hal-Zhaodong-Yang Feb 9, 2022
9eaaa0c
Changed some comments
Hal-Zhaodong-Yang Feb 9, 2022
0f0096d
update documentation, move accessor definitions to header
akshay-krishnan Feb 9, 2022
0df9836
throw invalid arg exception
akshay-krishnan Feb 9, 2022
a516b5f
remove debug print statements
akshay-krishnan Feb 9, 2022
62b4df4
remove debug statements in test
akshay-krishnan Feb 9, 2022
b5aade6
add file level comment in test
akshay-krishnan Feb 25, 2022
2bf85c5
create test data, update test doc
akshay-krishnan Feb 25, 2022
e2423e7
change measurement constructor to static method
akshay-krishnan Feb 26, 2022
5d2a3c2
add unit test for Jacobian of trifocal tensor
Hal-Zhaodong-Yang Feb 26, 2022
6cbde53
define constructor from matrices
akshay-krishnan Feb 26, 2022
2031ad1
Merge branch 'trifocal' of github.com:Hal-Zhaodong-Yang/gtsam into tr…
akshay-krishnan Feb 26, 2022
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
98 changes: 98 additions & 0 deletions gtsam/geometry/TrifocalTensor2.cpp
Original file line number Diff line number Diff line change
@@ -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
95 changes: 95 additions & 0 deletions gtsam/geometry/TrifocalTensor2.h
Original file line number Diff line number Diff line change
@@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

no traits ???

139 changes: 139 additions & 0 deletions gtsam/geometry/tests/testTrifocalTensor2.cpp
Original file line number Diff line number Diff line change
@@ -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);
}