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