diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 787efac51..5997ad224 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 Krishnan * @date December 17, 2013 */ diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 95d89ef8b..da1c197cb 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/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 080239b35..2faac24d1 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 diff --git a/python/gtsam/tests/testEssentialMatrixConstraint.py b/python/gtsam/tests/testEssentialMatrixConstraint.py new file mode 100644 index 000000000..8439ad2e9 --- /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()