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