Allow user to choose if imaginary roots should be filtered. Added exa…

…mples.

README.md

@@ -4,6 +4,7 @@ A Python 3 implementation of the absolute pose estimation method for minimal con
 
 > Lipu Zhou, Jiamin Ye, and Michael Kaess. A Stable Algebraic Camera Pose Estimation for Minimal Configurations of 2D/3D Point and Line Correspondences. In Asian Conference on Computer Vision, 2018.
 
+The methods implemented are able to address 4 modalities of minimal problems, namely: perspective-3-points (P3P), perspective-2-points-1-line (P2P1L), perspective-1-point-2-lines (P1P2L) and perspective-3-lines (P3L).
 This package also includes an implementation the robustified version of Kukelova et al. E3Q3 method presented in
 
 
@@ -24,6 +25,10 @@ Clone this repo and invoke from its root folder
 python setup.py install
 ```
 
+## Examples
+
+The library exposes 5 public functions: `p3p`, `p3l`, `p2p1l`, `p1p2l`, and `e3q3`. You can find a couple of examples showing how to use each in the [examples folder](https://github.com/SergioRAgostinho/zhou-accv-2018/blob/master/examples).
+
 ## Interesting Facts
 
 Despite being a Python implementation, the majority of code is written in C. It is a really small section if compared with the extent of the full implementation, but it is way bigger than everything else in its sheer byte size! There are some really long expressions in a very small fraction of the code which Python's interpreter simply couldn't parse, so I've relegated that job to a C compiler.

examples/p1p2l.py

@@ -0,0 +1,93 @@
+import numpy as np
+from zhou_accv_2018 import p1p2l
+
+# fix seed to allow for reproducible results
+np.random.seed(0)
+np.random.seed(42)
+
+# instantiate a couple of points centered around the origin
+pts = 0.6 * (np.random.random((1, 3)) - 0.5)
+
+# 3D lines are parameterized as pts and direction stacked into a tuple
+# instantiate a couple of points centered around the origin
+pts_l = 0.6 * (np.random.random((2, 3)) - 0.5)
+# generate normalized directions
+directions = 2 * (np.random.random((2, 3)) - 0.5)
+directions /= np.linalg.norm(directions, axis=1)[:, None]
+
+line_3d = (pts_l, directions)
+
+# Made up projective matrix
+K = np.array([[160, 0, 320], [0, 120, 240], [0, 0, 1]])
+
+# A pose
+R_gt = np.array(
+    [
+        [0.89802142, -0.41500101, 0.14605372],
+        [0.24509948, 0.7476071, 0.61725997],
+        [-0.36535431, -0.51851499, 0.77308372],
+    ]
+)
+t_gt = np.array([-0.0767557, 0.13917375, 1.9708239])
+
+# sample 2 points from each line and stack all
+pts_ls = np.hstack((pts_l, pts_l + directions)).reshape((-1, 3))
+pts_all = np.vstack((pts, pts_ls))
+
+# Project everything to 2D
+pts_all_2d = (pts_all @ R_gt.T + t_gt) @ K.T
+pts_all_2d = (pts_all_2d / pts_all_2d[:, -1, None])[:, :-1]
+
+pts_2d = pts_all_2d[:1]
+line_2d = pts_all_2d[1:].reshape((-1, 2, 2))
+
+# Compute pose candidates. the problem is not minimal so only one
+# will be provided
+poses = p1p2l(pts_2d=pts_2d, line_2d=line_2d, pts_3d=pts, line_3d=line_3d, K=K)
+
+# The error criteria for lines is to ensure that both 3D points and
+# direction, after transformation, are inside the plane formed by the
+# line projection. We start by computing the plane normals
+
+# line in 2D has two sampled points.
+line_2d_c = np.linalg.solve(
+    K, np.vstack((line_2d.reshape((2 * 2, 2)).T, np.ones((1, 2 * 2))))
+).T
+line_2d_c = line_2d_c.reshape((2, 2, 3))
+
+# row wise cross product + normalization
+n_li = np.cross(line_2d_c[:, 0, :], line_2d_c[:, 1, :])
+n_li /= np.linalg.norm(n_li, axis=1)[:, None]
+
+# Print results
+print("R (ground truth):", R_gt, sep="\n")
+print("t (ground truth):", t_gt)
+print("Nr of possible poses:", len(poses))
+for i, pose in enumerate(poses):
+    R, t = pose
+
+    # The error criteria for lines is to ensure that both 3D points and
+    # direction, after transformation, are inside the plane formed by the
+    # line projection
+
+    # Project points to 2D
+    pts_2d_est = (pts @ R.T + t) @ K.T
+    pts_2d_est = (pts_2d_est / pts_2d_est[:, -1, None])[:, :-1]
+    err_p = np.mean(np.linalg.norm(pts_2d - pts_2d_est, axis=1))
+
+    # pts_l
+    pts_l_est = pts_l @ R.T + t
+    err_l_pt = np.mean(np.abs(np.sum(pts_l_est * n_li, axis=1)))
+
+    # directions
+    dir_est = directions @ R.T
+    err_l_dir = np.mean(
+        np.arcsin(np.abs(np.sum(dir_est * n_li, axis=1))) * 180.0 / np.pi
+    )
+
+    print("Estimate -", i + 1)
+    print("R (estimate):", R, sep="\n")
+    print("t (estimate):", t)
+    print("Mean error (pixels):", err_p)
+    print("Mean pt distance from plane (m):", err_l_pt)
+    print("Mean angle error from plane (°):", err_l_dir)

examples/p2p1l.py

@@ -0,0 +1,93 @@
+import numpy as np
+from zhou_accv_2018 import p2p1l
+
+# fix seed to allow for reproducible results
+np.random.seed(0)
+np.random.seed(42)
+
+# instantiate a couple of points centered around the origin
+pts = 0.6 * (np.random.random((2, 3)) - 0.5)
+
+# 3D lines are parameterized as pts and direction stacked into a tuple
+# instantiate a couple of points centered around the origin
+pts_l = 0.6 * (np.random.random((1, 3)) - 0.5)
+# generate normalized directions
+directions = 2 * (np.random.random((1, 3)) - 0.5)
+directions /= np.linalg.norm(directions, axis=1)[:, None]
+
+line_3d = (pts_l, directions)
+
+# Made up projective matrix
+K = np.array([[160, 0, 320], [0, 120, 240], [0, 0, 1]])
+
+# A pose
+R_gt = np.array(
+    [
+        [0.89802142, -0.41500101, 0.14605372],
+        [0.24509948, 0.7476071, 0.61725997],
+        [-0.36535431, -0.51851499, 0.77308372],
+    ]
+)
+t_gt = np.array([-0.0767557, 0.13917375, 1.9708239])
+
+# sample 2 points from each line and stack all
+pts_ls = np.hstack((pts_l, pts_l + directions)).reshape((-1, 3))
+pts_all = np.vstack((pts, pts_ls))
+
+# Project everything to 2D
+pts_all_2d = (pts_all @ R_gt.T + t_gt) @ K.T
+pts_all_2d = (pts_all_2d / pts_all_2d[:, -1, None])[:, :-1]
+
+pts_2d = pts_all_2d[:2]
+line_2d = pts_all_2d[2:].reshape((-1, 2, 2))
+
+# Compute pose candidates. the problem is not minimal so only one
+# will be provided
+poses = p2p1l(pts_2d=pts_2d, line_2d=line_2d, pts_3d=pts, line_3d=line_3d, K=K)
+
+# The error criteria for lines is to ensure that both 3D points and
+# direction, after transformation, are inside the plane formed by the
+# line projection. We start by computing the plane normals
+
+# line in 2D has two sampled points.
+line_2d_c = np.linalg.solve(
+    K, np.vstack((line_2d.reshape((2 * 1, 2)).T, np.ones((1, 2 * 1))))
+).T
+line_2d_c = line_2d_c.reshape((1, 2, 3))
+
+# row wise cross product + normalization
+n_li = np.cross(line_2d_c[:, 0, :], line_2d_c[:, 1, :])
+n_li /= np.linalg.norm(n_li, axis=1)[:, None]
+
+# Print results
+print("R (ground truth):", R_gt, sep="\n")
+print("t (ground truth):", t_gt)
+print("Nr of possible poses:", len(poses))
+for i, pose in enumerate(poses):
+    R, t = pose
+
+    # The error criteria for lines is to ensure that both 3D points and
+    # direction, after transformation, are inside the plane formed by the
+    # line projection
+
+    # Project points to 2D
+    pts_2d_est = (pts @ R.T + t) @ K.T
+    pts_2d_est = (pts_2d_est / pts_2d_est[:, -1, None])[:, :-1]
+    err_p = np.mean(np.linalg.norm(pts_2d - pts_2d_est, axis=1))
+
+    # pts_l
+    pts_l_est = pts_l @ R.T + t
+    err_l_pt = np.mean(np.abs(np.sum(pts_l_est * n_li, axis=1)))
+
+    # directions
+    dir_est = directions @ R.T
+    err_l_dir = np.mean(
+        np.arcsin(np.abs(np.sum(dir_est * n_li, axis=1))) * 180.0 / np.pi
+    )
+
+    print("Estimate -", i + 1)
+    print("R (estimate):", R, sep="\n")
+    print("t (estimate):", t)
+    print("Mean error (pixels):", err_p)
+    print("Mean pt distance from plane (m):", err_l_pt)
+    print("Mean angle error from plane (°):", err_l_dir)

examples/p3l.py

@@ -0,0 +1,79 @@
+import numpy as np
+from zhou_accv_2018 import p3l
+
+# fix seed to allow for reproducible results
+np.random.seed(0)
+np.random.seed(42)
+
+# 3D lines are parameterized as pts and direction stacked into a tuple
+# instantiate a couple of points centered around the origin
+pts = 0.6 * (np.random.random((3, 3)) - 0.5)
+
+# generate normalized directions
+directions = 2 * (np.random.random((3, 3)) - 0.5)
+directions /= np.linalg.norm(directions, axis=1)[:, None]
+
+line_3d = (pts, directions)
+
+# Made up projective matrix
+K = np.array([[160, 0, 320], [0, 120, 240], [0, 0, 1]])
+
+# A pose
+R_gt = np.array(
+    [
+        [0.89802142, -0.41500101, 0.14605372],
+        [0.24509948, 0.7476071, 0.61725997],
+        [-0.36535431, -0.51851499, 0.77308372],
+    ]
+)
+t_gt = np.array([-0.0767557, 0.13917375, 1.9708239])
+
+# Sample to points from the line and project them to 2D
+pts_s = np.hstack((pts, pts + directions)).reshape((-1, 3))
+line_2d = (pts_s @ R_gt.T + t_gt) @ K.T
+
+# this variable is organized as (line, point, dim)
+line_2d = (line_2d / line_2d[:, -1, None])[:, :-1].reshape((-1, 2, 2))
+
+# Compute pose candidates. the problem is not minimal so only one
+# will be provided
+poses = p3l(line_2d=line_2d, line_3d=line_3d, K=K)
+
+# The error criteria for lines is to ensure that both 3D points and
+# direction, after transformation, are inside the plane formed by the
+# line projection. We start by computing the plane normals
+
+# line in 2D has two sampled points.
+line_2d_c = np.linalg.solve(
+    K, np.vstack((line_2d.reshape((2 * 3, 2)).T, np.ones((1, 2 * 3))))
+).T
+line_2d_c = line_2d_c.reshape((3, 2, 3))
+
+# row wise cross product + normalization
+n_li = np.cross(line_2d_c[:, 0, :], line_2d_c[:, 1, :])
+n_li /= np.linalg.norm(n_li, axis=1)[:, None]
+
+# Print results
+print("R (ground truth):", R_gt, sep="\n")
+print("t (ground truth):", t_gt)
+print("Nr of possible poses:", len(poses))
+for i, pose in enumerate(poses):
+    R, t = pose
+
+    # The error criteria for lines is to ensure that both 3D points and
+    # direction, after transformation, are inside the plane formed by the
+    # line projection
+
+    # pts
+    pts_est = pts @ R.T + t
+    err_pt = np.mean(np.abs(np.sum(pts_est * n_li, axis=1)))
+
+    # directions
+    dir_est = directions @ R.T
+    err_dir = np.mean(np.arcsin(np.abs(np.sum(dir_est * n_li, axis=1))) * 180.0 / np.pi)
+
+    print("Estimate -", i + 1)
+    print("R (estimate):", R, sep="\n")
+    print("t (estimate):", t)
+    print("Mean pt distance from plane (m):", err_pt)
+    print("Mean angle error from plane (°):", err_dir)

examples/p3p.py

@@ -0,0 +1,48 @@
+import numpy as np
+from zhou_accv_2018 import p3p
+
+# fix seed to allow for reproducible results
+np.random.seed(0)
+np.random.seed(42)
+
+# instantiate a couple of points centered around the origin
+pts = 0.6 * (np.random.random((3, 3)) - 0.5)
+
+# Made up projective matrix
+K = np.array([[160, 0, 320], [0, 120, 240], [0, 0, 1]])
+
+# A pose
+R_gt = np.array(
+    [
+        [-0.48048015, 0.1391384, -0.86589799],
+        [-0.0333282, -0.98951829, -0.14050899],
+        [-0.8763721, -0.03865296, 0.48008113],
+    ]
+)
+t_gt = np.array([-0.10266772, 0.25450789, 1.70391109])
+
+# Project points to 2D
+pts_2d = (pts @ R_gt.T + t_gt) @ K.T
+pts_2d = (pts_2d / pts_2d[:, -1, None])[:, :-1]
+
+# Compute pose candidates. the problem is not minimal so only one
+# will be provided
+poses = p3p(pts_2d=pts_2d, pts_3d=pts, K=K)
+
+# Print results
+print("R (ground truth):", R_gt, sep="\n")
+print("t (ground truth):", t_gt)
+
+print("Nr of possible poses:", len(poses))
+for i, pose in enumerate(poses):
+    R, t = pose
+
+    # Project points to 2D
+    pts_2d_est = (pts @ R.T + t) @ K.T
+    pts_2d_est = (pts_2d_est / pts_2d_est[:, -1, None])[:, :-1]
+    err = np.mean(np.linalg.norm(pts_2d - pts_2d_est, axis=1))
+
+    print("Estimate -", i + 1)
+    print("R (estimate):", R, sep="\n")
+    print("t (estimate):", t)
+    print("Mean error (pixels):", err)