00001
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 from utils.visualize import Visualizer
00020
00021 import ground_plane.utils as utils
00022
00023 def writeMatrix(matrix,fileD):
00024 for i in xrange(4):
00025 for j in xrange(4):
00026 fileD.write(str(matrix.GetElement(i,j)) + " ")
00027 fileD.write("\n")
00028
00029 def copyMatrix(matrix):
00030 copy = vtk.vtkMatrix4x4()
00031 for i in xrange(4):
00032 for j in xrange(4):
00033 copy.SetElement(i,j,matrix.GetElement(i,j))
00034
00035 return copy
00036
00037 def vtkToNumpy(vtk_matrix):
00038 np_matrix = numpy.zeros((4,4))
00039 for i in xrange(4):
00040 for j in xrange(4):
00041 np_matrix[i,j] = float(vtk_matrix.GetElement(i,j))
00042
00043 return np_matrix
00044
00045 def sample_views_tesselated_sphere(source_file, outfile):
00046
00047
00048
00049 sphereLevel = 0
00050 sphere = TesselatedSphere(sphereLevel, 0)
00051 sphere.construct()
00052 vis = Visualizer()
00053 reader1 = vtk.vtkPLYReader()
00054 reader1.SetFileName(source_file)
00055
00056 ren = vtk.vtkRenderer()
00057 renWin = vtk.vtkRenderWindow()
00058 renWin.AddRenderer(ren)
00059 iren = vtk.vtkRenderWindowInteractor()
00060 ren.SetBackground(1.0, 0.0, 0.0)
00061 xres = 350
00062 yres = 350
00063 renWin.SetSize(xres, yres)
00064 renWin.SetPosition(1000,200)
00065 worldPicker = vtk.vtkWorldPointPicker ()
00066 resolution =1
00067 viewAngle = 45
00068 ren.ResetCamera()
00069 cam = ren.GetActiveCamera()
00070 cam.SetFocalPoint(0,0,0)
00071 cam.SetClippingRange(0,10)
00072 cam.SetViewAngle(viewAngle)
00073
00074 stlMapper = vtk.vtkPolyDataMapper()
00075 stlMapper.SetInputConnection(reader1.GetOutputPort())
00076 stlMapper.Update()
00077
00078 CoM = sphere.computeCenterOfMass(stlMapper.GetInput())
00079 transCenter = vtk.vtkTransform()
00080 transCenter.Translate(-CoM[0],-CoM[1],-CoM[2])
00081 matrixCenter = transCenter.GetMatrix()
00082 transFilterCenter = vtk.vtkTransformFilter()
00083 transFilterCenter.SetTransform(transCenter)
00084 transFilterCenter.SetInputConnection(reader1.GetOutputPort())
00085
00086 stlMapper = vtk.vtkPolyDataMapper()
00087 stlMapper.SetInputConnection(transFilterCenter.GetOutputPort())
00088 stlMapper.Update()
00089
00090 bb = stlMapper.GetBounds()
00091 ms = max( (abs(bb[0]-bb[1]),abs(bb[2]-bb[3]),abs(bb[4]-bb[5]) ) )
00092 max_side=0.5
00093 scale_factor = max_side/ms
00094 vscale = vtk.vtkTransform()
00095
00096 vscale.Scale(scale_factor,scale_factor,scale_factor)
00097 matrixScale = vscale.GetMatrix()
00098
00099 transFilterScale = vtk.vtkTransformFilter()
00100 transFilterScale.SetTransform(vscale)
00101 transFilterScale.SetInputConnection(transFilterCenter.GetOutputPort())
00102
00103 stlMapper = vtk.vtkPolyDataMapper()
00104 stlMapper.SetInputConnection(transFilterScale.GetOutputPort())
00105 stlMapper.Update()
00106
00107
00108 radius=1
00109 firstPos = sphere.polygons[sphereLevel][0].getCenter()*radius
00110 gen_views = []
00111 views_transforms = []
00112 view = 0
00113
00114
00115 for polygon in sphere.polygons[sphereLevel]:
00116
00117 cam.SetPosition(polygon.getCenter()*radius)
00118
00119 pts = vtk.vtkPoints()
00120 ren.SetActiveCamera(cam)
00121 ren.ResetCameraClippingRange()
00122 stlActorTrans = vtk.vtkActor()
00123 stlActorTrans.SetMapper(stlMapper)
00124 ren.AddActor(stlActorTrans)
00125 ren.SetBackground(1.0, 0.0, 0.0)
00126
00127 renWin.Render()
00128
00129 w2i = vtk.vtkWindowToImageFilter()
00130 w2i.SetInput(renWin)
00131 imD = w2i.GetOutput()
00132 imD.Update()
00133 imD.SetNumberOfScalarComponents(3)
00134
00135 worldPicker.Pick(5,5,0,ren)
00136 p1 = numpy.array(worldPicker.GetPickPosition())
00137 worldPicker.Pick(xres-5,5,0,ren)
00138 p2 = numpy.array(worldPicker.GetPickPosition())
00139 worldPicker.Pick(5,yres-5,0,ren)
00140 p3 = numpy.array(worldPicker.GetPickPosition())
00141
00142 pn = numpy.cross(p2-p1, p3-p1 )
00143 pn = pn / sqrt(numpy.dot(pn,pn.conj()))
00144 d = numpy.dot(pn,p1.conj())
00145
00146 narr = []
00147
00148 for x in range(0,xres,resolution):
00149 for y in range(0,yres,resolution):
00150 if imD.GetScalarComponentAsFloat(x,y,0,0) == 255 and imD.GetScalarComponentAsFloat(x,y,0,1) == 0 and imD.GetScalarComponentAsFloat(x,y,0,2) == 0 :
00151 continue
00152 else:
00153 worldPicker.Pick(x,y,0,ren)
00154 nx,ny,nz = worldPicker.GetPickPosition()
00155
00156 if abs(numpy.dot([nx,ny,nz],pn.conj()) - d ) < 0.001:
00157 continue
00158
00159 narr.append([nx,ny,nz])
00160
00161
00162
00163 gen_views.append(narr)
00164 view += 1
00165
00166
00167 whole_mesh = list()
00168 for i in xrange(len(gen_views)):
00169 for j in xrange(len(gen_views[i])):
00170 whole_mesh.append(gen_views[i][j])
00171
00172 with open (outfile, 'w') as f:
00173 f.write ("COLUMNS x y z\n")
00174 f.write ("POINTS %i\n" % len(whole_mesh))
00175 f.write ("DATA ascii\n")
00176 for point in whole_mesh:
00177 for dim in point:
00178 f.write ("%f " % dim)
00179 f.write ("\n")
00180
00181
00182
00183 vis.visModelCam(stlMapper, cam, (1,0,1), whole_mesh, True)
00184
00185 if __name__ == "__main__":
00186 if (len(sys.argv) == 3):
00187 sample_views_tesselated_sphere(sys.argv[1], sys.argv[2])
00188 else:
00189 print "usage: %s infile outfile" % sys.argv[0]
00190
00191
