def __init__(self): self.fd = 500 self.shape = numpy.array([640.0, 480.0]) self.center = numpy.array([319.5, 239.5]) self.position = numpy.array([0,0,-20.0]) self.orientation = Quat(1.0,0,0,0)
class Camera: def __init__(self): self.fd = 500 self.shape = numpy.array([640.0, 480.0]) self.center = numpy.array([319.5, 239.5]) self.position = numpy.array([0,0,-20.0]) self.orientation = Quat(1.0,0,0,0) def project_points(self, points): M = self.orientation.rot() tp = numpy.dot((points - self.position), M) return self.intrinsic_transform(tp) def intrinsic_transform(self, points): nf = self.fd / points[:,2] return self.center + numpy.c_[points[:,0] * nf, points[:,1] * nf] def set_position(self, pos): self.position = pos def set_orientation(self, ori): self.orientation = ori def plot_points(self, ax, scn): pp = self.project_points(scn.points) ax.plot(pp[:,0], pp[:,1], 'bo') ax.axis('equal') ax.axis([0,self.shape[0], self.shape[1], 0]) def plot_edges(self, ax, scn): ip = self.project_edges(scn) for k in xrange(ip.shape[0]/2): ax.plot([ip[2*k,0], ip[2*k+1,0]], [ip[2*k,1], ip[2*k+1,1]], 'r-', lw=2, color=dir_colors[int(scn.edges[k,0])]) ax.axis('equal') ax.axis([0,self.shape[0], self.shape[1], 0]) def edgels_from_edges(self, scn): ip = self.project_edges(scn) out = numpy.zeros((ip.shape[0]/2, 4)) for k in xrange(ip.shape[0]/2): aa = ip[2 * k] bb = ip[2 * k + 1] direction = numpy.arctan2(*(aa - bb)) out[k] = numpy.r_[(aa + bb) * .5, numpy.cos(direction), -numpy.sin(direction)] return out def project_edges(self, scn): M = self.orientation.rot() pp = [] for d,x1,x2,y,z in scn.edges: if d == 0: pp.append([x1,y,z]) pp.append([x2,y,z]) elif d == 1: pp.append([z,x1,y]) pp.append([z,x2,y]) elif d == 2: pp.append([y,z,x1]) pp.append([y,z,x2]) return self.project_points(numpy.array(pp)) def plot_edgels(self, ax, edgels): scale = 20.0 ax.plot((edgels[:,[0,0]] - scale*numpy.c_[-edgels[:,3], edgels[:,3]]).T, (edgels[:,[1,1]] + scale*numpy.c_[-edgels[:,2], edgels[:,2]]).T, '-',lw=3.0,alpha=1.0,color='#ff0000')
if __name__ == '__main__': with open('bundle.out') as fp: aa = fp.readlines() Nc = int(aa[1].split(' ')[0]) Lori = zeros((Nc, 3,3)) for n in range(Nc): for j in range(3): Lori[n,j] = array([float(x) for x in aa[3+n*5+j].split(' ')]) Lq = zeros((Nc,4)) for n in range(Nc): ori = Quat(mtq(Lori[n])) Lq[n] = ori.normalize().q * array([1,-1,1,1]) ## Read the tardiff output ## Get orientations ordered by frame number Lr = loadtxt('tardiff_undistorted_quaternions.dat') ## fix and normalize reference quaternions. fix_reference_quaternions(Lr) Le = zeros((Nc,4))
def estimate_orientation(process_args, image_file_str): seterr(divide='ignore') ## Avoid zero divide warnings tt_total = utime() ## Edgel extraction parameters gmethod = process_args['grid']['method'] gspc = process_args['grid']['size'] glim = process_args['grid']['lim'] ## Optimization parameters ransac_iterations = process_args['ransac_itr'] optimization_tolerance = process_args['op_tol'] ## Parameters from the error function. Default is Tukey bisquare with s=0.15 error_function_parameters = array(process_args['err_func']) intrinsic_parameters = process_args['int_parm'] ################################################################# ## Load image and initialize pic object tt_init = utime() ## Creates picture object pic = Picture(intrinsic_parameters, image_file_str) if process_args['smooth'] is not None: pic.smooth(process_args['smooth']) ## Extract the edgels from the image using the grid mask tt_edgel_extraction = utime() pic.extract_edgels(gspc, glim, method=gmethod) tt_edgel_extraction = utime() - tt_edgel_extraction ## Caculate the edgel normals (interpretation plane), and Jacobians. pic.calculate_edgel_normals() pic.calculate_edgel_jacobians() ## Calculate initial estimate tt_initial_estimate = utime() qini = pic.random_search(ransac_iterations, error_function_parameters) qini = qini.canonical() tt_initial_estimate = utime() - tt_initial_estimate ## Perform second-stage continuous optimization procedure, based on FilterSQP. tt_filtersqp = utime() sqp_funcs = (val_c, grad_c, hess_c, val_f, grad_f, hess_f) args_f = (pic.edgels, pic.i_param, error_function_parameters) filterSQPout = filtersqp.filterSQP( qini.q, .0, 1e-3, sqp_funcs, args_f, delta_tol=optimization_tolerance ) # try: # filterSQPout = filtersqp.filterSQP( # qini.q, .0, 1e-3, sqp_funcs, args_f, delta_tol=optimization_tolerance # ) # except: # print '*** Numerical error for input:' # print {'img_md5': get_hash(image_file_str), # 'proc_args': process_args} # raise SystemExit xo, err, sqp_iters,Llam,Lrho = filterSQPout qopt = Quat(xo) tt_filtersqp = utime() - tt_filtersqp tt_total = utime() - tt_total first_orientation_estimate = qini.canonical().q.tolist() final_orientation_estimate = qopt.canonical().q.tolist() output_data = { 'input': {'img_md5': get_hash(image_file_str), 'proc_args': process_args}, 'time': { 'total': tt_total, 'edgel': tt_edgel_extraction, 'ransac': tt_initial_estimate, 'sqp': tt_filtersqp }, 'Nedgels': pic.edgels.shape[0], 'sqp_itr': sqp_iters, 'ransac_ori_est': first_orientation_estimate, 'ori_est': final_orientation_estimate, } return output_data, pic
if __name__ == "__main__": with open("bundle.out") as fp: aa = fp.readlines() Nc = int(aa[1].split(" ")[0]) Lori = zeros((Nc, 3, 3)) for n in range(Nc): for j in range(3): Lori[n, j] = array([float(x) for x in aa[3 + n * 5 + j].split(" ")]) Lq = zeros((Nc, 4)) for n in range(Nc): ori = Quat(mtq(Lori[n])) Lq[n] = ori.normalize().q * array([1, -1, 1, 1]) ## Read the Corisco output it = 10000 gs = 1 # b1 = array(load_json_dump('solutions_apa.dat', 'set-apa09.json', it, gs)) # b2 = array(load_json_dump('solutions_apa.dat', 'set-apa08.json', it, gs)) b1 = array(load_json_dump("solutions_apa.dat", "set-apa09.json", it, gs, smooth=1.0)) b2 = array(load_json_dump("solutions_apa.dat", "set-apa08.json", it, gs)) ## Get orientations ordered by frame number Lr = r_[b1[argsort(b1[:, 0]), 1:], b2[argsort(b2[:, 0]), 1:]] ## fix and normalize reference quaternions.
if __name__ == '__main__': with open('bundle.out') as fp: aa = fp.readlines() Nc = int(aa[1].split(' ')[0]) Lori = zeros((Nc, 3,3)) for n in range(Nc): for j in range(3): Lori[n,j] = array([float(x) for x in aa[3+n*5+j].split(' ')]) Lq = zeros((Nc,4)) for n in range(Nc): ori = Quat(mtq(Lori[n])) Lq[n] = ori.normalize().q * array([1,-1,1,1]) ## Read the Corisco output # it = 10000 # gs = 1 it = int(sys.argv[1]) gs = int(sys.argv[2]) b1 = array(load_json_dump('big_data_apa09.dat', it, gs)) #b1 = array(load_json_dump('apa09_smooth1.0.dat', it, gs)) b2 = array(load_json_dump('big_data_apa08.dat', it, gs)) ## Order by frame number b1 = b1[argsort(b1[:,0]), 1:] b2 = b2[argsort(b2[:,0]), 1:]