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.

Sign up for GitHub

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
+332 −0
Draft

Add planar trifocal tensor #1094

Changes from 1 commit
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
Prev Previous commit
Next Next commit
add unit test for Jacobian of trifocal tensor
  • Loading branch information
@Hal-Zhaodong-Yang
Hal-Zhaodong-Yang committed Feb 26, 2022
commit 5d2a3c2bd9a59bb2f7aa577455e397f254818e3a
51 changes: 40 additions & 11 deletions gtsam/geometry/tests/testTrifocalTensor2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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<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);
Expand Down