1、相机标定步骤
1.1 棋盘标定相机思路
1、建立终止准则：
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)#格式固定
2、设置棋盘格规格
objp = np.zeros((w * h, 3), np.float32) #w:棋盘宽度方向上的角点数，h:棋盘高度方向的角点数
objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2) #
objp = objp * p2p#p2p：棋盘上点与点横竖方向的间隔
3、寻找棋盘角点
ret, corners = cv2.findChessboardCorners(gray, (w, h), None) #参数1：寻找的角点的目标图像，参数2：棋盘横竖方向的角点数量。返回值：corners表示角点
4、增加棋盘角点准确度
cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) #参数1：寻找角点的目标图像，参数2：角点，参数5：终止准则，参数3和3为固定格式
5、将角点和棋盘规格储存到变量中
objpoints.append(objp)
imgpoints.append(corners)
6、对相机进行标定
ret, mtx, dist, rvecs, T = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)#参数1：棋盘规格列表，参数2：角点列表，参数3：目标图像固定格式。
返回值：将内参、畸变、旋转、平移矩阵转换成列表形式，为相机透视做做参数准备
mtx = mtx.tolist()
dist = dist.tolist()
r = np.zeros((3,3),dtype='float')
cv2.Rodrigues(rvecs[0],r)
r = r.tolist()
t = np.array(T[0]).reshape((1,-1)).tolist()
1.2 畸变矫正
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))#参数1：内参矩阵，参数2：畸变矩阵 mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)#参数1：内参矩阵 dst = cv2.remap(rawimage, mapx, mapy, cv2.INTER_LINEAR) x, y, W, H = roi dst = dst[y:y + H, x:x + W] #获取的图片是透视变换的基础图片
1.3 透视变换
M = cv2.getPerspectiveTransform(rect_in_image, rect_after_prespect)#参数1：源图像，目标图像
warped = cv2.warpPerspective(dst, M, (int(w), int(h)))
2、实例运用案例1
#内外参矩阵获取 def calIEmat(self): # 找棋盘格角点 # 阈值 try: #棋盘中行列点个数及点间距离 w = int(self.width_num_edit.text()) h = int(self.height_num_edit.text()) p2p = int(self.p2p_edit.text()) except: QMessageBox.information(self, "输出错误", self.tr("请正确填写点数和点距!")) return criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)#终止准则，参数固定格式 # 棋盘格模板规格 # 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵 objp = np.zeros((w * h, 3), np.float32) objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2) objp = objp * p2p # 储存棋盘格角点的世界坐标和图像坐标对 objpoints = [] # 在世界坐标系中的三维点 imgpoints = [] # 在图像平面的二维点 images = glob.glob('%i/*' %self.index) if len(images) < 20: QMessageBox.information(self, "图片数量过少", self.tr("需要有20张以上的图片!")) return for i,fname in enumerate(images): img = cv2.imread(fname) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # 找到棋盘格角点 ret, corners = cv2.findChessboardCorners(gray, (w, h), None)#寻找棋盘角点（指棋盘格黑白焦点个数） # 如果找到足够点对，将其存储起来 if ret == False and i ==0: QMessageBox.information(self, "标定失败", self.tr("基准图片找点异常!")) return if ret == True: cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)#参数1：被处理的图片，参数2：角点，参数5：终止准则。目的：增加找棋盘格角点准确度 objpoints.append(objp) imgpoints.append(corners) # 将角点在图像上显示 cv2.drawChessboardCorners(img, (w, h), corners, ret)#将找到的角点在棋盘中画出来 # plt.imshow(img) # plt.show() resize = cv2.resize(img,(1200,800)) cv2.imshow('findCorners', resize) cv2.waitKey(1) else: print "没找到点！" cv2.destroyAllWindows() # 标定 ret, mtx, dist, rvecs, T = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None) mtx = mtx.tolist() dist = dist.tolist() r = np.zeros((3,3),dtype='float') cv2.Rodrigues(rvecs[0],r) r = r.tolist() t = np.array(T[0]).reshape((1,-1)).tolist() self.params['cam'][str(self.index)]['mtx'] = mtx#内参矩阵 self.params['cam'][str(self.index)]['dist'] = dist#畸变矩阵 self.params['cam'][str(self.index)]['R'] = r#旋转矩阵 self.params['cam'][str(self.index)]['T'] = t#平移矩阵 paramsHandler.updateParams(self.params) self.displayMat() #相机标定 def undistort(self,rawimage,mtx,dist,w,h): newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h)) mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5) dst = cv2.remap(rawimage, mapx, mapy, cv2.INTER_LINEAR) x, y, W, H = roi dst = dst[y:y + H, x:x + W] return dst #透视矩阵 def calPrespectMat(self): try: # p0x = float(self.offset_x_edit.text()) # p0y = float(self.offset_y_edit.text()) # rw = float(self.area_width_edit.text()) # rh = float(self.area_height_edit.text()) p1x = float(self.p1x.text()) p1y = float(self.p1y.text()) p2x = float(self.p2x.text()) p2y = float(self.p2y.text()) p3x = float(self.p3x.text()) p3y = float(self.p3y.text()) p4x = float(self.p4x.text()) p4y = float(self.p4y.text()) width_after_prespect = self.params['vision']['width_after_prespect'] height_after_prespect = self.params['vision']['height_after_prespect'] except: QMessageBox.information(self, "参数错误", self.tr("请填写合理的数字!")) return real_rect = [(p1x,p1y),(p2x,p2y),(p3x,p3y),(p4x,p4y)] w1 = abs(max(p1x,p2x,p3x,p4x) - min(p1x,p2x,p3x,p4x)) w2 = abs(max(p1y,p2y,p3y,p4y) - min(p1y,p2y,p3y,p4y)) real_width = max(w1,w2) #透视变形中每mm像素数量 prespect_width_s = width_after_prespect * 1.0 / real_width # prespect_height_s = height_after_prespect * 1.0 / real_height self.params['cam'][str(self.index)]['prespect_s'] = prespect_width_s rect_in_image = self.getpointvalue(0,real_rect) if rect_in_image is -1: QMessageBox.information(self, "参数错误", self.tr("请先标定内外参矩阵!")) return rect_after_prespect = np.array(((0,0),(width_after_prespect,0), (width_after_prespect,height_after_prespect),(0,height_after_prespect)),dtype='float32') M = cv2.getPerspectiveTransform(rect_in_image, rect_after_prespect) self.params['cam'][str(self.index)]['prespect_mat'] = np.array(M).tolist() mtx = np.array(self.params['cam'][str(self.index)]['mtx']) dist = np.array(self.params['cam'][str(self.index)]['dist']) w = self.params['vision']['width_after_prespect']#？ h = self.params['vision']['height_after_prespect']#？ fname = glob.glob('%i/*' % self.index)[0] img = cv2.imread(fname) #在图片上做几个标记 cv2.circle(img,(100,100),100,(0,255,0),2) cv2.circle(img, (400, 100), 50, (0, 255, 0),2) cv2.circle(img, (100, 400), 30, (0, 255, 0),2) cv2.circle(img, (img.shape[1]-100, img.shape[0]-100), 100, (0, 255, 0),2) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) dst = self.undistort(gray,mtx,dist,w,h) warped = cv2.warpPerspective(dst, M, (int(w), int(h))) resize = cv2.resize(warped, (int(warped.shape[1]*0.5), int(warped.shape[0]*0.5))) resizegray = cv2.resize(gray, (int(gray.shape[1] * 0.5), int(gray.shape[0] * 0.5))) resizedst = cv2.resize(dst, (int(dst.shape[1] * 0.5), int(dst.shape[0] * 0.5))) cv2.imshow("prespect",resize) cv2.imshow("raw",resizegray) cv2.imshow("undistort",resizedst) cv2.waitKey(0) cv2.destroyAllWindows()
3、图像坐标系与机械手坐标系的转换（棋盘标定法）
思路：1、通过相机获取棋盘图片1，然后机械手旋转棋盘，获取棋盘图片2.
2、将图片1和图片2标记为一组，同时记录机械手的坐标
3、总共一起做4组图片，最后一组用来验证
4、对图片进行处理，每张照片在记录棋盘实际中的俩个点，在像素中的位置
5、通过计算，找出每组图片实际上相同位置在图片上俩个点连成直线的中垂线的交点（calcRealPoint函数）
6、运用旋转与平移矩阵、内参矩阵，在计算出中垂线在实际空间上的坐标值（xy2XY函数）
7、通过计算出的三个中垂线点的三个实际值，结合机械手坐标，计算出对应关系（CalcMatrix函数）
8、验证，输入中垂线的实际值，计算出机械手的坐标（CalcXY函数）
xc,yc=0,0 image1 = cv2.imread(image1) # rect = perspective.getpointvalue(0) # before,w,h = perspective.perspectivetransh(image,0,1027.4) image2 = cv2.imread(image2) image = image1/2 + image2/2 i=0 # xb1,yb1 = cornersb[i][0] # xb2,yb2 = cornersb[i+14][0] # xa1,ya1 = cornersa[134][0] # xa2,ya2 = cornersa[120][0] x1m = (xa1+xb1)/2 y1m = (ya1+yb1)/2 x2m = (xa2+xb2)/2 y2m = (ya2+yb2)/2 # k1,k2,xjd,yjd = 0,0,0,0 # if xa1 != xb1 and xa2!= xb2 and ya1 != ya1 and ya2 != yb2: k1 = -1.0*(xa1-xb1)/(ya1-yb1) k2 = -1.0*(xa2 - xb2)/(ya2 - yb2) xjd = ((y2m-y1m)+(k1*x1m-k2*x2m))/(k1-k2) #交点x yjd = k1*xjd + y1m -k1*x1m#交点y xc = xc + xjd yc = yc + yjd # 得到旋转中心，未校准 print xc,yc # 将坐标带入摄像机外部矩阵 xcr,ycr = xy2XY(xc,yc,0) print "x=%f,y=%f" % (xcr,ycr) # camera[j-1] = (xcr,ycr) p = np.array((xcr, ycr, 0, 1)) pc = np.dot(p, np.dot(RT, intr)) pc = pc / pc[2] print pc[0],pc[1] # 画线演示 x1m = int(x1m) y1m = int(y1m) x2m = int(x2m) y2m = int(y2m) xb1 = int(xb1) xb2 = int(xb2) yb1 = int(yb1) yb2 = int(yb2) xa1 = int(xa1) xa2 = int(xa2) ya1 = int(ya1) ya2 = int(ya2) xjd = int(xjd) yjd = int(yjd) cv2.line(image, (xjd, yjd), (x1m, y1m), 255) cv2.line(image, (xjd, yjd), (x2m, y2m), 255) cv2.line(image, (xb1, yb1), (xa1, ya1), 255) cv2.line(image, (xb2, yb2), (xa2, ya2), 255) cv2.circle(image, (xb1, yb1), 3, 255) cv2.circle(image, (xb2, yb2), 3, 255) cv2.circle(image, (xa1, ya1), 3,255) cv2.circle(image, (xa2, ya2), 3, 255) cv2.circle(image, (int(xjd), int(yjd)), 3, 255) image = cv2.resize(image,(int(image.shape[1]*0.4),int(image.shape[0]*0.4))) # winname = "image" + str(j) # cv2.namedWindow("image", cv2.WINDOW_NORMAL) cv2.imshow("image",image) # cv2.waitKey(0) return [xcr, ycr]
def xy2XY(u,v,z): #将像素屏幕内的坐标转化为外部坐标，z为摄像头到外部点的高度,uv位像素屏幕坐标 cm = np.dot(RT,intr) c13umc11 = cm[0][2]*u - cm[0][0] c13vmc12 = cm[0][2]*v - cm[0][1] c23umc21 = cm[1][2]*u - cm[1][0] c23vmc22 = cm[1][2]*v - cm[1][1] c31zac41 = cm[2][0]*z + cm[3][0] c32zac42 = cm[2][1] * z + cm[3][1] c33zac43u = (cm[2][2] * z + cm[3][2])*u c33zac43v = (cm[2][2] * z + cm[3][2]) * v y = ((c31zac41-c33zac43u)/(c13umc11)-(c32zac42-c33zac43v)/(c13vmc12))/((c23umc21)/(c13umc11)-(c23vmc22)/(c13vmc12)) x = -1*c23umc21*y/c13umc11+(-c33zac43u+c31zac41)/c13umc11 return x,y
def CalcMatrix(robot,camera): a=np.zeros((3,3)) matrix=np.zeros((2,3)) b1=np.array((robot[0][0],robot[1][0],robot[2][0])) b2=np.array((robot[0][1],robot[1][1],robot[2][1])) for i in range(0,3): a[i]=np.array((camera[i][0],camera[i][1],1)) matrix[0]=np.linalg.solve(a,b1) matrix[1]=np.linalg.solve(a,b2) print matrix return matrix
def CalcXY(x,y,matrix): slove = np.dot((x, y, 1), np.transpose(matrix)) return slove[0], slove[1]
4、实际运用案例2
#主要矩阵获取
import cv2 import numpy as np import glob criteria=(cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER,30,0.001) objp=np.zeros((19*19,3),np.float32) objp[:,:2]=np.mgrid[0:19,0:19].T.reshape(-1,2) objp=objp*20 objpoints=[] imgpoints=[] images=glob.glob('1/*') for i,fname in enumerate(images): img=cv2.imread(fname) print fname gray=cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) print i ret,corners=cv2.findChessboardCorners(gray,(19,19),None) print i if ret==False and i==0: print "fail" if ret==True: cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria) objpoints.append(objp) imgpoints.append(corners) cv2.drawChessboardCorners(img,(19,19),corners,ret) print i resize=cv2.resize(img,(1200,800)) cv2.imwrite('0/findCorners{}.bmp'.format(i),resize) cv2.waitKey(1) else: print "没有找到" ret,mtx,dist,rvecs,T=cv2.calibrateCamera(objpoints,imgpoints,gray.shape[::-1],None,None) mtx=mtx.tolist()#内参矩阵 dist=dist.tolist()#畸变矩阵 r=np.zeros((3,3),dtype='float')#旋转矩阵 cv2.Rodrigues(rvecs[0],r) r.tolist() t=np.array(T[0]).reshape((1,-1)).tolist()#平移矩阵
#coding=utf-8 透视变换 import cv2 import numpy as np image=cv2.imread("1/0.bmp") cv2.imshow("-Imagewdsd",image) image1=np.uint8(image) cv2.imshow("-Imagasdae1",image1) realrect = np.array(((680, -110),(680, 550),(-150, 550),(-150, -110)), dtype = "float32") #内参矩阵 mtx=np.array([[3503.2070085750343, 0.0, 1514.4329277183597], [0.0, 3504.0911621197565, 1070.0662685362333], [0.0, 0.0, 1.0]]) #内参矩阵行列互换 intr=map(list,zip(*mtx)) #畸变矩阵 # dist=np.array([[-0.09253522982007183, # 0.28611006818704393, # 0.00031988309555288686, # -0.00038812104294333216, # -1.0416930327050236]]) dist=np.array([[-0.09253522982007183, 0.28611006818704393, 0, 0, 0]]) #旋转矩阵，需要对Opencv函数获取的内参矩阵进行转秩 r=np.array(([ 0.04662873, -0.99886898,0.00930149], [ 0.9988022,0.04648333,-0.01527983], [ 0.01483019,0.01000283,0.99983999]), dtype = "float32").T t=np.array([[186.4710217500726, -286.7778264905354, 1242.0236930996398]]) RT = np.concatenate((r, t)) # RT=np.array(([ 0.04662873, -0.99886898,0.00930149], # [ 0.9988022,0.04648333,-0.01527983], # [ 0.01483019,0.01000283,0.99983999], # [186.4710217500726, -286.7778264905354, 1242.0236930996398]) # , dtype = "float32") boardwmm = 400#515 boardhmm = 400#455 calrectWmm = 830#830 calrectHmm = 630 def pixelx2mm(pixel,h): return pixel*(h / intr[0][0]) def pixely2mm(pixel,h): return pixel*(h / intr[1][1]) def mm2pixelx(xmm,h): return xmm/(h / intr[0][0]) def mm2pixely(ymm,h): return ymm/(h / intr[1][1]) def getcoreboardpixel(h): return int(mm2pixelx(boardwmm,h)),int(mm2pixely(boardhmm,h)) def getcalrectpixelint(h): return int(mm2pixelx(calrectWmm,h)),int(mm2pixely(calrectHmm,h)) def getcalrectpixelfloat(h): return int(mm2pixelx(calrectWmm,h)),int(mm2pixely(calrectHmm,h)) newcameratx,roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(3072,2048),1,(3072,2048)) mapx,mapy=cv2.initUndistortRectifyMap(mtx,dist,None,newcameratx,(3072,2048),5) dst=cv2.remap(image,mapx,mapy, cv2.INTER_LINEAR) cv2.imshow("-Dasdst",dst) def getpointvalue(z): imgrect = np.zeros((4,2), dtype="float32") for i, point in enumerate(realrect): p = np.array((point[0],point[1],z,1)) pc = np.dot(np.dot(p, RT), intr) pc = pc/pc[2] imgrect[i] = (np.float32(pc[0]),np.float32(pc[1])) return imgrect def perspectivetransh(image, z, h): rect = getpointvalue(z)#以标定图片的为参照 maxw,maxh = getcalrectpixelfloat(h)#摄像头离平面的高度，返回的应有的宽和高 dst = np.array(((0, 0), (maxw, 0), (maxw, maxh), (0, maxh)), dtype="float32") M = cv2.getPerspectiveTransform(rect, dst) warped = cv2.warpPerspective(image, M, (int(maxw), int(maxh))) print int(maxw),int(maxh) return warped,int(maxw),int(maxh) image=dst warped, w, h = perspectivetransh(dst, 0, 1242) dst1 = cv2.resize(dst, (int(dst.shape[1]*0.4), int(dst.shape[0]*0.4)), ) cv2.imshow("-Wdst1",dst1) warped2 = cv2.resize(warped, (int(warped.shape[1]*0.4), int(warped.shape[0]*0.4)), ) cv2.imshow("-Warpeasdd2",warped2) cv2.imwrite("warped2.png",warped2) cv2.waitKey(0)
#coding=utf-8 机械手标定 import cv2 import numpy as np mtx=np.array([[3503.2070085750343, 0.0, 1514.4329277183597], [0.0, 3504.0911621197565, 1070.0662685362333], [0.0, 0.0, 1.0]]) #内参矩阵行列互换 intr=map(list,zip(*mtx)) r=np.array(([ 0.04662873, -0.99886898,0.00930149], [ 0.9988022,0.04648333,-0.01527983], [ 0.01483019,0.01000283,0.99983999]), dtype = "float32").T t=np.array([[186.4710217500726, -286.7778264905354, 1242.0236930996398]]) RT = np.concatenate((r, t)) def xy2XY(u,v,z): cm=np.dot(RT,intr) c13umc11=cm[0][2]*u-cm[0][0] c13vmc12=cm[0][2]*v-cm[0][1] c23umc21=cm[1][2]*u-cm[1][0] c23vmc22=cm[1][2]*v-cm[1][1] c31zac41=cm[2][0]*z+cm[3][0] c32zac42=cm[2][1]*z+cm[3][1] c33zac43u=(cm[2][2]*z+cm[3][2])*u c33zac43v=(cm[2][2]*z+cm[3][2])*v y=( (c31zac41-c33zac43u)/(c13umc11) - (c32zac42-c33zac43v)/(c13vmc12) ) \ /((c23umc21)/(c13umc11)-(c23vmc22)/(c13vmc12)/(c13vmc12)) x=-1*c23umc21/c13umc11+(-c33zac43u+c31zac41)/c13umc11 return x,y def calRealPoint(xb1,yb1,xb2,yb2,xa1,ya1,xa2,ya2,image1,image2): xc,yc=0,0 name="ima12" image1=cv2.imread(image1) image2=cv2.imread(image2) image=image1/2+image2/2 # image = cv2.resize(image, (int(image.shape[1] * 0.4), int(image.shape[0] * 0.4))) i=0 x1m=(xa1+xb1)/2 y1m=(ya1+yb1)/2 x2m=(xa2+xb2)/2 y2m=(ya2+yb2)/2 k1=-1.0*(xa1-xb1)/(ya1-yb1) k2=-1.0*(xa2-xb2)/(ya2-yb2) xjd=((y2m-y1m)+(k1*x1m-k2*x2m))/(k1-k2) yjd=k1*xjd+y1m-k1*x1m xc=xc+xjd yc=yc+yjd cv2.circle(image, (x1m, y1m), 3, 255) cv2.circle(image, (x2m, y2m), 3, 255) cv2.circle(image, (int(xc), int(yc)), 3, 255) print xc,yc xcr,ycr=xy2XY(xc,yc,0) print "x=%f,y=%f"%(xcr,ycr) #在透视变换后的图像，机械手对应的中心坐标？ p = np.array((xcr, ycr, 0, 1)) pc = np.dot(p, np.dot(RT, intr)) pc = pc / pc[2] print pc[0], pc[1] x1m=int(x1m) y1m=int(y1m) x2m=int(x2m) y2m=int(y2m) xb1=int(xb1) xb2=int(xb2) yb1=int(yb1) yb2=int(yb2) xa1=int(xa1) xa2=int(xa2) ya1=int(ya1) ya2=int(ya2) xjd=int(xjd) yjd=int(yjd) cv2.line(image,(xjd,yjd),(x1m,y1m),255) cv2.line(image,(xjd,yjd),(x2m,y2m),255) cv2.line(image,(xb1,yb1),(xa1,ya1),255) cv2.line(image,(xb2,yb2),(xa2,ya2),255) cv2.circle(image, (xb2, yb2), 3, 255) cv2.circle(image, (xa1, ya1), 3, 255) cv2.circle(image, (xa2, ya2), 3, 255) cv2.circle(image, (xb1, yb1), 3, 255) cv2.circle(image,(xjd,yjd),3,255) cv2.imwrite("jd.png", image) #image=cv2.resize(image,(int(image.shape[1]*0.4),int(image.shape[0]*0.4))) image=cv2.resize(image,(int(image.shape[1]*0.4),int(image.shape[0]*0.4))) cv2.imshow(name,image) cv2.waitKey(0) print"-----------calRealPoint----------------------" return [xcr,ycr] def CalcMatrix(robot,camera): a=np.zeros((3,3)) matrix=np.zeros((2,3)) b1=np.array((robot[0][0],robot[1][0],robot[2][0])) b2=np.array((robot[0][1],robot[1][1],robot[2][1])) for i in range(0,3): a[i]=np.array((camera[i][0],camera[i][1],1)) matrix[0]=np.linalg.solve(a,b1) matrix[1]=np.linalg.solve(a,b2) print matrix return matrix def CalcXY(x,y,matrix): slove = np.dot((x, y, 1), np.transpose(matrix)) return slove[0], slove[1] x1,y1=calRealPoint(938,493,1936,464, 843,650,1799,362,"Image/rootImage-9.bmp","Image/rootImage-10.bmp") x2,y2=calRealPoint(922,502,1890,746, 781,623,1778,608,"Image/rootImage-11.bmp","Image/rootImage-12.bmp") x3,y3=calRealPoint(824,464,1822,466, 714,623,1678,371,"Image/rootImage-15.bmp","Image/rootImage-16.bmp") x4,y4=calRealPoint(1139,534,2110,770, 1006,651,2005,624,"Image/rootImage-17.bmp","Image/rootImage-18.bmp") robot=[[70,754],[130,690],[80,720]] camera=[[x1,y1],[x2,y2],[x3,y3]] print"-----------CalcMatrix----------------------" matrix=CalcMatrix(robot,camera) print"-----------CalcXY----------------------" print CalcXY(x1,y1,matrix)