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())]