From 1f166ff59c5efe8639b5fed73b5b857744800743 Mon Sep 17 00:00:00 2001
From: Paul-Edouard Sarlin <15985472+sarlinpe@users.noreply.github.com>
Date: Tue, 23 Jan 2024 23:28:17 +0100
Subject: [PATCH] Bind essential matrix utils (#244)

---
 pycolmap/estimators/two_view_geometry.h | 17 +++++++++++++++++
 pycolmap/geometry/bindings.h            |  2 ++
 2 files changed, 19 insertions(+)

diff --git a/pycolmap/estimators/two_view_geometry.h b/pycolmap/estimators/two_view_geometry.h
index 558e4c3..c168d22 100644
--- a/pycolmap/estimators/two_view_geometry.h
+++ b/pycolmap/estimators/two_view_geometry.h
@@ -1,6 +1,7 @@
 // Authors: John Lambert (johnwlambert), Paul-Edouard Sarlin (skydes)
 
 #include "colmap/estimators/two_view_geometry.h"
+#include "colmap/estimators/utils.h"
 #include "colmap/geometry/pose.h"
 #include "colmap/math/random.h"
 #include "colmap/optim/loransac.h"
@@ -161,4 +162,20 @@ void BindTwoViewGeometryEstimator(py::module& m) {
         "camera2"_a,
         "points2"_a,
         "geometry"_a);
+
+  m.def(
+      "squared_sampson_error",
+      [](const std::vector<Eigen::Vector2d>& points1,
+         const std::vector<Eigen::Vector2d>& points2,
+         const Eigen::Matrix3d& E) {
+        std::vector<double> residuals;
+        ComputeSquaredSampsonError(points1, points2, E, &residuals);
+        return residuals;
+      },
+      "points2D1"_a,
+      "points2D2"_a,
+      "E"_a,
+      "Calculate the squared Sampson error for a given essential or "
+      "fundamental matrix.",
+      py::call_guard<py::gil_scoped_release>());
 }
diff --git a/pycolmap/geometry/bindings.h b/pycolmap/geometry/bindings.h
index 6db1746..117f028 100644
--- a/pycolmap/geometry/bindings.h
+++ b/pycolmap/geometry/bindings.h
@@ -1,3 +1,4 @@
+#include "colmap/geometry/essential_matrix.h"
 #include "colmap/geometry/pose.h"
 #include "colmap/geometry/rigid3.h"
 #include "colmap/geometry/sim3.h"
@@ -51,6 +52,7 @@ void BindGeometry(py::module& m) {
       .def_readwrite("rotation", &Rigid3d::rotation)
       .def_readwrite("translation", &Rigid3d::translation)
       .def("matrix", &Rigid3d::ToMatrix)
+      .def("essential_matrix", &EssentialMatrixFromPose)
       .def(py::self * Eigen::Vector3d())
       .def(py::self * Rigid3d())
       .def("inverse", static_cast<Rigid3d (*)(const Rigid3d&)>(&Inverse))