def process_frame(deinterlacing, detectingWands, frame, opts, pair):
ci, md = pair
img = get_movie_frame(md, frame, deinterlacing)
#data = filter_movie_frame(img, small_blur, large_blur)
#img, data = get_processed_movie_frame(md, frame, small_blur, large_blur, deinterlacing)
QApp.view().cameras[ci + 1].invalidateImageData()
"""
if 1: # show the filtered image
img[:] = data
pass
if 0: # crush the image to see the blobs
lookup = np.zeros(256, dtype=np.uint8)
lookup[threshold_bright:] = 255
lookup[255 - threshold_dark_inv:threshold_bright] = 128
img[:] = lookup[img]
"""
if 1:
good_darks, pts0, good_lights, pts1, data = get_dark_and_light_points(
img, frame, ci, opts)
if 1: # show the filtered image
#print "data before insertion", type(data), data.shape
#sys.exit(0)
img[:] = data
if 0: # crush the image to see the blobs
lookup = np.zeros(256, dtype=np.uint8)
lookup[threshold_bright:] = 255
lookup[255 - threshold_dark_inv:threshold_bright] = 128
img[:] = lookup[img]
# good_darks, pts0 = Detect.detect_dots(255-data, opts['threshold_dark_inv'], opts)
# good_lights,pts1 = Detect.detect_dots(data, opts['threshold_bright'], opts)
print ci, frame, len(pts0), len(pts1), 'good points (darks,lights)'
if detectingWands:
ratio = 2.0
x2d_threshold = 0.5
straightness_threshold = 0.01 * 2
match_threshold = 0.07 * 2
x2ds_labels = -np.ones(pts1.shape[0], dtype=np.int32)
x2ds_splits = np.array([0, pts1.shape[0]], dtype=np.int32)
ISCV.label_T_wand(pts1, x2ds_splits, x2ds_labels, ratio,
x2d_threshold, straightness_threshold,
match_threshold)
print x2ds_labels
for r, li in zip(good_lights, x2ds_labels):
if li != -1: # make some red boxes
dx, dy = 10, 10
img[int(r.sy - dy):int(r.sy + dy),
int(r.sx - dx):int(r.sx + dx), 0] = 128
else:
pts0 = pts1 = []
return (pts0, pts1)
def detect_wand(x2ds_data, x2ds_splits, mats, thresh=20. / 2000., x3d_threshold=1000000.): Ps = np.array([m[2] / np.linalg.norm(m[2][0, :3]) for m in mats], dtype=np.float32) wand_x3ds = np.array([[160, 0, 0], [0, 0, 0], [-80, 0, 0], [0, 0, -120], [0, 0, -240]], dtype=np.float32) x2ds_labels = -np.ones(x2ds_data.shape[0], dtype=np.int32) ISCV.label_T_wand(x2ds_data, x2ds_splits, x2ds_labels, 2.0, 0.5, 0.01, 0.07) x2ds_labels2 = x2ds_labels.copy() count = np.sum(x2ds_labels2 != -1) / 5 if count < 3: return None, None, None x3ds, x3ds_labels, E_x2ds_single, x2ds_single_labels = Recon.solve_x3ds(x2ds_data, x2ds_splits, x2ds_labels2, Ps) count = ISCV.project_and_clean(x3ds, Ps, x2ds_data, x2ds_splits, x2ds_labels, x2ds_labels2, thresh ** 2, thresh ** 2, x3d_threshold) if count < 3: return None, None, None x3ds, x3ds_labels, E_x2ds_single, x2ds_single_labels = Recon.solve_x3ds(x2ds_data, x2ds_splits, x2ds_labels2, Ps) assert np.all(x3ds_labels == [0, 1, 2, 3, 4]), 'ERROR: Labels do not match' # skip if somehow not all points seen assert np.max(x3ds ** 2) < 1e9, 'ERROR: Values out of bounds' + repr(x3ds) mat = rigid_align_points(wand_x3ds, x3ds) x3ds = np.dot(wand_x3ds, mat[:3, :3].T) + mat[:, 3] return x3ds, x3ds_labels, x2ds_labels2
def generate_wand_correspondences(wand_frames, mats2, camera_solved, rigid_filter=True, error_thresholds=None, x3d_threshold=1000000.): """ Args: wand_frames mats2 camera_solved rigid_filter = True error_thresholds = None Returns: x2s_cameras x3s_cameras frames_cameras num_kept_frames Requires: ISCV.undistort_points ISCV.label_T_wand Recon.solve_x3ds ISCV.project_and_clean """ def get_order(labels): """ Return the x2d index of the five points of the T Wand Args: labels (int[]): Returns: int[5]: "order" label indexes """ try: l = list(labels) order = [l.index(x) for x in xrange(5)] return order except: return None numCameras = len(mats2) Ps2 = np.array([m[2]/np.linalg.norm(m[2][0,:3]) for m in mats2],dtype=np.float32) x2ds_frames = [] x2ds_labels_frames = [] x2ds_splits_frames = [] x3ds_frames = [] # TODO wand geo should be passed in? must be compatible with the label_T_wand wand_x3ds = np.array([[160,0,0],[0,0,0],[-80,0,0],[0,0,-120],[0,0,-240]],dtype=np.float32) thresh = (20./2000.)**2 if error_thresholds is None else error_thresholds**2 # projection must be close to be included for intersection num_kept_frames = 0 for fi,(x2ds_raw_data,x2ds_splits) in enumerate(wand_frames): # intersect over all frames with current solved cameras x2ds_data,_ = undistort_dets(x2ds_raw_data, x2ds_splits, mats2) x2ds_labels = -np.ones(x2ds_data.shape[0],dtype=np.int32) ISCV.label_T_wand(x2ds_data, x2ds_splits, x2ds_labels, 2.0, 0.5, 0.01, 0.07) x2ds_labels2 = x2ds_labels.copy() for cs,c0,c1 in zip(camera_solved,x2ds_splits[:-1],x2ds_splits[1:]): # remove labels for unsolved cameras if not cs: x2ds_labels2[c0:c1] = -1 count = np.sum(x2ds_labels2 != -1)/5 if count >= 3: # only use points seen in three solved cameras x3ds, x3ds_labels, E_x2ds_single, x2ds_single_labels = Recon.solve_x3ds(x2ds_data, x2ds_splits, x2ds_labels2, Ps2) count = ISCV.project_and_clean(x3ds, Ps2, x2ds_data, x2ds_splits, x2ds_labels, x2ds_labels2, thresh, thresh, x3d_threshold) if count < 3: continue x3ds, x3ds_labels, E_x2ds_single, x2ds_single_labels = Recon.solve_x3ds(x2ds_data, x2ds_splits, x2ds_labels2, Ps2) #if not np.all(x3ds_labels == [0,1,2,3,4]): print 'ERROR'; continue # skip if somehow not all points seen #if np.max(x3ds**2) > 1e9: print 'ERROR oh oh',x3ds; continue if rigid_filter: # enforce x3ds must be a rigid transform of the wand mat = rigid_align_points(wand_x3ds, x3ds) x3ds = np.dot(wand_x3ds,mat[:3,:3].T) + mat[:,3] for cs,c0,c1 in zip(camera_solved,x2ds_splits[:-1],x2ds_splits[1:]): #copy 'cleaned' labels for solved cameras to avoid bad data if cs: x2ds_labels[c0:c1] = x2ds_labels2[c0:c1] x2ds_frames.append(x2ds_raw_data) x2ds_splits_frames.append(x2ds_splits) x2ds_labels_frames.append(x2ds_labels) # CBD not x2ds_labels2, otherwise we can't add cameras! x3ds_frames.append(x3ds) num_kept_frames+=1 # TODO collapse this into the code above and clean up x2s_cameras,x3s_cameras,frames_cameras = [],[],[] for ci in xrange(numCameras): orders = [get_order(xlf[xsf[ci]:xsf[ci+1]]) for xlf,xsf in zip(x2ds_labels_frames,x2ds_splits_frames)] which_frames = np.where([o is not None for o in orders])[0] if len(which_frames) == 0: x2s,x3s = np.zeros((0,2),dtype=np.float32),np.zeros((0,3),dtype=np.float32) else: x2s = np.vstack([x2ds_frames[fi][x2ds_splits_frames[fi][ci]:x2ds_splits_frames[fi][ci+1]][orders[fi]] for fi in which_frames]) x3s = np.vstack([x3ds_frames[fi] for fi in which_frames]) x2s_cameras.append(x2s) x3s_cameras.append(x3s) frames_cameras.append(which_frames) return x2s_cameras,x3s_cameras,frames_cameras,num_kept_frames
def boot_cameras_from_wand(wand_frames, cameras_info, lo_focal_threshold=0.5, hi_focal_threshold=4.0, cv_2d_threshold=0.02):
"""
Attempt to boot position of cameras from 2d data containing a wand. This is assumed to be 5 marker T wand.
TODO: Generalise size of wand to allow 120mm, 240mm, 780mm etc variations. Also use actual measurements of wand.
Args:
wand_frames
cameras_info
lo_focal_threshold=0.5
hi_focal_threshold=4.0
cv_2d_threshold=0.02
Returns:
Mat[]: "mats2" - list of GRIP Camera Mats of solved or uninitalised cameras.
bool[]: "camera_solved" flag to show which cameras have been solved in this process.
Requires:
ISCV.label_T_wand
Recon.solve_x3ds
np.linalg.norm
"""
numCameras = len(cameras_info)
numFrames = len(wand_frames)
camera_solved = [False]*numCameras
# use the wand to boot the first camera
x2ds_data,x2ds_splits = wand_frames[0]
x2ds_labels = -np.ones(x2ds_data.shape[0], dtype=np.int32)
ISCV.label_T_wand(x2ds_data, x2ds_splits, x2ds_labels, 2.0, 0.5, 0.01, 0.07)
first_x3ds = np.array([[160, 0, 0],[0, 0, 0],[-80, 0, 0],[0, 0, -120],[0, 0, -240]], dtype=np.float32)
mats2 = [None]*numCameras
first_good_cameras = [None]*numCameras
for ci,(c0,c1) in enumerate(zip(x2ds_splits[:-1], x2ds_splits[1:])):
x2ds = x2ds_data[c0:c1]
labels = x2ds_labels[c0:c1]
try:
order = [list(labels).index(x) for x in range(5)]
except:
mats2[ci] = makeUninitialisedMat(ci, cameras_info[ci])
camera_solved[ci] = False
continue
print ('found wand in camera',ci)
first_good_cameras[ci] = x2ds[order]
cv2_mat = cv2_solve_camera_from_3d(first_x3ds, x2ds[order])
rms = cv2_mat[2]
mats2[ci] = makeMat(cv2_mat[0], cv2_mat[1], cameras_info[ci])
camera_solved[ci] = True
if mats2[ci][0][0,0] < lo_focal_threshold or mats2[ci][0][0,0] > hi_focal_threshold or rms > cv_2d_threshold:
print ('resetting bad camera',ci,'with focal',mats2[ci][0][0,0],'and error',rms)
mats2[ci] = makeUninitialisedMat(ci,cameras_info[ci])
camera_solved[ci] = False
Ps2 = np.array([m[2]/m[0][0,0] for m in mats2],dtype=np.float32)
x2ds_labels2 = x2ds_labels.copy()
for ci in xrange(numCameras): # remove unsolved cameras
if not camera_solved[ci]: x2ds_labels2[x2ds_splits[ci]:x2ds_splits[ci+1]] = -1
x3ds_ret, x3ds_labels, E_x2ds_single, x2ds_single_labels = Recon.solve_x3ds(x2ds_data, x2ds_splits, x2ds_labels2, Ps2)
print (x3ds_ret,first_x3ds) # all points should be within 2.5mm of 'true'
assert(np.allclose(x3ds_ret, first_x3ds, 0.0, 2.5))
# so, we booted some cameras and they reconstruct the wand in the correct place.
# unfortunately, there is still an ambiguity: something like the Necker cube (two different ways we can perceive the wand).
# as soon as the wand moves, we can resolve this
for mfi in xrange(40,numFrames,20):
print (mfi)
x2ds_data,x2ds_splits = wand_frames[mfi]
x2ds_labels = -np.ones(x2ds_data.shape[0],dtype=np.int32)
ISCV.label_T_wand(x2ds_data, x2ds_splits, x2ds_labels, 2.0, 0.5, 0.01, 0.07)
solved_cameras = np.where(camera_solved)[0]
good_cameras = []
second_good_cameras = [None]*numCameras
print (solved_cameras)
for ci in solved_cameras:
c0,c1 = x2ds_splits[ci:ci+2]
x2ds = x2ds_data[c0:c1]
labels = x2ds_labels[c0:c1]
try:
order = [list(labels).index(x) for x in range(5)]
except:
continue
diff = x2ds[order] - first_good_cameras[ci]
if np.linalg.norm(diff) < 0.02*len(diff): continue # must have moved 'enough'
good_cameras.append(ci)
second_good_cameras[ci] = x2ds[order]
print (good_cameras)
if len(good_cameras) >= 3: # this is the good frame...
x2ds_labels2 = x2ds_labels.copy()
for ci in xrange(numCameras): # remove unsolved cameras
if not ci in good_cameras: x2ds_labels2[x2ds_splits[ci]:x2ds_splits[ci+1]] = -1
second_x3ds, second_x3ds_labels, _,_ = Recon.solve_x3ds(x2ds_data, x2ds_splits, x2ds_labels2, Ps2)
for ci in solved_cameras:
if ci not in good_cameras:
print ('resetting bad camera',ci)
mats2[ci] = makeUninitialisedMat(ci,mats2[ci][5])
camera_solved[ci] = False
for ci in good_cameras:
cv2_mat = cv2_solve_camera_from_3d(np.concatenate((first_x3ds,second_x3ds)), np.concatenate((first_good_cameras[ci],second_good_cameras[ci])))
rms = cv2_mat[2]
print (ci,rms)
mats2[ci] = makeMat(cv2_mat[0],cv2_mat[1],mats2[ci][5])
camera_solved[ci] = True
break # finished
return mats2, camera_solved