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])
Example #3
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)
Example #4
0
# 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)