From f76b58014b7cbbc512d2222bf801bbc4ddd82d5b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 11 Dec 2021 13:20:47 -0500 Subject: [PATCH 1/3] Fixed small typos --- gtsam/slam/EssentialMatrixFactor.h | 15 ++++++++++++++- .../slam/tests/testEssentialMatrixConstraint.cpp | 2 +- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 787efac51e..49d8b82182 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -1,7 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2014, 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 EssentialMatrixFactor.cpp + * @file EssentialMatrixFactor.h * @brief EssentialMatrixFactor class * @author Frank Dellaert + * @author Ayush Baid + * @author Akshay Krisnan * @date December 17, 2013 */ diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 080239b350..2faac24d1b 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testEssentialMatrixConstraint.cpp + * @file TestEssentialMatrixConstraint.cpp * @brief Unit tests for EssentialMatrixConstraint Class * @author Frank Dellaert * @author Pablo Alcantarilla From 5a2ce5af3b50e6315af4c955b119454606079970 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 11 Dec 2021 13:21:15 -0500 Subject: [PATCH 2/3] wrapped and tested EssentialMatrixConstraint --- gtsam/slam/slam.i | 15 ++++++ .../tests/testEssentialMatrixConstraint.py | 47 +++++++++++++++++++ 2 files changed, 62 insertions(+) create mode 100644 python/gtsam/tests/testEssentialMatrixConstraint.py diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 95d89ef8b7..da1c197cbf 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -196,6 +196,21 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, const gtsam::noiseModel::Base* noiseModel); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const; + Vector evaluateError(const gtsam::EssentialMatrix& E) const; +}; + +#include +virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor { + EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE, + const gtsam::noiseModel::Base *model); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::EssentialMatrixConstraint& other, double tol) const; + Vector evaluateError(const gtsam::Pose3& p1, const gtsam::Pose3& p2) const; + const gtsam::EssentialMatrix& measured() const; }; #include diff --git a/python/gtsam/tests/testEssentialMatrixConstraint.py b/python/gtsam/tests/testEssentialMatrixConstraint.py new file mode 100644 index 0000000000..8439ad2e93 --- /dev/null +++ b/python/gtsam/tests/testEssentialMatrixConstraint.py @@ -0,0 +1,47 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +visual_isam unit tests. +Author: Frank Dellaert & Pablo Alcantarilla +""" + +import unittest + +import gtsam +import numpy as np +from gtsam import (EssentialMatrix, EssentialMatrixConstraint, Point3, Pose3, + Rot3, Unit3, symbol) +from gtsam.utils.test_case import GtsamTestCase + + +class TestVisualISAMExample(GtsamTestCase): + def test_VisualISAMExample(self): + + # Create a factor + poseKey1 = symbol('x', 1) + poseKey2 = symbol('x', 2) + trueRotation = Rot3.RzRyRx(0.15, 0.15, -0.20) + trueTranslation = Point3(+0.5, -1.0, +1.0) + trueDirection = Unit3(trueTranslation) + E = EssentialMatrix(trueRotation, trueDirection) + model = gtsam.noiseModel.Isotropic.Sigma(5, 0.25) + factor = EssentialMatrixConstraint(poseKey1, poseKey2, E, model) + + # Create a linearization point at the zero-error point + pose1 = Pose3(Rot3.RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0)) + pose2 = Pose3( + Rot3.RzRyRx(0.179693265735950, 0.002945368776519, + 0.102274823253840), + Point3(-3.37493895, 6.14660244, -8.93650986)) + + expected = np.zeros((5, 1)) + actual = factor.evaluateError(pose1, pose2) + self.gtsamAssertEquals(actual, expected, 1e-8) + + +if __name__ == "__main__": + unittest.main() From dd86cc19c51682844fb9099a4df614ae02c16bdd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 11 Dec 2021 13:56:17 -0500 Subject: [PATCH 3/3] Fix spelling --- gtsam/slam/EssentialMatrixFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 49d8b82182..5997ad2247 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -14,7 +14,7 @@ * @brief EssentialMatrixFactor class * @author Frank Dellaert * @author Ayush Baid - * @author Akshay Krisnan + * @author Akshay Krishnan * @date December 17, 2013 */