PoseFromCorrespondences.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright (C) 2009 Rosen Diankov
00004 # 
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #     http://www.apache.org/licenses/LICENSE-2.0
00009 # 
00010 # Unless required by applicable law or agreed to in writing, software
00011 # distributed under the License is distributed on an "AS IS" BASIS,
00012 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013 # See the License for the specific language governing permissions and
00014 # limitations under the License. 
00015 from __future__ import with_statement # for python 2.5
00016 PKG = 'posedetectiondb' # this package name
00017 NAME = 'PoseFromCorrespondences'
00018 
00019 import roslib; roslib.load_manifest(PKG) 
00020 import os, sys, time, threading, string, struct
00021 from optparse import OptionParser
00022 from numpy import *
00023 
00024 import cv, gtk
00025 import rospy, tf
00026 from roslib import rostime
00027 import std_msgs.msg
00028 import posedetection_msgs.msg
00029 import sensor_msgs.msg
00030 import geometry_msgs.msg
00031 from cv_bridge import CvBridge, CvBridgeError
00032 
00033 from IterativeClosestPoint import *
00034 import ParseMessages
00035 
00036 from openravepy import *
00037 
00038 from Tkinter import *
00039 import tkFileDialog
00040 import copy
00041 
00042 def FindExtrinsicCameraParams(imagepoints, objectpoints, KK):
00043     """ Use OpenCV to solve for the affine transformation that matches imagepoints to object points
00044     imagepoints - 2xN array
00045     objectpoints - 3xN array
00046     KK - 3x3 array or 4 element array
00047     """
00048     imagepoints = array(imagepoints,float)
00049     objectpoints = array(objectpoints,float)
00050     if len(KK.shape) == 1:
00051         cvKK = cv.CreateMat(3,3,cv.CV_32FC1)
00052         cvKK[0,0] = KK[0]; cvKK[0,1] = 0; cvKK[0,2] = KK[2];
00053         cvKK[1,0] = 0; cvKK[1,1] = KK[1]; cvKK[1,2] = KK[3];
00054         cvKK[2,0] = 0; cvKK[2,1] = 0; cvKK[2,2] = 1;
00055     else:
00056         cvKK = cv.fromarray(KK)
00057     cvDist = cv.CreateMat(4,1,cv.CV_32FC1)
00058     cvDist[0,0] = 0; cvDist[1,0] = 0; cvDist[2,0] = 0; cvDist[3,0] = 0;
00059     rvec = cv.CreateMat(3,1,cv.CV_32FC1)
00060     tvec = cv.CreateMat(3,1,cv.CV_32FC1)    
00061     object_points = cv.CreateMatHeader(3,objectpoints.shape[0],cv.CV_32FC1)
00062     cv.SetData(object_points,struct.pack('f'*(objectpoints.shape[0]*3),*transpose(objectpoints).flat),4*objectpoints.shape[0])
00063     image_points = cv.CreateMatHeader(2,imagepoints.shape[0],cv.CV_32FC1)
00064     cv.SetData(image_points,struct.pack('f'*(imagepoints.shape[0]*2),*transpose(imagepoints).flat),4*imagepoints.shape[0])
00065     cv.FindExtrinsicCameraParams2(object_points,image_points,cvKK,cvDist,rvec,tvec)
00066     T = matrixFromAxisAngle((rvec[0,0],rvec[1,0],rvec[2,0]))
00067     T[0:3,3] = [tvec[0,0],tvec[1,0],tvec[2,0]]
00068     return T
00069 
00070 class PoseFromCorrespondences(metaclass.AutoReloader):
00071     """
00072     Extracts poses from a set of point correspondences
00073     """
00074     def __init__(self,kinbodyfilename,verboselevel=1,frame_id=None):
00075         self.orenv = Environment()
00076         self.orenv.Load(kinbodyfilename)
00077         self.orbody = self.orenv.GetBodies()[0]
00078         self.orbody.SetTransform(eye(4))
00079         self.trimesh = self.orenv.Triangulate(self.orbody)
00080         self.orenv.SetViewer('qtcoin')
00081         self.eventhandle = self.orenv.GetViewer().RegisterCallback(Viewer.Events.ItemSelection,self.ormousecb)
00082         self.orpoint = None
00083         self.objectpoints = []
00084         self.verboselevel=verboselevel
00085         self.extractionlck = threading.Lock()
00086 
00087         self.cvpoint = None
00088         self.Tobjectrel = None
00089         self.imagepoints = []
00090 
00091         self.cvwindow = 'ImageDisplay'
00092         cv.NamedWindow(self.cvwindow, cv.CV_WINDOW_AUTOSIZE)
00093         cv.SetMouseCallback(self.cvwindow,self.cvmousecb)
00094          # register keycb with opencv window
00095         self.bridge = CvBridge()
00096 
00097         self.doquit = False
00098         self.gui = threading.Thread(target=self.rungui)
00099         self.gui.start()
00100 
00101         self.pattern = (eye(4),None)
00102         self.imagemsg = None
00103         self.KK = None
00104         self.image_sub = rospy.Subscriber("image",sensor_msgs.msg.Image,self.imagecb)
00105         self.camerainfo_sub = rospy.Subscriber("camera_info",sensor_msgs.msg.CameraInfo,self.camerainfocb)
00106         self.frame_id = frame_id
00107         if self.frame_id is None:
00108             self.object_sub = rospy.Subscriber("ObjectDetection",posedetection_msgs.msg.ObjectDetection,self.objectcb)
00109         else:
00110             self.tflistener=tf.TransformListener()
00111         self.pub_relpose = rospy.Publisher("RelativePose", geometry_msgs.msg.Pose)
00112         rospy.init_node(NAME, anonymous=True)#,disable_signals=False)
00113 
00114     def __del__(self):
00115         try:
00116             self.image_sub.unregister()
00117         except:
00118             pass
00119         try:
00120             self.camerainfo_sub.unregister()
00121         except:
00122             pass
00123         try:
00124             self.object_sub.unregister()
00125         except:
00126             pass
00127         try:
00128             self.pub_relpose.unregister()
00129         except:
00130             pass
00131         self.orenv.Destroy()
00132 
00133     def ormousecb(self,link,pos,org):
00134         if link.GetParent().GetNetworkId() == self.orbody.GetNetworkId():
00135             T = linalg.inv(link.GetParent().GetTransform())
00136             bodypos = dot(T[0:3,0:3],pos)+T[0:3,3]
00137             self.orpoint = bodypos
00138             self.ghandle = self.orenv.plot3(points=reshape(bodypos,(1,3)),pointsize=15.0,colors=array((1,0,0)))
00139         return False
00140 
00141     def cvmousecb(self,event,x,y,flags,param):
00142         if event == cv.CV_EVENT_LBUTTONUP:
00143             self.cvpoint = (x,y)
00144         if event == cv.CV_EVENT_RBUTTONUP:
00145             self.AddCorrespondence()
00146         if event == cv.CV_EVENT_MBUTTONUP:
00147             self.Reset()
00148 
00149     def rungui(self):
00150         self.main = Tk()
00151         self.main.title('Create Object Database - 3 channel image')      # window title
00152         self.main.resizable(width=False, height=False)
00153         buttonframe = Frame(self.main)
00154         buttonframe.pack()
00155         b1 = Button(buttonframe, text="Add Correspondence (R-Button)", command=self.AddCorrespondence)
00156         b1.grid(row=1,column=1)
00157         b2 = Button(buttonframe, text="Reset (M-Button)", command=self.Reset)
00158         b2.grid(row=1,column=2)
00159         b2 = Button(buttonframe, text="Quit", command=self.Quit)
00160         b2.grid(row=1,column=3)
00161        # b4 = Button(buttonframe, text="Keyboard", command=self.keycb)
00162        # b4.grid(row=1,column=4)
00163         entryframe = Frame(self.main)
00164         entryframe.pack()
00165         ltrans = Label(entryframe, text='Transform:')
00166         ltrans.grid(row=1,column=1,sticky=W+E)
00167         self.T_entry = Entry(entryframe,bg='white',width=100)
00168         self.T_entry.grid(row=1,column=2)
00169        # self.T_entry.grid(row=1,column=4)
00170         self.main.mainloop()
00171 
00172     def camerainfocb(self,infomsg):
00173         with self.extractionlck:
00174             self.KK = reshape(infomsg.K,(3,3))
00175             if any([f!=0 for f in infomsg.D]):
00176                 print 'Program does not support distorted images'
00177 
00178     def imagecb(self,imagemsg):
00179         with self.extractionlck:
00180             self.imagemsg = imagemsg
00181 
00182     def objectcb(self,objectmsg):
00183         with self.extractionlck:
00184             if len(objectmsg.objects) > 0:
00185                 quat = objectmsg.objects[0].pose.orientation
00186                 trans = objectmsg.objects[0].pose.position
00187                 self.pattern = (matrixFromPose([quat.w,quat.x,quat.y,quat.z,trans.x,trans.y,trans.z]),objectmsg.header.frame_id)
00188             else:
00189                 self.pattern = (None,None)
00190 
00191     def AddCorrespondence(self):
00192         with self.extractionlck:
00193             print 'add correspondence'
00194 #             if self.frame_id:
00195 #                 base_frame_id = self.orbody.GetLinks()[0].GetName()
00196 #                 (trans,rot) = self.lookupTransform(base_frame_id, self.frame_id, rospy.Time(0))
00197 #                 pose = r_[rot[3],rot[0],rot[1],rot[2],trans]
00198 #                 self.pattern = (matrixFromPose(pose),base_frame_id)
00199             if self.cvpoint is not None and self.orpoint is not None and self.pattern[0] is not None:
00200                 Tpattern_inv = linalg.inv(self.pattern[0])
00201                 self.imagepoints.append(array(self.cvpoint))
00202                 self.objectpoints.append(array(self.orpoint))
00203                 print 'total gathered points: %d'%len(self.imagepoints)
00204                 if len(self.imagepoints) >= 4:
00205                     print array(self.imagepoints)
00206                     print array(self.objectpoints)
00207                     print self.pattern[0]
00208                     Tcameraobject = FindExtrinsicCameraParams(array(self.imagepoints,float),array(self.objectpoints,float),self.KK)
00209                     self.Tobjectrel = dot(Tpattern_inv,Tcameraobject)
00210                     print 'camera transform: ', Tcameraobject                        
00211                     values = reshape(self.Tobjectrel[0:3,0:4],(12,))
00212                     print "relative transform is: ",self.Tobjectrel
00213                     self.T_entry.insert(0, ' '.join(str(f) for f in values))
00214             else:
00215                 print 'point data not initialized'
00216 
00217     def Reset(self):
00218         print 'reset'
00219         self.imagepoints = []
00220         self.objectpoints = []
00221         self.Tobjectrel = None
00222     def Quit(self):
00223         print 'quitting from gui'
00224         self.doquit = True
00225         self.main.quit()
00226 
00227     def drawpart(self,cv_image,T,KK):
00228         N = self.trimesh.vertices.shape[0]
00229         pts = dot(transformPoints(T,self.trimesh.vertices),transpose(KK))
00230         imagepts = pts[0:N,0:2]/reshape(repeat(pts[0:N,2],2),[N,2])
00231         cvimagepts = [tuple(p) for p in array(imagepts,int)]
00232         for tri in self.trimesh.indices:
00233             cv.Line(cv_image,cvimagepts[tri[0]],cvimagepts[tri[1]],(255,255,255))
00234             cv.Line(cv_image,cvimagepts[tri[1]],cvimagepts[tri[2]],(255,255,255))
00235             cv.Line(cv_image,cvimagepts[tri[2]],cvimagepts[tri[0]],(255,255,255))
00236 
00237     def drawcoordsys(self,cv_image,T,KK):
00238         points3d = array(((0,0,0),(0.05,0,0),(0,0.05,0),(0,0,0.05)))
00239         projpts = dot(transformPoints(T,points3d),transpose(KK))
00240         x = array(projpts[:,0]/projpts[:,2],int)
00241         y = array(projpts[:,1]/projpts[:,2],int)
00242         cv.Line(cv_image,(x[0],y[0]),(x[1],y[1]),(0,0,255),1)
00243         cv.Line(cv_image,(x[0],y[0]),(x[2],y[2]),(0,255,0),1)
00244         cv.Line(cv_image,(x[0],y[0]),(x[3],y[3]),(255,0,0),1)
00245 
00246     def keycb(self, char):
00247         a=1
00248         if (char != -1):
00249             with self.extractionlck:
00250                 minangle=pi/200
00251                 mintranslation=.001
00252                 if char==1113955: #NumLock Insert
00253                     R=rotationMatrixFromAxisAngle(array([1,0,0]),minangle)
00254                     T=array([0,0,0])
00255                 elif char==1114111: #Num Lock Delete
00256                     R=rotationMatrixFromAxisAngle(array([1,0,0]),-minangle)
00257                     T=array([0,0,0])
00258                 elif char==1113936: #NumLock Home
00259                     R=rotationMatrixFromAxisAngle(array([0,1,0]),minangle)
00260                     T=array([0,0,0])
00261                 elif char==1113943: #Num Lock End
00262                     R=rotationMatrixFromAxisAngle(array([0,1,0]),-minangle)
00263                     T=array([0,0,0])
00264                 elif char==1113941: #Num Lock Page Up
00265                     R=rotationMatrixFromAxisAngle(array([0,0,1]),minangle)
00266                     T=array([0,0,0])
00267                 elif char==1113942: #Num Lock Page Down
00268                     R=rotationMatrixFromAxisAngle(array([0,0,1]),-minangle)
00269                     T=array([0,0,0])
00270                 elif char==65379: #Insert
00271                     R=eye(3)
00272                     T=array([1,0,0])
00273                 elif char==65535: #Delete
00274                     R=eye(3)
00275                     T=array([-1,0,0])
00276                 elif char==65360: #Home
00277                     R=eye(3)
00278                     T=array([0,1,0])
00279                 elif char==65367: #End
00280                     R=eye(3)
00281                     T=array([0,-1,0])
00282                 elif char==65365: #Page Up
00283                     R=eye(3)
00284                     T=array([0,0,1])
00285                 elif char==65366: #Page Down
00286                     R=eye(3)
00287                     T=array([0,0,-1])
00288                 else:
00289                     a=0
00290                 if a==1:
00291                     self.Tobjectrel[:3,:3]=dot(R,self.Tobjectrel[:3,:3])
00292                     self.Tobjectrel[:3,3]=self.Tobjectrel[:3,3]+mintranslation*T
00293                     print "relative: ",self.Tobjectrel
00294                     
00295     def spin(self):
00296         while not rospy.is_shutdown() and not self.doquit:
00297             with self.extractionlck:
00298                 imagemsg = copy.copy(self.imagemsg)
00299                 KK = array(self.KK)
00300                 Tobjectrel = array(self.Tobjectrel) if self.Tobjectrel is not None else None
00301                 Tpattern = array(self.pattern[0]) if self.pattern[0] is not None else None
00302 
00303             if KK is None or imagemsg is None:
00304                 time.sleep(0.1)
00305                 continue
00306 
00307             try:
00308                 cv_image = self.bridge.imgmsg_to_cv(imagemsg, "bgr8")
00309             except CvBridgeError, e:
00310                 print e
00311 
00312             if Tpattern is not None:
00313                 if Tobjectrel is not None:
00314                     self.drawpart(cv_image,dot(Tpattern,Tobjectrel),KK)
00315                 self.drawcoordsys(cv_image,Tpattern,KK)
00316             if self.cvpoint is not None:
00317                 cv.Circle(cv_image,self.cvpoint,2,(0,0,255),2)
00318 
00319             if Tobjectrel is not None:
00320                 posemsg = geometry_msgs.msg.Pose()
00321                 posemsg.orientation.w, posemsg.orientation.x, posemsg.orientation.y, posemsg.orientation.z, posemsg.position.x, posemsg.position.y, posemsg.position.z = poseFromMatrix(Tobjectrel)
00322                 self.pub_relpose.publish(posemsg)
00323             gtk.gdk.threads_enter()
00324             cv.ShowImage(self.cvwindow, cv_image)
00325             char=cv.WaitKey(20)
00326             self.keycb(char)
00327             gtk.gdk.threads_leave()
00328             time.sleep(1.0)
00329         print 'quitting spin'
00330             
00331 if __name__== '__main__':
00332     parser = OptionParser(description='Estiamtes the pose of an openrave object by specifying manual correspondeces between the image and the openrave environment. If a separate ObjectDetection pattern is added, will publish the pose of the object with respect to the pattern.')
00333     parser.add_option('--quiet',action='store_true', dest='quiet',default=False,
00334                       help="If set will not print extraction output and times.")
00335     parser.add_option('--kinbody', action='store', type='string', dest='kinbody',
00336                       help='OpenRAVE Kinbody file to load')
00337 #     parser.add_option('--frame_id', action='store', type='string', dest='frame_id',default=None,
00338 #                       help='tf frame id for possible frame that camera is connected to')
00339     parser.add_option('--Tobjectrel', action='store', type='string', dest='Tobjectrel',default=None,
00340                       help='Initial estimate of Tobject (3x4 matrix serialized by row-order)')
00341     (options, args) = parser.parse_args()
00342     if not options.kinbody:
00343         print('Error: Need to specify a template')
00344         sys.exit(1)
00345 
00346     gtk.gdk.threads_init()
00347 
00348     try:
00349         processor = PoseFromCorrespondences(options.kinbody)
00350         if options.Tobjectrel is not None:
00351             processor.Tobjectrel = r_[reshape([float(s) for s in options.Tobjectrel.split()[0:12]],[3,4]),[[0,0,0,1]]]
00352         processor.spin()
00353     except KeyboardInterrupt, e:
00354         pass


posedetectiondb
Author(s): Rosen Diankov (rdiankov@cs.cmu.edu)
autogenerated on Mon Oct 6 2014 01:17:28