save_2d threading

colors_hls_3d_line.py

@@ -14,9 +14,9 @@
 #file_nr = 11050
 file_nr = 12000
 
-file_nr = 10000
+file_nr = 15860
 
-path = './scans/s/'
+path = './scans/p2/'
 extension = '.png'
 name1 = path + str(file_nr) + extension
 name2 = path + str(file_nr+1) + extension
@@ -26,16 +26,16 @@
 
 
 def transform(disp_img, img1, img2, t=25):
-    res = disp_img
+    res = disp_img.copy()
     mask, (x,y) = pi2R.lines2d.transform(img1, img2, t)
 
 #    res[mask] = [0.,0.,0.]
     color = [0,1.,1.]
     res[x,y] = color
-    res[x+1,y] = color
-    res[x-1,y] = color
-    res[x,y+1] = color
-    res[x,y-1] = color
+#    res[x+1,y] = color
+#    res[x-1,y] = color
+#    res[x,y+1] = color
+#    res[x,y-1] = color
 
     return res, (x,y)
 
@@ -45,28 +45,28 @@ def norm2(a):
 
 
 # Load an color image in grayscale
-img1 = cv2.imread(name1,cv2.IMREAD_COLOR)
+img1 = cv2.imread(name1,cv2.IMREAD_UNCHANGED)
 #img1 = cv2.GaussianBlur(img1,(15,5),0)
-img1 = img1.astype(np.float32)/255.
+img1 = img1.astype(np.float32)/float(np.iinfo(img1.dtype).max)
 img1_hls = cv2.cvtColor(img1, cv2.COLOR_BGR2HLS);
 
 #img1_hls = img1_hls.astype(np.float32)/[180.,255.,255.]
 
-img2 = cv2.imread(name2,cv2.IMREAD_COLOR)
+img2 = cv2.imread(name2,cv2.IMREAD_UNCHANGED)
 #img2 = cv2.GaussianBlur(img2,(15,5),0)
-img2 = img2.astype(np.float32)/255.
+img2 = img2.astype(np.float32)/float(np.iinfo(img2.dtype).max)
 img2_hls = cv2.cvtColor(img2, cv2.COLOR_BGR2HLS)
 
 disp_img = img2[:,:,[2,1,0]]
 
-disp_img, xy = transform(disp_img, img1_hls, img2_hls)
+disp_img2, xy = transform(disp_img, img1_hls, img2_hls)
 
 
 fig = plt.figure()
 
 ax = fig.add_subplot(1,2,1)
 
-imgplot = ax.imshow(disp_img)
+imgplot = ax.imshow(disp_img2)
 #imgplot = ax.imshow(norm2(pi2R.lines2d.y_data))
 
 rectangle = ax.add_patch(Rectangle((0, 0),2*N,2*N,alpha=0.2))
@@ -106,11 +106,11 @@ def onclick(event):
     print 'button=%d, x=%d, y=%d, xdata=%f, ydata=%f'%(
         event.button, event.x, event.y, event.xdata, event.ydata)
 
-r_t = 15.
+r_t = 1
 
 def onbutton(event):
     global N, r_x, r_y, r_t, disp_img
-    dt=1
+    dt=0.1
     if event.key == 'x':
         r_y+=1
     if event.key == 'w':
@@ -128,8 +128,8 @@ def onbutton(event):
     if event.key == 't':
         r_t+=dt
 
-    disp_img, xy = transform(disp_img, img1_hls, img2_hls, r_t)
-    imgplot.set_data(norm2(pi2R.lines2d.y_data))
+    disp_img2, xy = transform(disp_img, img1_hls, img2_hls, r_t)
+    imgplot.set_data(norm2(disp_img2))
     fig.canvas.draw()
     print 'r_t:',r_t
 

pi2R/lines.py

@@ -60,9 +60,7 @@ def rotate(self, angle):
         self.calc_()
 
     def calc_(self):
-#        print 'D=', self.laser_D
         self.a = -self.laser_D-np.dot(self.n_laser_N, self.n_cam_O)
-#        print 'a=', self.a
 
     def get_point_3d(self, point_2d):
         point_2d -= self.cam_resolution_2
@@ -107,19 +105,17 @@ def compute_points_3d_(self, points_2d):
 
 class Line:
     def __init__(self, normal_image_name, laser_image_name):
-        self.n_img = cv2.imread(normal_image_name, cv2.IMREAD_COLOR)
+        self.n_img = cv2.imread(normal_image_name, cv2.IMREAD_UNCHANGED)
+        self.n_img = self.n_img.astype(np.float32)/float(np.iinfo(self.n_img.dtype).max)
         #self.n_img = cv2.GaussianBlur(self.n_img,(15,5),0)
-        self.l_img = cv2.imread(laser_image_name, cv2.IMREAD_COLOR)
+        self.l_img = cv2.imread(laser_image_name, cv2.IMREAD_UNCHANGED)
+        self.l_img = self.l_img.astype(np.float32)/float(np.iinfo(self.l_img.dtype).max)
+
         #self.l_img = cv2.GaussianBlur(self.l_img,(15,5),0)
 
 
     def get_points_2d_(self):
         (x,y) = lines2d.get_points_2d_g(self.n_img, self.l_img)
-#        self._do_mask_rgb()
-#        a = np.argwhere(self.mask)
-#        if len(a) > 25000:
-#            print 'error rgb:', len(a)
-
         if len(x) > 3500:
             print 'error reduce:', len(x)
             return []
@@ -146,7 +142,7 @@ def get_points_3d(self, cam_laser):
         return retu
 
     def get_colors(self):
-        return [self.n_img[x,y] for y,x in self.points_2d]
+        return np.array([self.n_img[x,y] for y,x in self.points_2d])
 
     def get_colors_rgb(self):
-        return [ [r,g,b] for b,g,r in [self.n_img[x,y] for y,x in self.points_2d]]
+        return np.array([ [r,g,b] for b,g,r in [self.n_img[x,y] for y,x in self.points_2d]])

pi2R/lines2d.py

@@ -13,14 +13,6 @@
 from timeit import default_timer as timer
 
 
-def bright_mask(h1,s1,v1,h2,s2,v2):
-    # bright objects
-    mask_bright1 = 0.9 < v1
-    mask_bright2 = 0.9 < v2
-
-    mask_satur = s2+0.05 < s1
-    return np.logical_and(np.logical_and(mask_bright1, mask_bright2),mask_satur)
-
 def max_laser(y):
     start = timer()
 
@@ -45,31 +37,30 @@ def split(img):
 
 y_data = []
 
-def transform(img1, img2, t=25):
+def transform(img1, img2, t=3):
     global y_data, kernel
     start = timer()
     h1,l1,s1 = split(img1)
     h2,l2,s2 = split(img2)
 
     y_data = l2-l1
-    dy = 0.03
-    y_data[y_data<dy] = dy
-
-    kernel_size_x = 25
-    kernel_size_y = 11
-    kernel_x = np.ones((kernel_size_x), dtype=np.float32)*-1
-    kernel_x[kernel_size_x/4:(kernel_size_x*3)/4] = 1.
-    kernel = np.outer(signal.gaussian(kernel_size_y,7,),kernel_x)
-#    kernel/= kernel.sum()#kernel_size_y*kernel_size_x*2
+    dy = 0.01
+    y_data[y_data<dy] = -0.02
+
+    kernel_size_x = 15
+    kernel_size_y = 15
+    kt = 1.8
+    kernel = np.outer(signal.gaussian(kernel_size_y,kt,),signal.gaussian(kernel_size_x,kt,))
     kernel.astype(np.float32)
-#    a = kernel.sum()
     y_data = signal.fftconvolve(y_data, kernel, mode='same')
 
     ymax = y_data.max()
-    print 'kernel sum:', kernel.sum(), 'ymax:', ymax
+#    y_data[500:500+kernel_size_y,500:500+kernel_size_x] = ymax*kernel
+#    print 'kernel sum:', kernel.sum(), 'ymax:', ymax
     #d = ymax/10
-    print y_data
-    d = 0.5
+#    print y_data
+#    d = 0.5
+    d=0
     mask_not = y_data < d
     mask_not[:,0:20] = True
     mask_not[:,-20:] = True
@@ -88,11 +79,8 @@ def transform(img1, img2, t=25):
 
 def get_points_2d_g(img1, img2):
     start = timer()
-    img1_hsv = cv2.cvtColor(img1, cv2.COLOR_BGR2HLS);
-    img1_hsv = img1_hsv.astype(np.float32)/[180.,255.,255.]
-
+    img1_hsv = cv2.cvtColor(img1, cv2.COLOR_BGR2HLS)
     img2_hsv = cv2.cvtColor(img2, cv2.COLOR_BGR2HLS)
-    img2_hsv = img2_hsv.astype(np.float32)/[180.,255.,255.]
     image, (x,y) = transform(img1_hsv, img2_hsv)
     end = timer()
     print 'get_points_2d_g time:',end - start

pi2R/point_cloud.py

@@ -33,6 +33,8 @@ def addPointsAktor(self, points, colors):
         if len(points) > 0:
             obj = VTKObject()
             obj.CreateFromArray(points)
+            if colors.dtype == np.float32:
+                colors*=256
             obj.AddColors(colors.astype(np.uint8))
             self.pointObjects.append(obj)
             self.renderer.AddActor(obj.GetActor())
@@ -64,6 +66,15 @@ def addActors(self):
     def run(self):
 
         self.addActors()
+
+        transform = vtk.vtkTransform()
+        transform.Translate(1.0, 0.0, 0.0)
+
+        axes = vtk.vtkAxesActor()
+        #  The axes are positioned with a user transform
+#        axes.SetUserTransform(transform)
+        self.renderer.AddActor(axes)
+
     #    renderer.AddActor(pointCloud.vtkActor)
     # Begin Interaction
         self.renderWindow.Render()

print_cloud.py

@@ -11,7 +11,7 @@
 
 filename = 'points2d.dat'
 filename = 'a6.dat'
-filename = 'b8.dat'
+filename = 'a2.dat'
 
 
 
@@ -20,28 +20,27 @@
     inp = cPickle.load(input)
 
 
-def set_points(x = 0):
-    lN = v(0.37, 0.65, 0.)
+def set_points(x = 0.30):
+    lN = v(0.37-0.5+0.3, 0.65, 0.)
     print 'lN=', lN
     lN = np.dot(rotation_matrix(v(0.,0.,1.), math.pi/2.), lN)
     print 'lN=', lN
 
     cam_laser = CamLaser(cam_O=v(0.,0.065,0.),cam_C=v(0.,1.,0.),
-                     cam_DX=v(0.855/1944.,0.,0.), cam_DY=v(0.,0.,0.995/2592.), cam_resolution=v2(1944.,2592.),
+                     cam_DX=v(0.855/1944.,0.,0.), cam_DY=v(0.,0.,-0.995/2592.), cam_resolution=v2(1944./2,2592./2),
                      laser_N=lN, laser_O=v(-0.37,0.,0.))
 
-    cam_laser.rotateCam(-x)
+#    cam_laser.rotateCam(-x)
     skip = 0
     for i, p2d, colors in inp:
         skip+=1
         if skip % 1 != 0:
             continue
-
-        alfa = (i-10000)/2.*2.*math.pi/(2048.*3.)
+        alfa = -(i-10000)/2.*2.*math.pi/(2048.*3.)
         cam_laser.rotate(alfa)
-        rp = cam_laser.compute_points_3d(p2d.copy())
+        rp = cam_laser.compute_points_3d_(p2d.copy())
         print i, len(rp), len(colors), 'alfa=', alfa
-        pc.addPoints(rp, colors)
+        pc.addPoints(rp, 2*colors)
 
 def callback(obj, event):
     v = pc.getSliderValue(obj)
@@ -51,5 +50,5 @@ def callback(obj, event):
     pc.addActors()
 
 set_points()
-#pc.addSlider(callback)
+pc.addSlider(callback)
 pc.run()

print_lines.py

@@ -13,8 +13,11 @@
 
 pc = PointCloud()
 
-path = "./s4/"
+path = "./scans/p1/"
+extension = '.png'
 
+res_x = 1944/2
+res_y = 2592/2
 #    def __init__(self, cam_O, cam_C, cam_DX, cam_DY, cam_resolution, laser_O, laser_N):
 def v(x,y,z):
     return np.asarray([x,y,z])
@@ -28,20 +31,20 @@ def v2(x,y):
 print 'lN=', lN
 
 cam_laser = CamLaser(cam_O=v(0.,0.,0.),cam_C=v(0.,1.,0.),
-                     cam_DX=v(0.855/1944.,0.,0.), cam_DY=v(0.,0.,0.995/2592.), cam_resolution=v2(1944.,2592.),
+                     cam_DX=v(0.855/1944.,0.,0.), cam_DY=v(0.,0.,0.995/2592.), cam_resolution=v2(res_x,res_y),
                      laser_N=lN, laser_O=v(-0.37,0.,0.))
 
-for i in range(12000,12500,20):
+for i in range(10000,15100,200):
 
     start = timer()
-    line = Line(path + str(i) + ".jpg", path + str(i+1) + ".jpg")
+    line = Line(path + str(i) + extension, path + str(i+1) + extension)
     cam_laser.rotate(i/2.*2.*math.pi/(2048.*3.))
     rp = line.get_points_3d(cam_laser)
     colors = line.get_colors_rgb()
 #    print line.points_2d
 #    print colors
     print i, len(rp), len(colors), 'time:', timer() - start
 
-    pc.addPoints(rp, colors)
+    pc.addPoints(rp, colors*2)
 
 pc.run()

save_2d.py

@@ -7,23 +7,47 @@
 import math
 import cPickle
 from timeit import default_timer as timer
+import threading
+import Queue
 
 
-path = "./s4/"
-
-filename = 'points2d.dat'
+path = "./scans/p2/"
+extension = '.png'
+out_filename = 'a3.dat'
+threads = 8
 
 retu = []
 
+work_queue = Queue.Queue()
+result_queue = Queue.Queue()
+
+def worker_thread():
+    while not work_queue.empty():
+        start = timer()
+        i = work_queue.get()
+        line = Line(path + str(i) + extension, path + str(i+1) + extension)
+        rp = line.get_points_2d()
+        colors = line.get_colors_rgb()*256.
+        print i, len(rp), len(colors), 'time:', timer() - start
+        result_queue.put([i,np.asarray(rp, dtype=np.float32), np.asarray(colors, dtype=np.uint8)])
+        work_queue.task_done()
+
+
+for i in range(10000,15860,2):
+    work_queue.put(i)
+
+for i in range(1, threads):
+    worker = threading.Thread(target=worker_thread)
+    worker.setDaemon(True)
+    worker.start()
+
+work_queue.join()
 
-for i in range(10000,13100,2):
-    start = timer()
-    line = Line(path + str(i) + ".jpg", path + str(i+1) + ".jpg")
-    rp = line.get_points_2d()
-    colors = line.get_colors_rgb()
-    print i, len(rp), len(colors), 'time:', timer() - start
-    retu.append([i,np.asarray(rp, dtype=np.float32), np.asarray(colors, dtype=np.uint8)])
+while not result_queue.empty():
+    r = result_queue.get()
+    print 'got:', r[0]
+    retu.append(r)
 
 
-with open(filename, 'wb') as output:
+with open(out_filename, 'wb') as output:
     cPickle.dump(retu, output, cPickle.HIGHEST_PROTOCOL)