def sbaDriver(camname, ptsname, intrname=None, camoname=None, ptsoname=None):
logging.debug("sbaDriver() {0} {1}".format(camname, ptsname))
if intrname is None:
cameras = sba.Cameras.fromTxt(camname) # Load cameras from file
else:
# Intrinsic file for all identical cameras not yet implemented
logging.warning("Fixed intrinsic parameters not yet implemented")
cameras = sba.Cameras.fromTxtWithIntr(camname, intrname)
points = sba.Points.fromTxt(ptsname, cameras.ncameras) # Load points
options = sba.Options.fromInput(cameras, points)
#options.camera = sba.OPTS_CAMS_NODIST # only use this if passing in 5+3+3
options.nccalib = sba.OPTS_FIX2_INTR # fix all intrinsics
# If you wish to fix the intrinsics do so here by setting options
options.ncdist = sba.OPTS_FIX5_DIST # fix all distortion coeffs
newcameras, newpoints, info = sba.SparseBundleAdjust(
cameras, points, options)
info.printResults()
if camoname:
newcameras.toTxt(camoname)
if ptsoname:
newpoints.toTxt(ptsoname)
def bundleAdjustment(self):
np.savetxt('camarasMegaTotales',self.camera_params[:,:],"%4.5f")
print self.camera_params[:,5:].shape
cameras= sba.Cameras.fromTxt('camarasMegaTotales')
print "shapes Pont Params: "
print self.pointParams.shape
np.savetxt("puntosTotales",self.pointParams,"%4.5f")
points = sba.Points.fromTxt('puntosTotales',cameras.ncameras)
options = sba.Options.fromInput(cameras,points)
options.nccalib=sba.OPTS_FIX5_INTR
self.newcams, self.newpts, info = sba.SparseBundleAdjust(cameras,points,options)
self.puntos3dTotal=self.newpts.B
self.newcams.toTxt('nuevascamaraspro')
def fix(self):
print('Found ' + str(self.pts.shape[0]) + ' 3D points')
print('Passing points to SBA...')
sys.stdout.flush()
points = sba.Points.fromDylan(self.pts)
cameras = sba.Cameras.fromDylan(np.hstack((self.cams, self.ext)))
cameras.toTxt(self.name + '-sba-profile-orig.txt')
options = sba.Options.fromInput(cameras, points)
options.camera = sba.OPTS_CAMS
if self.modeString[0] == '0':
options.nccalib = sba.OPTS_FIX5_INTR # fix all intrinsics
elif self.modeString[0] == '1':
options.nccalib = sba.OPTS_FIX4_INTR # optimize focal length
elif self.modeString[0] == '2':
options.nccalib = sba.OPTS_FIX2_INTR # optimize focal length and principal point
if self.modeString[1] == '0':
# If you wish to fix the intrinsics do so here by setting options
options.ncdist = sba.OPTS_FIX5_DIST # fix all distortion coeffs
elif self.modeString[1] == '1':
options.ncdist = sba.OPTS_FIX4_DIST # optimize k2
elif self.modeString[1] == '2':
options.ncdist = sba.OPTS_FIX3_DIST # optimize k2, k4
elif self.modeString[1] == '3':
options.ncdist = sba.OPTS_FIX0_DIST # optimize all distortion coefficients
newcameras, newpoints, info = sba.SparseBundleAdjust(
cameras, points, options)
info.printResults()
sys.stdout.flush()
key = self.id_generator()
# write out temp files as a means of passing data to grapher
newcameras.toTxt(self.temp + '/' + key + '_cn.txt')
newpoints.toTxt(self.temp + '/' + key + '_np.txt')
# write out cameras for the user in the original order
camO = np.loadtxt(self.temp + '/' + key + '_cn.txt')
if self.order is not None:
camO = camO[list(self.order), :]
camO = pandas.DataFrame(camO)
camO.to_csv(self.name + '-sba-profile.txt',
header=False,
index=False,
sep=' ')
# newcameras.toTxt(self.name + '-sba-profile.txt')
if self.ppts is not None:
npframes = self.ppts.shape[0]
else:
npframes = None
if self.uppts is not None:
nupframes = self.uppts.shape[0]
else:
nupframes = None
if self.ref is not None:
refBool = True
else:
refBool = False
uvs = list()
for k in range(self.ncams):
# print self.cams[k]
uvs.append(
undistort_pts(self.pts[:, 3 + 2 * k:3 + 2 * (k + 1)],
self.cams[k]))
if self.ref is not None:
nRef = self.ref.shape[0]
else:
nRef = 0
# nppts - Number of triangulatable paired point correspondences in both tracks
# nuppts - Number of triangulatable unpaired point correspondences in N tracks
# scale - Wand distance
# refBool - Are there reference points?
# indices - Dictionary for paired and unpaired indices in the original CSV file
# ncams - Number of cameras
# npframes - Number of frames, columns, in the paired CSV file, not inculding header
# nupframes - Number of frames in the unpaired CSV file
# name - tag and location for output files
# temporary directory
# Boolean to decide on graphing
# note that wandGrapher also does lots of other tasks: alignment, generating and saving DLT coefficients, etc.
grapher = wandGrapher(key, self.nppts, self.nuppts, self.scale,
refBool, self.indices, self.ncams, npframes,
nupframes, self.name, self.temp, self.display,
uvs, nRef, self.order, self.report, self.cams)
if self.display:
print('Graphing and writing output files...')
sys.stdout.flush()
if self.report:
root = Tk()
outliers, index = grapher.graph()
# outliers = sorted(outliers)
nps = np.loadtxt(self.name + '-sba-profile.txt')
if self.outputCameraProfiles:
for k in range(len(nps)):
l = nps[k]
l = np.insert(l, 0, 1.)
l = np.delete(l, [5] + list(np.arange(11, 18)), axis=0)
np.savetxt(self.name + '-camera-' + str(self.order[k] + 1) +
'-profile.txt',
np.asarray([l]),
fmt='%-1.5g')
def redo():
if self.display and self.report:
root.destroy()
self.pts = np.delete(self.pts, index, axis=0)
# print 'Index length: {0}'.format(len(index))
# a = 0
tmp = self.indices['paired']
# print 'Length of all paired indices: {0}'.format(len(tmp[0]) + len(tmp[1]))
if tmp is not None:
for k in range(len(outliers)):
if '(set 1)' in outliers[k][2]:
# print 'removed outlier'
try:
tmp[0].remove(outliers[k][0] - 1)
# a += 1
self.nppts -= 1
except:
pass
elif '(set 2)' in outliers[k][2]:
# print 'removed outlier'
try:
tmp[1].remove(outliers[k][0] - 1)
# a += 1
self.nppts -= 1
except:
pass
# print 'Number of paired indices removed: {0}'.format(a)
# print len(self.pts)
self.indices['paired'] = copy.copy(tmp)
# print len(self.indices['paired'][0]) + len(self.indices['paired'][0])
tmp = copy.copy(self.indices['unpaired'])
if tmp is not None:
for k in range(len(outliers)):
if 'Unpaired' in outliers[k][2]:
try:
tmp[outliers[k][-1]].remove(outliers[k][0] - 1)
self.nuppts -= 1
except:
pass
self.indices['unpaired'] = tmp
print('\nRunning again with outliers removed...')
self.fix()
def exitLoop():
if self.display and self.report:
root.destroy()
# sys.exit()
if self.report:
if len(outliers) == 0:
root.withdraw()
six.moves.tkinter_messagebox.showwarning(
"No outliers", "No outliers were found! Exiting...")
root.mainloop()
else:
table = Texttable()
table.header([
'Frame', 'Undistorted Pixel Coordinate', 'Point Type',
'Error'
])
for k in range(len(outliers)):
if len(outliers[k]) == 4:
table.add_row(outliers[k])
else:
table.add_row(outliers[k][:-1])
if self.display:
root.resizable(width=FALSE, height=FALSE)
root.wm_title("Argus - Outlier report")
Label(root,
text='Found ' + str(len(outliers)) +
' possible outliers:',
font="-weight bold").grid(row=0,
column=0,
sticky=W,
padx=10,
pady=10)
scrollbar = Scrollbar(root, width=20)
log = Text(root,
yscrollcommand=scrollbar.set,
bg="white",
fg="black")
log.grid(row=1, column=0, padx=10, pady=5)
scrollbar.grid(row=1, column=1, padx=5, pady=5, sticky=NS)
scrollbar.configure(command=log.yview)
# Label(root, text = 'Try again without these outliers?').grid(row = 2,
# column = 0, padx = 10, pady = 10, sticky = W)
yes = Button(root,
text='Try Again',
command=redo,
padx=5,
pady=5)
yes.grid(row=2, column=0, padx=5, pady=10)
no = Button(root,
text='Happy with calibration',
command=exitLoop,
padx=5,
pady=5)
no.grid(row=2, column=0, padx=50, sticky=W)
log.insert(END, table.draw())
"""
f = mplfig.Figure(figsize=(5,4), dpi=100)
a = f.add_subplot(111)
x = np.zeros(len(outliers))
y = np.zeros(len(outliers))
for k in range(len(outliers)):
x[k] = outliers[k][1][0]
y[k] = outliers[k][1][1]
a.plot(x, y, 'ro')
canvas = tkagg.FigureCanvasTkAgg(f, master=root)
canvas.show()
canvas.get_tk_widget().grid(row = 1, column = 2, padx = 5, pady = 5)
"""
root.mainloop()
else:
print('Found ' + str(len(outliers)) +
' possible outliers:')
print(table.draw())
go_again = input(
'Try again without these outliers? (Y/n): ')
if go_again == 'y' or go_again == 'Y':
redo()
else:
exitLoop()
else:
exitLoop()
def bundle_adjust(self, n):
i = len(self.mapmem.patch_3d_pts) - n
count = 0
cameras = []
list3d = []
for feats in range(len(self.mapmem.patch_3d_pts[i])):
featpt = self.mapmem.patch_3d_pts[i][feats]
list3d.append([featpt[0], featpt[1], featpt[2]])
fx = self.camera_matrix[0, 0]
fy = self.camera_matrix[1, 1]
cx = self.camera_matrix[0, 2]
cy = self.camera_matrix[1, 2]
ar = fy / fx
s = 0
ds1 = self.distortion_coeff[0]
ds2 = self.distortion_coeff[1]
ds3 = self.distortion_coeff[2]
ds4 = self.distortion_coeff[3]
ds5 = self.distortion_coeff[4]
x = np.zeros((len(list3d), n, 2), dtype=np.double)
vmask = np.zeros((len(list3d), n), dtype=np.byte)
for j in range(i, len(self.mapmem.patch_3d_pts)):
# print 'mat',self.mapmem.rot_mats[j]
quat = Quaternion(self.mapmem.rot_mats[j])
# a = Matrix33(quat)
# print type(a)
# print a
# print a[0]
# print a[2][1]
# print 'quat',quat
trans = self.mapmem.trans_mats[j]
tx = trans[0]
ty = trans[1]
tz = trans[2]
# trans = self.mapmem.cam_poses[j]
# tx = trans[0]
# ty = trans[1]
# tz = trans[2]
q1 = quat[0]
q2 = quat[1]
q3 = quat[2]
# fx cx cy AR s r2 r4 t1 t2 r6 qi qj qk tx ty tz
cameras.append((fx, cx, cy, ar, s, ds1, ds2, ds3, ds4, ds5, q1, q2,
q3, tx, ty, tz))
for val in range(len(self.mapmem.patch_3d_pts[j])):
obj = self.mapmem.patch_3d_pts[j][val]
imgpt = self.mapmem.patch_2d_pts[j][val]
# ind = np.where(np.all(vals==list3d,axis=1))[0][0]
ind = list3d.index([obj[0], obj[1], obj[2]])
x[ind][count][0] = imgpt[0]
x[ind][count][1] = imgpt[1]
vmask[ind][count] = 1
count += 1
# print sba.Cameras(cameras)
# print len(list3d)
# print x
# print cameras
if count != n:
print 'count error in bundle adjustment!!'
# print vmask.shape
sba_points = sba.Points(list3d, x, vmask)
sba_cameras = sba.Cameras(cameras)
# print sba_cameras.camarray
# print 'pts',sba_points.X
# print 'cams',sba_cameras
# print sba.Options.optsToC
# options = sba.Options
# print options.optsToC
newcams, newpts, info = sba.SparseBundleAdjust(sba_cameras, sba_points)
for count, real_idx in enumerate(
range(i, len(self.mapmem.patch_3d_pts))):
new_cam_tr = np.array([[newcams.camarray[count, 13]],
[newcams.camarray[count, 14]],
[newcams.camarray[count, 15]]])
quatr = Quaternion()
# print 'newcam_',new_cam_tr
quatr.x = newcams.camarray[count, 10]
quatr.y = newcams.camarray[count, 11]
quatr.z = newcams.camarray[count, 12]
quatr.w = math.sqrt(1 - float(quatr.x)**2 - float(quatr.y)**2 -
float(quatr.z)**2)
rot = Matrix33(quatr)
rot_mat = np.array([[rot[0][0], rot[0][1], rot[0][2]],
[rot[1][0], rot[1][1], rot[1][2]],
[rot[2][0], rot[2][1], rot[2][2]]])
pose = np.dot(-(self.mapmem.rot_mats[real_idx].T), new_cam_tr)
self.mapmem.replace_cam_poses_sba(pose[:, 0], real_idx)
print pose
# print 'qss',quatr
# print newcams.camarray[count]
# print math.sqrt(1-0^2-0^2)
# new_cam_quat =
# print quat.normalised
# new_cam_rot = Matrix33(Quaternion())
# print newcams.camarray[count]
# print Matrix33(quat)
# print new_cam_tr
# print newcams.camarray
# print sba.Options.optsToC
# for camera_array,new_pts in zip(newcams.camarray,newpts.X:
# for a in range(n):
# new2dpts = newpts.X[:,a,:]
# print newpts.X
# print info
print 'done'