def projectionPlots(rV, tV, data): ''' compara los puntos de calibracio y sus proyecciones en las tres etapas ''' imagePoints, objectPoints, cameraMatrix, model = data imagePointsProj = cl.direct(objectPoints, rV, tV, cameraMatrix, distCoeffs, model) objectPointsProj = cl.inverse(imagePoints, rV, tV, cameraMatrix, distCoeffs, model) xc, yc, zc = cl.rotoTrasRodri(objectPoints, rV, tV).T xp, yp = cl.ccd2homUndistorted(imagePoints, cameraMatrix, distCoeffs, model) xp2, yp2 = cl.rotoTrasHomog(objectPoints, rV, tV).T ll = np.linalg.norm(objectPoints, axis=0) r = np.mean(ll) / 7 plt.figure() plt.title('image Points') plt.imshow(img) plt.scatter(imagePoints[:, 0], imagePoints[:, 1], marker='+', label='calibration') plt.scatter(imagePointsProj[:, 0], imagePointsProj[:, 1], marker='x', label='projected') plt.legend() #plt.figure() #plt.title('homogenous Points') #plt.scatter(xp, yp, marker='+', label='undistorted from image') #plt.scatter(xp2, yp2, marker='x', label='rptotraslated from map') #plt.legend() plt.figure() plt.title('object Points') plt.scatter(objectPoints[:, 0], objectPoints[:, 1], marker='+', label='calibration') plt.scatter(objectPointsProj[:, 0], objectPointsProj[:, 1], marker='x', label='projected') plt.legend() #fig = plt.figure() #plt.title('3D object Points, camera ref frame') #ax = fig.gca(projection='3d') #ax.axis('equal') #ax.scatter(xc, yc, zc) #ax.plot([0, r], [0, 0], [0, 0], "-r") #ax.plot([0, 0], [0, r], [0, 0], "-b") #ax.plot([0, 0], [0, 0], [0, r], "-k") return imagePointsProj, objectPointsProj
def Esq(imagePoints, objectPoints, rVec, tVec, cameraMatrix, distCoeffs, model): objectPointsProj = cl.inverse(imagePoints, rVec, tVec, cameraMatrix, distCoeffs, model) e = objectPoints - objectPointsProj return np.sum(e**2)
def Esq(imagePoints, objectPoints, rVec, tVec, cameraMatrix, distCoeffs, model): objectPointsProj = cl.inverse(imagePoints, rVec, tVec, cameraMatrix, distCoeffs, model) e = objectPoints - objectPointsProj return np.sum(e**2)
def project2diagonalisedError(xAll): ''' no me queda otra que leer case desde el main como una variable global ''' c = case xm, ym, Cm = cl.inverse(c.xi, c.yi, xAll[:3], xAll[3:], c.cameraMatrix, c.distCoeffs, c.model, c.Ci, c.Cf, c.Ck, c.covOpt, c.Cfk) xNorm, yNorm = cl.points2linearised(xm - c.objpoints[:, 0], ym - c.objpoints[:, 1], Cm).T return np.concatenate([xNorm, yNorm])
def errorCuadraticoImagen(Xext, Xint, Ns, params, j, mahDist=False): ''' el error asociado a una sola imagen, es para un par rvec, tvec necesita tambien los paramatros intrinsecos if mahDist=True it returns the squared mahalanobis distance for the proyection points ''' # saco los parametros de flat para que los use la func de projection cameraMatrix, distCoeffs = flat2int(Xint, Ns, params['model']) rvec, tvec = flat2ext(Xext) # saco los parametros auxiliares imagePoints = params["imagePoints"] model = params["model"] chessboardModel = params["chessboardModel"] Cccd = params["Cccd"] Cf = params["Cf"] Ck = params["Ck"] Crt = params["Crt"] Cfk = params["Cfk"] try: # check if there is covariance for this image Cov = Cccd[j] except: Cov = None # hago la proyeccion xi, yi = imagePoints[j].T xm, ym, Cm = cl.inverse(xi, yi, rvec, tvec, cameraMatrix, distCoeffs, model, Cov, Cf, Ck, Crt[j], Cfk) # error er = ([xm, ym] - chessboardModel[:, :2].T).T Cmbool = anny(Cm) if Cmbool: # devuelvo error cuadratico pesado por las covarianzas S = np.linalg.inv(Cm) # inverse of covariance matrix # distancia mahalanobis Er = np.sum(er.reshape((-1, 2, 1)) * S * er.reshape((-1, 1, 2)), axis=(1, 2)) if not mahDist: # add covariance normalisation error Er += np.linalg.det(Cm) else: # error cuadratico sin pesos ni nada Er = np.sum(er**2, axis=1) return Er
def projectionPlots(rV, tV, data): ''' compara los puntos de calibracio y sus proyecciones en las tres etapas ''' imagePoints, objectPoints, cameraMatrix, model = data imagePointsProj = cl.direct(objectPoints, rV, tV, cameraMatrix, distCoeffs, model) objectPointsProj = cl.inverse(imagePoints, rV, tV, cameraMatrix, distCoeffs, model) xc, yc, zc = cl.rotoTrasRodri(objectPoints,rV,tV).T xp, yp = cl.ccd2homUndistorted(imagePoints, cameraMatrix, distCoeffs, model) xp2, yp2 = cl.rotoTrasHomog(objectPoints, rV, tV).T ll = np.linalg.norm(objectPoints, axis=0) r = np.mean(ll) / 7 plt.figure() plt.title('image Points') plt.imshow(img) plt.scatter(imagePoints[:,0], imagePoints[:,1], marker='+', label='calibration') plt.scatter(imagePointsProj[:,0], imagePointsProj[:,1], marker='x', label='projected') plt.legend() #plt.figure() #plt.title('homogenous Points') #plt.scatter(xp, yp, marker='+', label='undistorted from image') #plt.scatter(xp2, yp2, marker='x', label='rptotraslated from map') #plt.legend() plt.figure() plt.title('object Points') plt.scatter(objectPoints[:,0], objectPoints[:,1], marker='+', label='calibration') plt.scatter(objectPointsProj[:,0], objectPointsProj[:,1], marker='x', label='projected') plt.legend() #fig = plt.figure() #plt.title('3D object Points, camera ref frame') #ax = fig.gca(projection='3d') #ax.axis('equal') #ax.scatter(xc, yc, zc) #ax.plot([0, r], [0, 0], [0, 0], "-r") #ax.plot([0, 0], [0, r], [0, 0], "-b") #ax.plot([0, 0], [0, 0], [0, r], "-k") return imagePointsProj, objectPointsProj
def perform(self, node, inputs_storage, output_storage): # global projCount # projCount += 1 # self.count += 1 # print('IDX %d projection %d, global %d' % # (self.idx, self.count, projCount)) Xint, Xext = inputs_storage # saco los parametros de flat para que los use la func de projection # print(Xint) cameraMatrix, distCoeffs = bl.flat2int(Xint, Ns, model) # print(cameraMatrix, distCoeffs) xy = np.zeros((n, m, 2)) cm = np.zeros((n, m, 2, 2)) for j in range(n): rVec, tVec = bl.flat2ext(Xext[j]) xy[j, :, 0], xy[j, :, 1], cm[j] = cl.inverse(imagePoints.reshape( (n, m, 2))[j], rVec, tVec, cameraMatrix, distCoeffs, model, Cccd=Ci[j]) # print(xy) xy -= objpoints2D S = np.linalg.inv(cm) u, s, v = np.linalg.svd(S) sdiag = np.zeros_like(u) sdiag[:, :, [0, 1], [0, 1]] = np.sqrt(s) A = (u.reshape((n, m, 2, 2, 1, 1)) * sdiag.reshape( (n, m, 1, 2, 2, 1)) * v.transpose((0, 1, 3, 2)).reshape( (n, m, 1, 1, 2, 2))).sum(axis=(3, 4)) xy = np.sum(xy.reshape((n, m, 2, 1)) * A, axis=3) output_storage[0][0] = xy
def errorCuadraticoImagen(Xext, Xint, Ns, params, j): ''' el error asociado a una sola imagen, es para un par rvec, tvec necesita tambien los paramatros intrinsecos ''' # saco los parametros de flat para que los use la func de projection cameraMatrix, distCoeffs = flat2int(Xint, Ns) rvec, tvec = flat2ext(Xext) # saco los parametros auxiliares n, m, imagePoints, model, chessboardModel, Ci = params try: # check if there is covariance for this image Cov = Ci[j] except: Cov = None # hago la proyeccion xm, ym, Cm = cl.inverse(imagePoints[j, 0], rvec, tvec, cameraMatrix, distCoeffs, model, Cccd=Cov) # print(Cm) # error er = ([xm, ym] - chessboardModel[0, :, :2].T).T Cmbool = anny(Cm) if Cmbool: # devuelvo error cuadratico pesado por las covarianzas S = np.linalg.inv(Cm) # inverse of covariance matrix # q1 = [np.sum(S[:, :, 0] * er.T, 1), # fastest way I found to do product # np.sum(S[:, :, 1] * er.T, 1)] # distancia mahalanobis Er = (er.reshape((-1, 2, 1)) * S * er.reshape((-1, 1, 2))).sum() Er += np.log(np.linalg.det(Cm)).sum() # sumo termino de normalizacion else: # error cuadratico pelado, escalar Er = np.sum(er**2) return Er
def perform(self, node, inputs_storage, output_storage): # global projCount # projCount += 1 # self.count += 1 # print('IDX %d projection %d, global %d' % # (self.idx, self.count, projCount)) Xint, Xext = inputs_storage # saco los parametros de flat para que los use la func de projection # print(Xint) cameraMatrix, distCoeffs = bl.flat2int(Xint, Ns, model) # print(cameraMatrix, distCoeffs) xy = np.zeros((n, m, 2)) cm = np.zeros((n, m, 2, 2)) for j in range(n): rVec, tVec = bl.flat2ext(Xext[j]) xy[j, :, 0], xy[j, :, 1], cm[j] = cl.inverse( imagePoints.reshape((n, m, 2))[j], rVec, tVec, cameraMatrix, distCoeffs, model, Cccd=Ci[j]) # print(xy) xy -= objpoints2D S = np.linalg.inv(cm) u, s, v = np.linalg.svd(S) sdiag = np.zeros_like(u) sdiag[:,:,[0,1],[0,1]] = np.sqrt(s) A = (u.reshape((n, m, 2, 2, 1, 1)) * sdiag.reshape((n, m, 1, 2 ,2, 1)) * v.transpose((0,1,3,2)).reshape((n, m, 1, 1, 2, 2)) ).sum(axis=(3,4)) xy = np.sum(xy.reshape((n,m,2,1)) * A, axis=3) output_storage[0][0] = xy
# %% ret = opt.minimize(objective, case.xAllT) xAllOpt = ret.x covOpt = np.exp(ret.fun / 2) * ret.hess_inv sigOpt = np.sqrt(np.diag(covOpt)) crrOpt = covOpt / (sigOpt.reshape((-1, 1)) * sigOpt.reshape((1, -1))) # plt.matshow(crrOpt, vmin=-1, vmax=1, cmap='coolwarm') xm, ym, Cm = cl.inverse(xi, yi, xAllOpt[:3], xAllOpt[3:], cameraMatrix, distCoeffs, model, Ci, Cf, Ck, covOpt, Cfk) plt.figure() ax = plt.gca() ax.scatter(xm, ym) ax.scatter(objpoints[:, 0], objpoints[:, 1]) for i in range(len(xm)): cl.plotEllipse(ax, Cm[i], xm[i], ym[i], 'k') ax.axis('equal') reload(cl) xNorm, yNorm = cl.points2linearised(xm - objpoints[:, 0], ym - objpoints[:, 1], Cm).T
fig = plt.figure() ax = fig.gca() ax.set_title('propagacion') ax.scatter(xM, yM, marker='.', c='b', s=1) cl.plotPointsUncert(ax, Cm, xm, ym, 'k') cl.plotPointsUncert(ax, varM, xMm, yMm, 'b') # %% TESTEO LOS TRES PASOS JUNTOS # hago todos los mapeos de Monte Carlo for posM, posI, pars in zip(posMap, posIgen, parsGen): r, t, k, d = sacoParmams(pars) posM[:,0], posM[:,1], _ = cl.inverse(posI, r, t, k, d, model) # %% saco media y varianza de cada nube de puntos posMapMean = np.mean(posMap, axis=0) dif = (posMap - posMapMean).T posMapVar = np.mean([dif[0] * dif, dif[1] * dif], axis=-1).T # %% mapeo propagando incerteza los valores con los que comparar xmJ, ymJ, CmJ = cl.inverse(imagePoints[j,0], rVecs[j], tVecs[j], cameraMatrix, distCoeffs, model, Cccd, Cf, Ck, Crt) XJ = np.array([xmJ, ymJ]).T
# %% TEST INVERSE MAPPING (DISTORTION MODEL) # pruebo con la imagen j-esima if plotCornersInverse: plt.figure() plt.plot(chessboardModel[0, :, 0], chessboardModel[0, :, 1], 'xr', markersize=10) for j in range(n): # range(len(imgpoints)): rvec = rVecs[j] tvec = tVecs[j] xPos, yPos, cc = cl.inverse(imagePoints[j, 0], rvec, tvec, K, D, model) im = plt.imread(images[j]) print(j, cc, images[j]) plt.plot(xPos, yPos, '+b', markersize=10) # %% START BAYESIAN CALIBRATION from dev import bayesLib as bl from importlib import reload import scipy.stats as sts import sys sys.path.append("/home/sebalander/Code/sebaPhD") reload(cl) reload(bl) # standar deviation from subpixel epsilon used
# saco condiciones iniciales para los parametros uso opencv y heuristica #reload(cl) if model == modelos[3]: # saco rVecs, tVecs de la galera # tvecIni = camPrior # rvecIni = np.array([np.pi, 0, 0]) # camara apuntando para abajo Xext0 = camPrior.copy() rvecIni, tvecIni = bl.flat2ext(Xext0) else: ret, rvecIni, tvecIni, inliers = cv2.solvePnPRansac(calibPts, xIm, cameraMatrix, distCoeffs) Xext0 = bl.ext2flat(rvecIni, tvecIni) xpr, ypr, c = cl.inverse(xIm, rvecIni, tvecIni, cameraMatrix, distCoeffs, model=model) plt.plot(xpr, ypr, '*') # %% std = 1.0 # output file #extrinsicParamsOutFile = extrinsicFolder + camera + model + "intrinsicParamsAP" #extrinsicParamsOutFile = extrinsicParamsOutFile + str(std) + ".npy" Ci = np.repeat([ std**2 * np.eye(2)],n*m, axis=0).reshape(n,m,2,2) params = dict() params["n"] = n params["m"] = m params["imagePoints"] = xIm.reshape((1,1,-1,2))
IP[model] = [] OP[model] = [] # MAP TO HOMOGENOUS PLANE TO GET RADIUS for j in range(n): print('\t imagen', j) rvec = rVecs[j] tvec = tVecs[j] imagePointsProjected = cl.direct(chessboardModel, rvec, tvec, cameraMatrix, distCoeffs, model) imagePointsProjected = imagePointsProjected.reshape((-1, 2)) IP[model].append(imagePointsProjected) objectPointsProjected = cl.inverse(imagePoints[j, 0], rvec, tvec, cameraMatrix, distCoeffs, model) #objectPointsProjected = opbjectPointsProjected.reshape((-1,3)) OP[model].append(objectPointsProjected) if plotCorners: imagePntsX = imagePoints[j, 0, :, 0] imagePntsY = imagePoints[j, 0, :, 1] xPos = imagePointsProjected[:, 0] yPos = imagePointsProjected[:, 1] plt.figure(j) im = plt.imread(images[j]) plt.imshow(im) plt.plot(imagePntsX, imagePntsY, 'xr', markersize=10) plt.plot(xPos, yPos, '+b', markersize=10)
# ## # ## %% go to undistorted homogenous #xp, yp, Cp = cl.homDist2homUndist(xpp, ypp, distCoeffs, model, Cpp=Cpp, Ck=Ck) # #fig = plt.figure() #ax = fig.gca() #for i in range(nPts): # cl.plotEllipse(ax, Cp[i], xp[i], yp[i], 'b') ## # ## %% project to map #xm, ym, Cm = cl.xypToZplane(xp, yp, rV, tV, Cp=Cp, Crt=[Cr, Ct]) # #fig = plt.figure() #ax = fig.gca() #ax.plot(xm, ym, '+', markersize=2) #for i in range(nPts): # cl.plotEllipse(ax, Cm[i], xm[i], ym[i], 'b') # %% in one line xm, ym, Cm = cl.inverse(imagePoints, rV, tV, cameraMatrix, distCoeffs, model, Cccd, Cf, Ck, Crt) fig = plt.figure() ax = fig.gca() ax.plot(xm, ym, '+', markersize=2) for i in range(nPts): cl.plotEllipse(ax, Cm[i], xm[i], ym[i], 'b')
# Cfk = np.eye(distCoeffs.shape[0], 4) # rows for distortion coeffs # Crt = np.eye(6) # 6x6 covariance of pose # Crt[[0,1,2],[0,1,2]] *= np.deg2rad(1)**2 # 5 deg stdev in every angle # Crt[[3,4,5],[3,4,5]] *= 0.01**2 # 1/100 of the unit length as std for pos # Crt = np.repeat([Crt] , n, axis=0) Crt = np.repeat([False], n) # no RT error # output file intrinsicParamsOutFile = imagesFolder + camera + model + "intrinsicParamsML" intrinsicParamsOutFile = intrinsicParamsOutFile + str(stdPix) + ".npy" # pruebo con un par de imagenes for j in range(0, n, 3): xm, ym, Cm = cl.inverse(imagePoints[j, 0], rVecs[j], tVecs[j], cameraMatrix, distCoeffs, model, Cccd=Ci[j], Cf=False, Ck=False, Crt=False, Cfk=False) print(xm, ym, Cm) # datos medidos, observados, experimentalmente en el mundo y en la imagen yObs = objpoints2D.reshape(-1) # no usar las constantes como tensores porque da error... # # diccionario de parametros, constantes de calculo # xObsConst = T.as_tensor_variable(imagePoints.reshape((n,m,2)), 'xObsConst', # ndim=3) # CiConst = T.as_tensor_variable(Ci, 'cIConst', ndim=4)
#fig.savefig("distortedPoints3.png") # # %% TEST INVERSE MAPPING (DISTORTION MODEL) # pruebo con la imagen j-esima if plotCornersInverse: plt.figure() plt.plot(chessboardModel[0,:,0], chessboardModel[0,:,1], 'xr', markersize=10) for j in range(n): # range(len(imgpoints)): rvec = rVecs[j] tvec = tVecs[j] xPos, yPos, cc = cl.inverse(imagePoints[j, 0], rvec, tvec, K, D, model) im = plt.imread(images[j]) print(j, cc, images[j]) plt.plot(xPos, yPos, '+b', markersize=10)
# Crt[[3,4,5],[3,4,5]] *= 0.01**2 # 1/100 of the unit length as std for pos # Crt = np.repeat([Crt] , n, axis=0) Crt = np.repeat([False], n) # no RT error # output file intrinsicParamsOutFile = imagesFolder + camera + model + "intrinsicParamsML" intrinsicParamsOutFile = intrinsicParamsOutFile + str(stdPix) + ".npy" # pruebo con un par de imagenes for j in range(0, n, 3): xm, ym, Cm = cl.inverse(imagePoints[j, 0], rVecs[j], tVecs[j], cameraMatrix, distCoeffs, model, Cccd=Ci[j], Cf=False, Ck=False, Crt=False, Cfk=False) print(xm, ym, Cm) # datos medidos, observados, experimentalmente en el mundo y en la imagen yObs = objpoints2D.reshape(-1) # no usar las constantes como tensores porque da error... # # diccionario de parametros, constantes de calculo # xObsConst = T.as_tensor_variable(imagePoints.reshape((n,m,2)), 'xObsConst', # ndim=3) # CiConst = T.as_tensor_variable(Ci, 'cIConst', ndim=4)
# cl.plotEllipse(ax, Cpp[i], xpp[i], ypp[i], 'b') # ## # ## %% go to undistorted homogenous #xp, yp, Cp = cl.homDist2homUndist(xpp, ypp, distCoeffs, model, Cpp=Cpp, Ck=Ck) # #fig = plt.figure() #ax = fig.gca() #for i in range(nPts): # cl.plotEllipse(ax, Cp[i], xp[i], yp[i], 'b') ## # ## %% project to map #xm, ym, Cm = cl.xypToZplane(xp, yp, rV, tV, Cp=Cp, Crt=[Cr, Ct]) # #fig = plt.figure() #ax = fig.gca() #ax.plot(xm, ym, '+', markersize=2) #for i in range(nPts): # cl.plotEllipse(ax, Cm[i], xm[i], ym[i], 'b') # %% in one line xm, ym, Cm = cl.inverse(imagePoints, rV, tV, cameraMatrix, distCoeffs, model, Cccd, Cf, Ck, Crt) fig = plt.figure() ax = fig.gca() ax.plot(xm, ym, '+', markersize=2) for i in range(nPts): cl.plotEllipse(ax, Cm[i], xm[i], ym[i], 'b')
#reload(cl) if model == modelos[3]: # saco rVecs, tVecs de la galera # tvecIni = camPrior # rvecIni = np.array([np.pi, 0, 0]) # camara apuntando para abajo Xext0 = camPrior.copy() rvecIni, tvecIni = bl.flat2ext(Xext0) else: ret, rvecIni, tvecIni, inliers = cv2.solvePnPRansac( calibPts, xIm, cameraMatrix, distCoeffs) Xext0 = bl.ext2flat(rvecIni, tvecIni) xpr, ypr, c = cl.inverse(xIm, rvecIni, tvecIni, cameraMatrix, distCoeffs, model=model) plt.plot(xpr, ypr, '*') # %% std = 1.0 # output file #extrinsicParamsOutFile = extrinsicFolder + camera + model + "intrinsicParamsAP" #extrinsicParamsOutFile = extrinsicParamsOutFile + str(std) + ".npy" Ci = np.repeat([std**2 * np.eye(2)], n * m, axis=0).reshape(n, m, 2, 2) params = dict() params["n"] = n
IP[model] = [] OP[model] = [] # MAP TO HOMOGENOUS PLANE TO GET RADIUS for j in range(n): print('\t imagen', j) rvec = rVecs[j] tvec = tVecs[j] imagePointsProjected = cl.direct(chessboardModel, rvec, tvec, cameraMatrix, distCoeffs, model) imagePointsProjected = imagePointsProjected.reshape((-1,2)) IP[model].append(imagePointsProjected) objectPointsProjected = cl.inverse(imagePoints[j,0], rvec, tvec, cameraMatrix, distCoeffs, model) #objectPointsProjected = opbjectPointsProjected.reshape((-1,3)) OP[model].append(objectPointsProjected) if plotCorners: imagePntsX = imagePoints[j, 0, :, 0] imagePntsY = imagePoints[j, 0, :, 1] xPos = imagePointsProjected[:, 0] yPos = imagePointsProjected[:, 1] plt.figure(j) im = plt.imread(images[j]) plt.imshow(im) plt.plot(imagePntsX, imagePntsY, 'xr', markersize=10) plt.plot(xPos, yPos, '+b', markersize=10)