vfh_recognition: vfh_recognition: views

00001 #! /usr/bin/env python
00002 
00003 import os
00004 import time
00005 import vtk
00006 import sys 
00007 import time
00008 import numpy
00009 import random
00010 from math import sqrt
00011 import pickle
00012 import threading
00013 import os
00014 import math
00015 import copy
00016 
00017 sys.path.append("..")
00018 from utils.tesselatedSphere import TesselatedSphere 
00019 
00020 def writeMatrix(matrix,fileD):
00021         for i in xrange(4):
00022                 for j in xrange(4):
00023                         fileD.write(str(matrix.GetElement(i,j)) + " ")
00024                 fileD.write("\n")
00025 
00026 def copyMatrix(matrix):
00027         copy = vtk.vtkMatrix4x4()
00028         for i in xrange(4):
00029                 for j in xrange(4):
00030                         copy.SetElement(i,j,matrix.GetElement(i,j))
00031 
00032         return copy
00033 
00034 def vtkToNumpy(vtk_matrix):
00035         np_matrix = numpy.zeros((4,4))
00036         for i in xrange(4):
00037                 for j in xrange(4):
00038                         np_matrix[i,j] = float(vtk_matrix.GetElement(i,j))
00039         
00040         return np_matrix
00041 
00042 def sample_views_tesselated_sphere(source_file):
00043         sphereLevel = 0
00044         sphere = TesselatedSphere(sphereLevel, 0)
00045         sphere.construct()
00046         reader1 = vtk.vtkPLYReader()
00047         reader1.SetFileName(source_file)
00048 
00049         ren = vtk.vtkRenderer()
00050         renWin = vtk.vtkRenderWindow()
00051         renWin.AddRenderer(ren)
00052         iren = vtk.vtkRenderWindowInteractor()
00053         ren.SetBackground(1.0, 0.0, 0.0)
00054         xres = 150
00055         yres = 150
00056         renWin.SetSize(xres, yres)
00057         renWin.SetPosition(1000,200)
00058         worldPicker = vtk.vtkWorldPointPicker ()
00059         resolution =1
00060         viewAngle = 45
00061         ren.ResetCamera()
00062         cam = ren.GetActiveCamera()
00063         cam.SetFocalPoint(0,0,0)
00064         cam.SetClippingRange(0,10)
00065         cam.SetViewAngle(viewAngle)
00066 
00067         stlMapper = vtk.vtkPolyDataMapper()
00068         stlMapper.SetInputConnection(reader1.GetOutputPort())
00069         stlMapper.Update()
00070 
00071         CoM = sphere.computeCenterOfMass(stlMapper.GetInput())
00072         transCenter = vtk.vtkTransform()
00073         transCenter.Translate(-CoM[0],-CoM[1],-CoM[2])
00074         matrixCenter = transCenter.GetMatrix()
00075         transFilterCenter = vtk.vtkTransformFilter()
00076         transFilterCenter.SetTransform(transCenter)
00077         transFilterCenter.SetInputConnection(reader1.GetOutputPort())
00078 
00079         stlMapper = vtk.vtkPolyDataMapper()
00080         stlMapper.SetInputConnection(transFilterCenter.GetOutputPort())
00081         stlMapper.Update()
00082 
00083         bb = stlMapper.GetBounds()
00084         ms = max( (abs(bb[0]-bb[1]),abs(bb[2]-bb[3]),abs(bb[4]-bb[5]) ) )
00085         max_side=0.5
00086         scale_factor = max_side/ms
00087         vscale = vtk.vtkTransform()
00088 
00089         vscale.Scale(scale_factor,scale_factor,scale_factor)
00090         matrixScale = vscale.GetMatrix()
00091 
00092         transFilterScale = vtk.vtkTransformFilter()
00093         transFilterScale.SetTransform(vscale)
00094         transFilterScale.SetInputConnection(transFilterCenter.GetOutputPort())
00095 
00096         stlMapper = vtk.vtkPolyDataMapper()
00097         stlMapper.SetInputConnection(transFilterScale.GetOutputPort())
00098         stlMapper.Update()
00099 
00100         print len(sphere.polygons)
00101         radius=1
00102         firstPos = sphere.polygons[sphereLevel][0].getCenter()*radius
00103         gen_views = []
00104         views_transforms = []
00105         view = 0
00106 
00107         #loop over the tesselated sphere to get camera positions
00108         for polygon in sphere.polygons[sphereLevel]:
00109                 #print polygon.getCenter()
00110                 cam.SetPosition(polygon.getCenter()*radius)
00111 
00112                 camC = vtk.vtkCamera()
00113                 camC.SetFocalPoint(0,0,0)
00114                 camC.SetViewUp(0,1,0)
00115                 camC.SetPosition(firstPos)
00116                 camC.SetViewAngle(viewAngle)
00117                 camC.Modified()
00118 
00119                 transROT = vtk.vtkTransform()
00120                 transROT.Identity()
00121                 #transROT.RotateZ(view*30)
00122                 transROT.Concatenate(camC.GetViewTransformObject().GetInverse().GetMatrix())
00123                 transROT.Concatenate(cam.GetViewTransformObject().GetMatrix())
00124                 print "determinant:", numpy.linalg.det(vtkToNumpy(transROT.GetMatrix()))
00125 
00126                 transFilter11 = vtk.vtkTransformFilter()
00127                 transFilter11.SetTransform(transROT)
00128                 #transFilter11.SetInputConnection(transFilterCenter.GetOutputPort())
00129                 transFilter11.SetInputConnection(transFilterScale.GetOutputPort())
00130                 matrixRotModel = transROT.GetMatrix()
00131 
00132                 transMapper = vtk.vtkPolyDataMapper()
00133                 transMapper.SetInputConnection(transFilter11.GetOutputPort())
00134                 transMapper.Update()
00135 
00136                 #translate points so camera is at (0,0,0)
00137                 translation = vtk.vtkTransform()
00138                 translation.Translate(firstPos*-1)
00139                 matrixTranslation = translation.GetMatrix()
00140 
00141                 translationFilter1 = vtk.vtkTransformFilter()
00142                 translationFilter1.SetTransform(translation)
00143                 translationFilter1.SetInputConnection(transFilter11.GetOutputPort())
00144 
00145                 transMapper = vtk.vtkPolyDataMapper()
00146                 transMapper.SetInputConnection(translationFilter1.GetOutputPort())
00147                 transMapper.Update()
00148 
00149                 camC.SetPosition((0,0,0))
00150                 camC.SetFocalPoint(firstPos*-1)
00151                 #vis.visModelCam(stlMapper, cam)
00152                 #vis.visModelCam(transMapper, camC, (1,0,0))
00153 
00154                 transMapperTranslated = vtk.vtkPolyDataMapper()
00155                 transMapperTranslated.SetInputConnection(translationFilter1.GetOutputPort())
00156                 transMapperTranslated.Update()
00157 
00158                 #camC is now located at (0,0,0) and gives the same view as cam
00159                 viewT = camC.GetViewTransformMatrix()
00160                 pts = vtk.vtkPoints()
00161                 ren.SetActiveCamera(camC)
00162                 ren.ResetCameraClippingRange()
00163                 stlActorTrans = vtk.vtkActor()
00164                 stlActorTrans.SetMapper(transMapperTranslated)
00165                 ren.AddActor(stlActorTrans)
00166                 ren.SetBackground(1.0, 0.0, 0.0)
00167 
00168                 renWin.Render()
00169 
00170                 w2i = vtk.vtkWindowToImageFilter()
00171                 w2i.SetInput(renWin)
00172                 imD  = w2i.GetOutput()  
00173                 imD.Update()
00174                 imD.SetNumberOfScalarComponents(3)
00175 
00176                 writer = vtk.vtkPNGWriter()
00177                 writer.SetInput(imD)
00178                 writer.SetFileName("view_" + str(view) + ".png")
00179                 writer.Write()
00180 
00181                 #ren_cam = vtk.vtkRenderer()
00182                 #ren_cam.SetActiveCamera(cam)
00183                 #ren_cam.ResetCameraClippingRange()
00184                 #actor_static = vtk.vtkActor()
00185                 #actor_static.SetMapper(stlMapper)
00186                 #ren_cam.AddActor(actor_static)
00187                 #ren_cam.SetBackground(1.0, 0.0, 0.0)
00188 
00189                 #renWinCam = vtk.vtkRenderWindow()
00190                 #renWinCam.AddRenderer(ren_cam)
00191                 #renWinCam.Render()
00192 
00193                 #w2i_cam = vtk.vtkWindowToImageFilter()
00194                 #w2i_cam.SetInput(renWinCam)
00195                 #imD_cam  = w2i.GetOutput()     
00196                 #imD_cam.Update()
00197                 #imD_cam.SetNumberOfScalarComponents(3)
00198 
00199                 
00200                 #writer = vtk.vtkPNGWriter()
00201                 #writer.SetInput(imD_cam)
00202                 #writer.SetFileName("view_cam_" + str(view) + ".png")
00203                 #writer.Write()
00204                 
00205                 worldPicker.Pick(5,5,0,ren)
00206                 p1 = numpy.array(worldPicker.GetPickPosition())
00207                 worldPicker.Pick(xres-5,5,0,ren)
00208                 p2 = numpy.array(worldPicker.GetPickPosition())
00209                 worldPicker.Pick(5,yres-5,0,ren)
00210                 p3 = numpy.array(worldPicker.GetPickPosition())
00211 
00212                 pn = numpy.cross(p2-p1, p3-p1 )
00213                 pn = pn / sqrt(numpy.dot(pn,pn.conj()))
00214                 d = numpy.dot(pn,p1.conj())
00215 
00216                 narr = []
00217 
00218                 matrixTranslationCopy = copyMatrix(matrixTranslation)
00219                 backToRealScale = vtk.vtkTransform()
00220                 backToRealScale.PostMultiply()
00221                 backToRealScale.Identity()
00222                 backToRealScale.Concatenate(matrixScale) #scale to real scale                   
00223                 backToRealScale.Concatenate(matrixTranslationCopy) #translate back to (0,0,0)           
00224                 backToRealScale.Inverse()
00225                 backToRealScale.Modified()
00226                 backToRealScale.Concatenate(matrixTranslation)
00227                 backToRealScale.Modified()
00228                 
00229                 for x in range(0,xres,resolution):
00230                         for y in range(0,yres,resolution):
00231                                 if imD.GetScalarComponentAsFloat(x,y,0,0) == 255 and imD.GetScalarComponentAsFloat(x,y,0,1)  == 0 and imD.GetScalarComponentAsFloat(x,y,0,2)  == 0 :
00232                                         continue
00233                                 else:
00234                                         worldPicker.Pick(x,y,0,ren)
00235                                         nx,ny,nz = worldPicker.GetPickPosition()
00236                                         #THIS is causing the missing lines, if you comment out, ... yea, see the odd behaviour of VTK ;)
00237                                         if abs(numpy.dot([nx,ny,nz],pn.conj()) - d ) < 0.001:
00238                                                 continue
00239 
00240                                         #we want the coordinates in camera coordinates! z forward, y down and x to the right
00241                                         #points are in world coordinates
00242                                         pWorld = [nx,ny,nz,1]
00243 
00244                                         pWorldTrans = backToRealScale.GetMatrix().MultiplyPoint(pWorld)
00245                                         pCamera = numpy.array(viewT.MultiplyPoint(pWorldTrans))
00246                                         narr.append([pCamera[0],-pCamera[1],-pCamera[2]]) #rotation about the x axes (180 degrees) to match view up + flip x coordinate due to VIEW VTK coordinate system...
00247 
00248                 kinect = vtk.vtkCamera()
00249                 kinect.SetFocalPoint(0,0,5)
00250                 kinect.SetViewUp(0,-1,0)
00251                 kinect.SetPosition((0,0,0))
00252                 kinect.Modified()
00253 
00254                 stdCam = vtk.vtkCamera()
00255                 ren.RemoveActor(stlActorTrans)
00256 
00257                 #create the whole transformation, from object coordinates to camera coordinates
00258                 transOCtoCC = vtk.vtkTransform()
00259                 transOCtoCC.PostMultiply()
00260                 transOCtoCC.Identity()
00261                 #print matrixCenter
00262                 transOCtoCC.Concatenate(matrixCenter)
00263                 transOCtoCC.Concatenate(matrixRotModel)
00264                 transOCtoCC.Concatenate(matrixTranslation)
00265                 transOCtoCC.Concatenate(viewT)
00266         
00267                 #kinect transform
00268                 kinectTMatrix = vtk.vtkMatrix4x4()
00269                 kinectTMatrix.Identity()
00270                 kinectTMatrix.SetElement(0,0,1)
00271                 kinectTMatrix.SetElement(1,1,-1)
00272                 kinectTMatrix.SetElement(2,2,-1)
00273 
00274                 transOCtoCC.Concatenate(kinectTMatrix)
00275                 transOCtoCC.Modified()
00276 
00277                 print "determinant:", numpy.linalg.det(vtkToNumpy(transOCtoCC.GetMatrix()))
00278                 reader1 = vtk.vtkPLYReader()
00279                 reader1.SetFileName(source_file)
00280 
00281                 finalFilter = vtk.vtkTransformFilter()
00282                 #transOCtoCC.Inverse()
00283                 finalFilter.SetTransform(transOCtoCC)
00284                 finalFilter.SetInputConnection(reader1.GetOutputPort())
00285 
00286                 mapper1 = vtk.vtkPolyDataMapper()
00287                 mapper1.SetInputConnection(finalFilter.GetOutputPort())
00288                 mapper1.Update()
00289                 #vis.visModel(mapper1)
00290 
00291                 #vis.visModelCam(mapper1, kinect, (1,0,1), narr, True)
00292 
00293                 #check if view already exists by comparing the rendered image!
00294 
00295                 gen_views.append(narr) #views in camera coordinates
00296                 #print transOCtoCC.GetMatrix()
00297                 views_transforms.append(transOCtoCC.GetMatrix()) #transformation
00298 
00299                 view += 1
00300         #print views_transforms[0], views_transforms[1]
00301 
00302         #quit()
00303         return [gen_views,views_transforms]
views_generator.py
