Skip to content
Browse files

Began updating table_calibration so I can update boundptsM. Probably …

…not worth it. Also started updating the demos so that they use wxpy3d rather than blockplayer.visuals. Finally, added a hardcoded offset to main.py in order to have the off-origin projection center.

Signed-off-by: Andrew Miller <amiller@dappervision.com>
  • Loading branch information...
1 parent 53be376 commit 746a45e0a332d7d461adeb9cba63d2a2a18044c9 @amiller committed Jun 6, 2012
Showing with 84 additions and 24 deletions.
  1. +10 −1 blockplayer/main.py
  2. +12 −3 blockplayer/table_calibration.py
  3. +2 −2 demos/demo_align.py
  4. +3 −2 demos/demo_grid.py
  5. +2 −1 demos/demo_lattice.py
  6. +54 −14 demos/demo_meanshift.py
  7. +1 −1 demos/demo_normals.py
View
11 blockplayer/main.py
@@ -52,6 +52,15 @@ def from_rect(m,rect):
# Find the lattice orientation and then translation
global R_oriented, R_aligned, R_correct
R_oriented = lattice.orientation_opencl()
+
+ # Use a preferred initial location
+ LW = config.LW
+ modelmat = R_oriented
+ modelmat = np.linalg.inv(modelmat)
+ modelmat[:3,3] += [0, 0, np.round(0.45/LW)*LW]
+ modelmat = np.linalg.inv(modelmat).astype('f')
+ R_oriented = modelmat
+
R_aligned = lattice.translation_opencl(R_oriented)
# Use occvac to estimate the voxels from just the current frame
@@ -99,7 +108,7 @@ def from_rect(m,rect):
#print 'nothing happened'
return
- def matrix_slerp(matA, matB, alpha=0.6):
+ def matrix_slerp(matA, matB, alpha=0.4):
if matA is None:
return matB
import transformations
View
15 blockplayer/table_calibration.py
@@ -85,6 +85,9 @@ def make_mask(boundpts, size=(640,480)):
return mask
+def make_boundptsM(boundpts, KtableKK, tb0, tableplane):
+ pass
+
def find_plane(depth, boundpts):
from wxpy3d.camerawindow import CameraWindow
global window
@@ -111,9 +114,6 @@ def find_plane(depth, boundpts):
#tablemean = np.array([x,y,z])
# Backproject the table plane into the image using inverse transpose
- global tb0
- tb0 = np.dot(KK.T, tableplane)
- tb0[2] = tb0[2]
# Build a matrix projecting sensor points to an system with
# the origin on the table, and Y pointing up from the table
@@ -129,7 +129,16 @@ def find_plane(depth, boundpts):
KtableKK = np.dot(Ktable, KK).astype('f')
+ #tableplane2 = np.linalg.inv(KtableKK)[1,:]
+ #tableplane2
+
+ #within_eps = lambda a, b: np.abs(a-b) < 1e-5
+ #assert within_eps(tableplane2, tableplane)
+
global boundptsM
+ tb0 = np.dot(KK.T, tableplane)
+ tb0[2] = tb0[2]
+
boundptsM = []
for (up,vp) in boundpts:
# First project the image points (u,v) onto to the (imaged) tableplane
View
4 demos/demo_align.py
@@ -16,7 +16,7 @@
from blockplayer import hashalign
from blockplayer import blockdraw
-from blockplayer.visuals.blockwindow import BlockWindow
+from blockplayer.blockwindow import BlockWindow
global window
if not 'window' in globals():
window = BlockWindow(title='demo_align', size=(640,480))
@@ -58,7 +58,7 @@ def once():
bestmatch, besterr = hashalign.find_best_alignment(GT, GT*0,
problem, problem*0)
- if 0:
+ if 1:
for match in matches:
show_align(GT, hashalign.apply_correction(problem, *match))
window.Refresh()
View
5 demos/demo_grid.py
@@ -74,9 +74,10 @@ def once():
rgb,_ = opennpy.sync_get_video()
main.update_frame(depth, rgb)
+ print main.R_aligned
blockdraw.clear()
- #blockdraw.show_grid('o1', main.occvac.occ, color=np.array([1,1,0,1]))
+ blockdraw.show_grid('o1', main.occvac.occ, color=np.array([1,1,0,1]))
if 'RGB' in stencil.__dict__:
blockdraw.show_grid('occ', grid.occ, color=grid.color)
else:
@@ -91,7 +92,7 @@ def once():
#window.clearcolor=[1,1,1,0]
window.flag_drawgrid = True
- if 1:
+ if 0:
update_display()
if 'R_correct' in main.__dict__:
View
3 demos/demo_lattice.py
@@ -1,11 +1,12 @@
import numpy as np
import pylab
from OpenGL.GL import *
+import opennpy
if not 'FOR_REAL' in globals():
FOR_REAL = False
-from blockplayer.visuals.pointwindow import PointWindow
+from wxpy3d.pointwindow import PointWindow
global window
if not 'window' in globals():
window = PointWindow(title='demo_lattice', size=(640,480))
View
68 demos/demo_meanshift.py
@@ -1,8 +1,12 @@
import numpy as np
from OpenGL.GL import *
+from OpenGL.GLUT import *
+from OpenGL.GLU import *
+
from blockplayer import expmap
from wxpy3d import PointWindow
from wxpy3d.opengl_state import opengl_state
+
global window
if not 'window' in globals():
window = PointWindow(title='demo_meanshift', size=(640,480))
@@ -18,7 +22,7 @@ def random_basis():
X = normalize(np.cross(Y, Z))
return np.array([X,Y,Z]).astype('f')
-def random_points(N=1000, mu=0.1):
+def random_points(N=1000, mu=0.2):
basis = random_basis()
ind = np.random.randint(3, size=(N,))
points = (np.sign(np.random.rand(N,1)-0.5) * basis[ind, :] +
@@ -54,9 +58,12 @@ def mean_shift(estimate, nxyz, d=0.3):
global cx, cy, cz
cx, cy, cz = cc
- nx = ((cy-cz)*Y*Z).sum() / (cy+cz).sum()
- ny = ((cz-cx)*Z*X).sum() / (cz+cx).sum()
- nz = ((cx-cy)*X*Y).sum() / (cx+cy).sum()
+ nx = ((cy-cz)*Y*Z).sum() / (cy+cz).sum() / 1 \
+ if (cy+cz).sum() > 0 else 0
+ ny = ((cz-cx)*Z*X).sum() / (cz+cx).sum() / 1 \
+ if (cz+cx).sum() > 0 else 0
+ nz = ((cx-cy)*X*Y).sum() / (cx+cy).sum() / 1 \
+ if (cx+cy).sum() > 0 else 0
rgba = np.array(cc + [np.ones_like(cc[0])]).transpose()
window.update_points(XYZ=nxyz, RGBA=rgba/2+.5)
@@ -67,6 +74,7 @@ def mean_shift(estimate, nxyz, d=0.3):
@window.event
def post_draw():
with opengl_state():
+ if 0:
B = np.eye(4); B[:3,:3] = basis
glMultMatrixf(np.linalg.inv(B).transpose())
glScale(1.3, 1.3, 1.3)
@@ -79,6 +87,23 @@ def post_draw():
glEnd()
with opengl_state():
+ #quad = gluNewQuadric()
+ #gluSphere(quad, 0.99, 10, 10)
+ glEnable(GL_LINE_STIPPLE)
+ glLineStipple(3, 0xAAAA)
+ glColor(0.1,0.1,0.1)
+ glutWireSphere(1, 10, 10)
+ glTranslate(-1,-1,-1)
+ glScale(0.3,0.3,0.3)
+ glBegin(GL_LINES)
+ glColor(1,0,0); glVertex(0,0,0); glVertex(1,0,0)
+ glColor(0,1,0); glVertex(0,0,0); glVertex(0,1,0)
+ glColor(0,0,1); glVertex(0,0,0); glVertex(0,0,1)
+ glEnd()
+
+
+ with opengl_state():
+ if 1:
B = np.eye(4); B[:3,:3] = estimate
glMultMatrixf(np.linalg.inv(B).transpose())
glLineWidth(2)
@@ -87,6 +112,9 @@ def post_draw():
glColor(1,0,0); glVertex(0,0,0); glVertex(1,0,0)
glColor(0,1,0); glVertex(0,0,0); glVertex(0,1,0)
glColor(0,0,1); glVertex(0,0,0); glVertex(0,0,1)
+ #glColor(1,0,0); glVertex(-1,0,0); glVertex(1,0,0)
+ #glColor(0,1,0); glVertex(0,-1,0); glVertex(0,1,0)
+ #glColor(0,0,1); glVertex(0,0,-1); glVertex(0,0,1)
glEnd()
@@ -98,22 +126,34 @@ def EVT_IDLE(*kwargs):
window.Refresh()
+D = 0.4
+
def meanshift():
global estimate
- for i in xrange(80):
- estimate = np.dot(estimate, np.linalg.inv(mean_shift(estimate, nxyz)))
- mean_shift(estimate, nxyz)
+ for i in xrange(20):
+ estimate = np.dot(np.linalg.inv(mean_shift(estimate, nxyz, D)), estimate)
+ mean_shift(estimate, nxyz, d=D)
window.Refresh()
- pylab.waitforbuttonpress(0.1)
+ pylab.waitforbuttonpress(0.2)
-#basis, nxyz = random_points()
+basis, nxyz = random_points(mu=0.1)
estimate = random_basis()
-estimate = array([[ 0.86042833, 0.48787376, 0.14711326],
- [-0.40038016, 0.46868348, 0.78742081],
- [ 0.31521237, -0.73642039, 0.59860349]],
- dtype=float32)
+#estimate = array([[ 0.86042833, 0.48787376, 0.14711326],
+# [-0.40038016, 0.46868348, 0.78742081],
+# [ 0.31521237, -0.73642039, 0.59860349]],
+# dtype=float32)
-mean_shift(estimate, nxyz)
+mean_shift(estimate, nxyz, D)
#window.update_points(XYZ=nxyz)
window.Refresh()
+
+@window.eventx
+def EVT_KEY_DOWN(evt):
+ global basis, nxyz
+ if evt.GetKeyCode() == ord('R'):
+ basis, nxyz = random_points(mu=0.1)
+ mean_shift(estimate, nxyz, D)
+ if evt.GetKeyCode() == ord('M'):
+ meanshift()
+ window.Refresh()
View
2 demos/demo_normals.py
@@ -20,7 +20,7 @@
if not 'FOR_REAL' in globals():
FOR_REAL = False
-from blockplayer.visuals.pointwindow import PointWindow
+from wxpy3d.pointwindow import PointWindow
global window
if not 'window' in globals():
window = PointWindow(title='demo_normals', size=(640,480))

0 comments on commit 746a45e

Please sign in to comment.
Something went wrong with that request. Please try again.