diff --git a/limap/triangulation/bindings.cc b/limap/triangulation/bindings.cc index 2070d90f..940f1f08 100644 --- a/limap/triangulation/bindings.cc +++ b/limap/triangulation/bindings.cc @@ -21,6 +21,8 @@ void bind_functions(py::module &m) { m.def("get_normal_direction", &getNormalDirection); m.def("get_direction_from_VP", &getDirectionFromVP); + m.def("compute_essential_matrix", &compute_essential_matrix); + m.def("compute_fundamental_matrix", &compute_fundamental_matrix); m.def("compute_epipolar_IoU", &compute_epipolar_IoU); m.def("triangulate_endpoints", &triangulate_endpoints); m.def("triangulate", &triangulate); diff --git a/limap/triangulation/functions.cc b/limap/triangulation/functions.cc index 112f1db4..ce0b6833 100644 --- a/limap/triangulation/functions.cc +++ b/limap/triangulation/functions.cc @@ -39,14 +39,10 @@ V3D getDirectionFromVP(const V3D& vp, const CameraView& view) { return direc.normalized(); } -double compute_epipolar_IoU(const Line2d& l1, const CameraView& view1, - const Line2d& l2, const CameraView& view2) +M3D compute_essential_matrix(const CameraView& view1, const CameraView& view2) { - const M3D K1_inv = view1.K_inv(); const M3D R1 = view1.R(); const V3D T1 = view1.T(); - - const M3D K2_inv = view2.K_inv(); const M3D R2 = view2.R(); const V3D T2 = view2.T(); @@ -60,7 +56,21 @@ double compute_epipolar_IoU(const Line2d& l1, const CameraView& view1, tskew(1, 0) = relT[2]; tskew(1, 1) = 0.0; tskew(1, 2) = -relT[0]; tskew(2, 0) = -relT[1]; tskew(2, 1) = relT[0]; tskew(2, 2) = 0.0; M3D E = tskew * relR; - M3D F = K2_inv.transpose() * E * K1_inv; + return E; +} + +M3D compute_fundamental_matrix(const CameraView& view1, const CameraView& view2) +{ + M3D E = compute_essential_matrix(view1, view2); + M3D F = view2.K_inv().transpose() * E * view1.K_inv(); + return F; +} + +double compute_epipolar_IoU(const Line2d& l1, const CameraView& view1, + const Line2d& l2, const CameraView& view2) +{ + // fundamental matrix + M3D F = compute_fundamental_matrix(view1, view2); // epipolar lines V3D coor_l2 = l2.coords(); diff --git a/limap/triangulation/functions.h b/limap/triangulation/functions.h index 70522586..80fd275a 100644 --- a/limap/triangulation/functions.h +++ b/limap/triangulation/functions.h @@ -17,6 +17,9 @@ V3D getNormalDirection(const Line2d& l, const CameraView& view); V3D getDirectionFromVP(const V3D& vp, const CameraView& view); // weak epipolar constraints +M3D compute_essential_matrix(const CameraView& view1, const CameraView& view2); +M3D compute_fundamental_matrix(const CameraView& view1, const CameraView& view2); + // intersect epipolar lines with the matched line on image 2 double compute_epipolar_IoU(const Line2d& l1, const CameraView& view1, const Line2d& l2, const CameraView& view2); diff --git a/limap/triangulation/triangulation.py b/limap/triangulation/triangulation.py index 66f6aad3..678b0cc8 100644 --- a/limap/triangulation/triangulation.py +++ b/limap/triangulation/triangulation.py @@ -8,6 +8,12 @@ def get_normal_direction(l, view): def get_direction_from_VP(vp, view): return _tri.get_direction_from_VP(vp, view) +def compute_essential_matrix(view1, view2): + return _tri.compute_essential_matrix(view1, view2) + +def compute_fundamental_matrix(view1, view2): + return _tri.compute_fundamental_matrix(view1, view2) + def compute_epipolar_IoU(l1, view1, l2, view2): return _tri.compute_epipolar_IoU(l1, view1, l2, view2)