Merge pull request #962 from borglab/feature/wrap_EssentialMatrixCons…

…traint wrapped and tested EssentialMatrixConstraint
borglab · Dec 11, 2021 · 4abe1a1 · 4abe1a1
2 parents 3615c4b + dd86cc1
commit 4abe1a1
Show file tree

Hide file tree

Showing 4 changed files with 77 additions and 2 deletions.
diff --git 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
@@ -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 <gtsam/slam/EssentialMatrixConstraint.h>
+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 <gtsam/slam/dataset.h>

diff --git 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
@@ -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()