def reProjectResidualwithX(nx, *args): shp = args[0] Xp_1 = args[1] Xp_2 = args[2] k = args[3] R = args[4] t = args[5] stackedx = nx[0:shp[0]*shp[1]] #x = nx[shp[0]*shp[1]:shp[0]*shp[1]+6] Str_4D = stackedx.reshape(shp) #R = cv2.Rodrigues(x[0:3]) #t = np.array(x[3:6]).reshape(3,1) myC1 = Camera.myCamera(k) myC2 = Camera.myCamera(k) myC1.projectiveMatrix(np.mat([0,0,0]).transpose(),[0, 0, 0]) myC2.projectiveMatrix(np.mat(t),R[0]) rXp_1 = np.mat(myC1.project(Str_4D)) rXp_2 = np.mat(myC2.project(Str_4D)) res_1 = Xp_1 - rXp_1 res_2 = Xp_2 - rXp_2 Res = np.hstack((res_1,res_2)).reshape(-1) nRes = 2*np.sqrt(np.sum(np.power(Res,2))/len(Res)) return nRes
def reProjectResidual(x, *args): Str_4D = args[0] Xp_1 = args[1] Xp_2 = args[2] k = args[3] R = cv2.Rodrigues(x[0:3]) t = np.array(x[3:6]).reshape(3,1) myC1 = Camera.myCamera(k) myC2 = Camera.myCamera(k) myC1.projectiveMatrix(np.mat([0,0,0]).transpose(),[0, 0, 0]) myC2.projectiveMatrix(np.mat(t),R[0]) rXp_1 = np.mat(myC1.project(Str_4D)) rXp_2 = np.mat(myC2.project(Str_4D)) res_1 = Xp_1 - rXp_1 res_2 = Xp_2 - rXp_2 Res = np.hstack((res_1,res_2)).reshape(-1) nRes = 2*np.sqrt(np.sum(np.power(Res,2))/len(Res)) return nRes
#Camera.myCamera.showProjectiveView(Xh_1,'-r')
#Camera.myCamera.showProjectiveView(Xh_2,'-b')
#normaliza os pontos de acordo com a projecao
#Xhn_1, Trh_1 = myC1.normalizePoints(Xh_1)
#Xhn_2, Trh_2 = myC2.normalizePoints(Xh_2)
#Camera.myCamera.showProjectiveView(xn_1,'-r')
#Camera.myCamera.showProjectiveView(xn_2,'-b')
#Xp_1 = np.hstack((Xh_1[:,0], Xh_1[:,1]))
#Xp_2 = np.hstack((Xh_2[:,0], Xh_2[:,1]))
myC1 = Camera.myCamera(k)
myC1.projectiveMatrix(np.mat([0,0,0]).transpose(),[0, 0, 0])
#retorna pontos correspondentes
Xp_1, Xp_2 = clsReconstruction.getMathingPoints('b4.jpg','b5.jpg','k_cam_hp.dat')
#evaluate the essential Matrix using the camera parameter(using the original points)
E, mask0 = cv2.findEssentialMat(Xp_1,Xp_2,k,cv2.FM_RANSAC)
#evaluate the fundamental matrix (using the normilized points)
#F, mask = cv2.findFundamentalMat(Xp_1,Xp_2,cv2.FM_RANSAC)
#ki = np.linalg.inv(k)
R1, R2, t = cv2.decomposeEssentialMat(E)
def sparceRecostructionTrueCase(file1,file2,kdef):
k = np.mat(clsReconstruction.loadData(kdef))
ki = np.linalg.inv(k)
im_1 = cv2.imread(file1)
im_2 = cv2.imread(file2)
im_b1 = cv2.cvtColor(im_1,cv2.COLOR_RGB2GRAY)
im_b2 = cv2.cvtColor(im_2,cv2.COLOR_RGB2GRAY)
myC1 = Camera.myCamera(k)
myC2 = Camera.myCamera(k)
#place camera 1 at origin
myC1.projectiveMatrix(np.mat([0,0,0]).transpose(),[0, 0, 0])
#return macthing points
Xp_1, Xp_2 = clsReconstruction.getMatchingPoints(file1,file2,kdef,30)
#evaluate the essential Matrix using the camera parameter(using the original points)
E, mask0 = cv2.findEssentialMat(Xp_1,Xp_2,k,cv2.FM_RANSAC)
#evaluate Fundamental to get the epipolar lines
#since we already know the camera intrincics, it is better to evaluate F from the correspondence rather than from the 8 points routine
F = ki.T*np.mat(E)*ki
#retrive R and t from E
retval, R, t, mask2 = cv2.recoverPose(E,Xp_1,Xp_2)
#place camera 2
myC2.projectiveMatrix(np.mat(t),R)
#clsReconstruction.drawEpipolarLines(Xp_1,Xp_2,F,im_1,im_2)
#triangulate points
Str_4D = cv2.triangulatePoints(myC1.P[:3],myC2.P[:3],Xp_1.transpose()[:2],Xp_2.transpose()[:2]).T
#make them euclidian
Str_3D = cv2.convertPointsFromHomogeneous(Str_4D).reshape(-1,3)
#evaluate reprojection
Xh_Reprojection_1 = myC1.project(Str_4D)
Xh_Reprojection_2 = myC2.project(Str_4D)
#three ways to carry on the bundle adjustment I am using R,t and K as parameters. using the points is too time
# consuming although the results are much better;
#nR,nt, R0, R1 = clsReconstruction.bundleAdjustment(Str_4D,Xp_1,Xp_2,k,R,t)
#Str_4D, nR,nt, R0, R1 = clsReconstruction.bundleAdjustmentwithX(Str_4D,Xp_1,Xp_2,k,R,t) #### not working right now...
nk, nR, nt, R0, R1 = clsReconstruction.bundleAdjustmentwithK(Str_4D,Xp_1,Xp_2,k,R,t)
print('old value {0:.3f}, optimized pose: {1:.3f} \n'.format(R0,R1))
nki = np.linalg.inv(nk)
#reevaluate essential and fundamental matrixes
nE = clsReconstruction.skew(nt)*np.mat(nR)
nF = nki.T*np.mat(nE)*nki
#if we use the 3th option, we should reinitiate the cameras and the essential matrix, once the projective matrix will change
myC1 = Camera.myCamera(nk)
myC2 = Camera.myCamera(nk)
myC1.projectiveMatrix(np.mat([0,0,0]).transpose(),[0, 0, 0])
#reevaluate all variables based on new values of nR and nt
myC2.projectiveMatrix(np.mat(nt),nR)
Str_4D = cv2.triangulatePoints(myC1.P[:3],myC2.P[:3],Xp_1.transpose()[:2],Xp_2.transpose()[:2]).T
Str_3D = cv2.convertPointsFromHomogeneous(Str_4D).reshape(-1,3)
#Camera.myCamera.show3Dplot(Str_3D)
Xh_Opt_1 = myC1.project(Str_4D)#.reshape(-1,2)
Xh_Opt_2 = myC2.project(Str_4D)#.reshape(-1,2)
#POSSIBLE IMPLEMENTATION find residuals bigger a threshould value and optimize their location in R3
#clsReconstruction.drawEpipolarLines(Xp_1,Xp_2,nF,im_b1,im_b2)
im = clsReconstruction.drawPoints(im_1,Xp_1,(50,50,250))
im = clsReconstruction.drawPoints(im,Xh_Reprojection_1,(50,150,100))
im = clsReconstruction.drawPoints(im,Xh_Opt_1,(250,250,50))
im2 = clsReconstruction.drawPoints(im_2,Xp_2,(50,50,250))
im2 = clsReconstruction.drawPoints(im2,Xh_Reprojection_2,(50,150,100))
im2 = clsReconstruction.drawPoints(im2,Xh_Opt_2,(250,250,50))
cv2.imshow("im",im)
cv2.imshow("im2",im2)
cv2.waitKey(0)
