def __init__(self):
self.target_num_rows = 4
self.target_num_cols = 8
self.target_col_width = 0.233
self.target_row_height = 0.241
# construct the grid of marker points in target coords
tf_target_points = []
for y in range(self.target_num_rows-1, -1, -1):
for x in range(0, self.target_num_cols):
tf_target_points.append(np.array((x, y, 0), dtype=float))
self.tf_target_points = np.array(tf_target_points)
self.tf_target_points[:,0] *= self.target_col_width
self.tf_target_points[:,1] *= self.target_row_height
pm = FCParametrizer()
# initial guesses for cam to head and target to base trasnforms
self.initial_base_H_target = np.eye(4)
self.initial_base_H_target[:3,3] = (-0.8, 0.60, 0.78)
# fix the head to camera transform, and optimize the table position
neck_cam_params = np.array([0.061, 0.161, 0.05, 0.0, 0.0, 3.1415])
self.initial_cam_H_neck = linalg.inv(pm.params_to_matrix(neck_cam_params))
# other params for the estimate
self.ransac_numpoints = 3
self.ransac_numiters = 300
self.inlier_dist = 0.002
def __init__(self, tf_target_points, base_H_target, cam_H_neck, frame):
self.tf_target_points = tf_target_points
self.base_H_target = base_H_target
self.cam_H_neck = cam_H_neck
self.frame = frame
self.bf_target_points = geom.transform_points(self.tf_target_points, self.base_H_target)
self.pm = FCParametrizer()
def __init__(self, frames, tf_target_points, initial_base_H_target, initial_cam_H_neck,
opt_frame, W=np.eye(3)):
self.frames = frames
self.tf_target_points = tf_target_points
self.initial_base_H_target = initial_base_H_target
self.initial_cam_H_neck = initial_cam_H_neck
self.opt_frame = opt_frame
self.W = W
self.pm = FCParametrizer()
class FCSingleFrameOpt:
def __init__(self, tf_target_points, base_H_target, cam_H_neck, frame):
self.tf_target_points = tf_target_points
self.base_H_target = base_H_target
self.cam_H_neck = cam_H_neck
self.frame = frame
self.bf_target_points = geom.transform_points(self.tf_target_points, self.base_H_target)
self.pm = FCParametrizer()
def calc_error(self, params):
#params[:2] = 0.0
cam_H_ccam = self.pm.params_to_matrix(params)
neck_H_ccam = np.dot(linalg.inv(self.cam_H_neck), cam_H_ccam)
err = 0.0
num_corr = 0
base_H_ccam = np.dot(linalg.inv(self.frame.neck_H_base), neck_H_ccam)
for m_i, cf_p in self.frame.visible_markers:
# position of marker in base frame as seen by camera
bf_p = geom.transform_points(cf_p, base_H_ccam)
# position of marker in base frame based on target position
bf_p_target = self.bf_target_points[m_i]
# error
err += linalg.norm(bf_p - bf_p_target)
num_corr += 1
err /= num_corr
self.errors.append(err)
#print len(self.errors), ':', params, ':', '%.3f' % err
return err
def optimize(self):
self.errors = []
initial_params = np.zeros((6,), dtype=np.float)
est_params = optimize.fmin(self.calc_error, initial_params, xtol=0.000000000001, ftol=0.003, maxiter=2000,
maxfun=2000, disp=False)
#est_params[:2] = 0.0
return est_params
class FCOptimizer:
def __init__(self, frames, tf_target_points, initial_base_H_target, initial_cam_H_neck,
opt_frame, W=np.eye(3)):
self.frames = frames
self.tf_target_points = tf_target_points
self.initial_base_H_target = initial_base_H_target
self.initial_cam_H_neck = initial_cam_H_neck
self.opt_frame = opt_frame
self.W = W
self.pm = FCParametrizer()
def calc_error(self, params):
if self.opt_frame == 'target':
base_H_target = self.pm.params_to_matrix(params)
cam_H_neck = self.initial_cam_H_neck
elif self.opt_frame == 'cam':
base_H_target = self.initial_base_H_target
cam_H_neck = self.pm.params_to_matrix(params)
else:
raise NotImplementedError
bf_target_points = pier.geom.transform_points(self.tf_target_points, base_H_target)
err = 0.0
num_corr = 0
for f_i, f in enumerate(self.frames):
base_H_cam = linalg.inv(np.dot(cam_H_neck, f.neck_H_base))
for m_i, cf_p in f.visible_markers:
# position of marker in base frame as seen by camera
bf_p = pier.geom.transform_points(cf_p, base_H_cam)
# position of marker in base frame based on target position
bf_p_target = bf_target_points[m_i]
# error
pdiff = bf_p - bf_p_target
err += np.dot(pdiff, np.dot(self.W, pdiff))
num_corr += 1
err /= num_corr
self.errors.append(err)
if len(self.errors) % 400 == 0:
print len(self.errors), ':', ' '.join(['%.3f' % x for x in params]), ':', '%.3f' % err**0.5
return err
def optimize(self):
self.errors = []
if self.opt_frame == 'target':
initial_params = self.pm.matrix_to_params(self.initial_base_H_target)
elif self.opt_frame == 'cam':
initial_params = self.pm.matrix_to_params(self.initial_cam_H_neck)
else:
raise NotImplementedError
est_params = optimize.fmin(self.calc_error, initial_params, xtol=0.000000000001, ftol=0.000001, maxiter=10000,
maxfun=10000)
if self.opt_frame == 'target':
self.base_H_target = self.pm.params_to_matrix(est_params)
self.cam_H_neck = self.initial_cam_H_neck
elif self.opt_frame == 'cam':
self.base_H_target = self.initial_base_H_target
self.cam_H_neck = self.pm.params_to_matrix(est_params)
else:
raise NotImplementedError
return self.base_H_target, self.cam_H_neck
def print_stats(self):
'''
Assumes self.optimize() has already been called.
'''
bf_target_points = pier.geom.transform_points(self.tf_target_points, self.base_H_target)
per_frame_errors = []
for f_i, f in enumerate(self.frames):
base_H_cam = linalg.inv(np.dot(self.cam_H_neck, f.neck_H_base))
frame_err = 0.0
for m_i, cf_p in f.visible_markers:
# position of marker in base frame as seen by camera
bf_p = pier.geom.transform_points(cf_p, base_H_cam)
# position of marker in base frame based on target position
bf_p_target = bf_target_points[m_i]
# error
frame_err += linalg.norm(bf_p - bf_p_target)
frame_err /= len(f.visible_markers)
per_frame_errors.append(frame_err)
print 'Per frame average errors:', ' '.join(['%6.3f' % frame_err for frame_err in per_frame_errors])
