def estimate_pose(pts_2d, line_2d, pts_3d, line_3d, K):
# compose all geometric constraints
xxn, XXw = VakhitovHelper.points(pts_2d, pts_3d, K)
xs, xe, Xs, Xe = VakhitovHelper.lines(line_2d, line_3d, K)
# Invoke method on matlab
R, t = Suite.matlab_engine.DLT(XXw, xxn, xs, xe, Xs, Xe, nargout=2)
# Cast to numpy types
return [(np.array(R), np.array(t).ravel())]
def estimate_pose(line_2d, line_3d, K):
# compose line constraints
xs, xe, Xs, Xe = VakhitovHelper.lines(line_2d, line_3d, K)
# Invoke method on matlab
R, t = Suite.matlab_engine.mirzWrapper(xs, xe, Xs, Xe, nargout=2)
# Cast to numpy types
return [(np.array(R), np.array(t).ravel())]
def estimate_pose(pts_2d, line_2d, pts_3d, line_3d, K):
# requires a minimum of 6 elements
if (len(line_2d) + len(pts_2d)) < 6:
return [(np.full((3, 3), np.nan), np.full(3, np.nan))]
# compose all geometric constraints
xxn, XXw = VakhitovHelper.points(pts_2d, pts_3d, K)
xs, xe, Xs, Xe = VakhitovHelper.lines(line_2d, line_3d, K)
# Invoke method on matlab
R, t = Suite.matlab_engine.EPnPLS_GN(XXw,
xxn,
xs,
xe,
Xs,
Xe,
nargout=2)
# Cast to numpy types
return [(np.array(R), np.array(t).ravel())]
def estimate_pose(pts_2d, line_2d, pts_3d, line_3d, K):
# compose all geometric constraints
xxn, XXw = VakhitovHelper.points(pts_2d, pts_3d, K)
xs, xe, Xs, Xe = VakhitovHelper.lines(line_2d, line_3d, K)
# Invoke method on matlab
Rs, ts = Suite.matlab_engine.OPnPL(XXw, xxn, xs, xe, Xs, Xe, nargout=2)
Rs, ts = np.array(Rs), np.array(ts)
# Detect if there's no multiple solutions
if len(Rs.shape) == 2:
return [(Rs, ts.ravel())]
# This method returns multiple poses even in a non-minimal case
# repackage results
poses_out = []
for i in range(Rs.shape[2]):
R = Rs[:, :, i]
t = ts[:, i]
poses_out.append((R, t))
return poses_out
def estimate_pose(line_2d, line_3d, K):
# requires a minimum of 9 lines to work properly
if len(line_2d) < 9:
return [(np.full((3, 3), np.nan), np.full(3, np.nan))]
# compose line constraints
xs, xe, Xs, Xe = VakhitovHelper.lines(line_2d, line_3d, K)
# Invoke method on matlab
R, t = Suite.matlab_engine.plueckerWrapper(xs, xe, Xs, Xe, nargout=2)
# Cast to numpy types
return [(np.array(R), np.array(t).ravel())]
def estimate_pose(pts_2d, pts_3d, K):
# compose point variables constraints
xxn, XXw = VakhitovHelper.points(pts_2d, pts_3d, K)
# Invoke method on matlab
Rs, ts = Suite.matlab_engine.OPnP(XXw, xxn, nargout=2)
Rs, ts = np.array(Rs), np.array(ts)
# Detect if there's no multiple solutions
if len(Rs.shape) == 2:
return [(Rs, ts.ravel())]
# repackage results
poses_out = []
for i in range(Rs.shape[2]):
R = Rs[:, :, i]
t = ts[:, i]
poses_out.append((R, t))
return poses_out
def estimate_pose(line_2d, line_3d, K):
# compose all geometric constraints
xxn = Suite.matlab_engine.double.empty(2, 0)
XXw = Suite.matlab_engine.double.empty(3, 0)
xs, xe, Xs, Xe = VakhitovHelper.lines(line_2d, line_3d, K)
# Invoke method on matlab
Rs, ts = Suite.matlab_engine.OPnPL(XXw, xxn, xs, xe, Xs, Xe, nargout=2)
Rs, ts = np.array(Rs), np.array(ts)
# Detect if there's no multiple solutions
if len(Rs.shape) == 2:
return [(Rs, ts.ravel())]
# repackage results
poses_out = []
for i in range(Rs.shape[2]):
R = Rs[:, :, i]
t = ts[:, i]
poses_out.append((R, t))
return poses_out
def estimate_pose(line_2d, line_3d, K):
# the method needs at least 6 lines to work
if len(line_2d) < 6:
return [(np.full((3, 3), np.nan), np.full(3, np.nan))]
# compose all geometric constraints
xxn = Suite.matlab_engine.double.empty(2, 0)
XXw = Suite.matlab_engine.double.empty(3, 0)
xs, xe, Xs, Xe = VakhitovHelper.lines(line_2d, line_3d, K)
# Invoke method on matlab
R, t = Suite.matlab_engine.EPnPLS_GN(XXw,
xxn,
xs,
xe,
Xs,
Xe,
nargout=2)
# Cast to numpy types
return [(np.array(R), np.array(t).ravel())]
