pt1i = np.int32(pt1)
pt2i = np.int32(pt2)
dT = np.array([piInv(imgT, depT, x, y) for (x, y) in pt1i])
ptT, clT = dT[:, 0, :], dT[:, 1, :]
points(pos=ptT, size=3, color=(0, 1, 0))
print ptT.shape
dM = np.array([piInv(imgM, depM, x, y) for (x, y) in pt2i])
ptM, clM = dM[:, 0, :], dM[:, 1, :]
#points(pos=ptM,size=3,color=(0,0,1))
print ptM.shape
print "ptM", icp.distData(ptM, ptT)
Tcv, piM = icp.fitICP(ptM, ptT)
print Tcv
piMa = icp.transformDataSetUsingTransform(ptsM, Tcv)
points(pos=piMa, size=1, color=clrM) #center and display points
points(pos=piM, size=5, color=(1, 0, 0))
print "piM", icp.distData(piM, ptT)
#cv2.imshow("Fm1",img1)
# Tim,imM=icp.fitICP(ptsM,ptsT)
# points(pos=imM,size=1,color=clrM)#center and display points
#
# print Tim
# print imM.shape,ptsT.shape
# ms=min(imM.shape[0],ptsT.shape[0])
pt1i=np.int32(pt1)
pt2i=np.int32(pt2)
dT=np.array([piInv(imgT,depT,x,y) for (x,y) in pt1i])
ptT,clT=dT[:,0,:],dT[:,1,:]
points(pos=ptT,size=3,color=(0,1,0))
print ptT.shape
dM=np.array([piInv(imgM,depM,x,y) for (x,y) in pt2i])
ptM,clM=dM[:,0,:],dM[:,1,:]
#points(pos=ptM,size=3,color=(0,0,1))
print ptM.shape
print "ptM",icp.distData(ptM,ptT)
Tcv,piM=icp.fitICP(ptM, ptT)
print Tcv
piMa=icp.transformDataSetUsingTransform(ptsM, Tcv)
points(pos=piMa,size=1,color=clrM)#center and display points
points(pos=piM,size=5,color=(1,0,0))
print "piM",icp.distData(piM,ptT)
#cv2.imshow("Fm1",img1)
# Tim,imM=icp.fitICP(ptsM,ptsT)
# points(pos=imM,size=1,color=clrM)#center and display points
#
# print Tim
# print imM.shape,ptsT.shape
# ms=min(imM.shape[0],ptsT.shape[0])
# m3c=getPickleModel('lidar3c.dat')
# points(pos=m3c,color=(0,1,1))
#dataset to test ICP in 3D
model3D = np.array([(0, 1, 0), (1, 1, 0), (0, 0, -1), (0, 1, -1)]) * 500
points(pos=model3D, color=(0, 0, 1))
Ti = np.array([[0.5, 0.1, 0.8], [0.1, 0.5, 0.3], [0.8, 0.3, 0.5]])
target3D = np.array([(0.1, 1, 0), (1.1, 1.1, 0), (0.1, 0.1, -1),
(0.1, 1, -1)]) * 500
# target3D=np.array([[-103.47318056, -4.73010521, 489.24986969],
# [ -81.20086263 , 494.66670749, 499.58269659],
# [-487.54454916 , 24.66799255, -103.72983964],
# [-591.94480715 , 19.20743331 ,385.21879614]])
#target3D=Ti.dot(model3D.transpose()).transpose()+np.array([(1,1,1),(1,1,1),(1,1,1),(1,1,1)])*500
#dataset to fit the model in this target usually is a 2D map or a 3D geometric point cloud
target = model0
vtarget = points(pos=target, color=(0, 1, 0))
if __name__ == '__main__':
Tf = np.eye(3, 3)
Tf, pit1 = icp.fitICP(model1, target)
#target=np.vstack((target,pit1))
points(pos=pit1, color=(1, 0, 0))
print Tf
Tf, pit2 = icp.fitICP(model2, target)
points(pos=pit2, color=(1, 0, 1))
#target=np.vstack((target,pit2))
print Tf
while True:
rate(10)
# points(pos=m3c,color=(0,1,1))
#dataset to test ICP in 3D
model3D =np.array([(0,1,0),(1,1,0),(0,0,-1),(0,1,-1)])*500
points(pos=model3D ,color=(0,0,1))
Ti=np.array([[0.5,0.1,0.8],
[0.1,0.5,0.3],
[0.8,0.3,0.5]])
target3D=np.array([(0.1,1,0),(1.1,1.1,0),(0.1,0.1,-1),(0.1,1,-1)])*500
# target3D=np.array([[-103.47318056, -4.73010521, 489.24986969],
# [ -81.20086263 , 494.66670749, 499.58269659],
# [-487.54454916 , 24.66799255, -103.72983964],
# [-591.94480715 , 19.20743331 ,385.21879614]])
#target3D=Ti.dot(model3D.transpose()).transpose()+np.array([(1,1,1),(1,1,1),(1,1,1),(1,1,1)])*500
#dataset to fit the model in this target usually is a 2D map or a 3D geometric point cloud
target=model0
vtarget=points(pos=target,color=(0,1,0))
if __name__ == '__main__':
Tf=np.eye(3,3)
Tf,pit1=icp.fitICP(model1,target)
#target=np.vstack((target,pit1))
points(pos=pit1,color=(1,0,0))
print Tf
Tf,pit2=icp.fitICP(model2,target)
points(pos=pit2,color=(1,0,1))
#target=np.vstack((target,pit2))
print Tf
while True:
rate(10)