def perturb(max_iters=100, mod=10): global pnew, uv, err, points_range, rimg, range_image # Apply a perturb to points_p obj.RT = np.eye(4, dtype='f') obj.RT[:3,3] = -obj.vertices[:,:3].mean(0) obj.RT[:3,3] += [0,0,-3.0] # Rotated object view RT = obj.RT rp = random_perturbation().astype('f') obj.RT = np.dot(rp, obj.RT) range_image = obj.range_render(camera.kinect_camera()) obj.RT = RT points_range = range_image.point_model(True) # Original object view rimg = obj.range_render(camera.kinect_camera()) pnew = rimg.point_model() # Estimate the transformation rp for iters in range(max_iters): npairs, pnew = model.align(range_image, pnew, rtmodel.RATE1, rtmodel.DIST1, 6) #pnew, err, npairs, uv = fasticp.fast_icp(range_image, pnew, 0.1, dist=0.05) if iters % mod == 0 or 1: #print '%d iterations, [%d] RMS: %.3f' % (iters, npairs, np.sqrt(err)) window.Refresh() pylab.waitforbuttonpress(0.02) break window.Refresh()
def animate_random(max_iters=1000, mod=100): global pnew, points_range # Apply a perturb to points_p obj.RT = np.eye(4, dtype='f') obj.RT[:3,3] = -obj.vertices[:,:3].mean(0) obj.RT[:3,3] += [0,0,-3.0] RT = obj.RT prev_rimg = obj.range_render(camera.kinect_camera()) window.canvas.SetCurrent() pnew = prev_rimg.point_model(True) points_range = pnew if 0: obj.RT = np.dot(RT, M) rimg = obj.range_render(camera.kinect_camera()) window.canvas.SetCurrent() pm = rimg.point_model(True) points_range = pm for iters in range(max_iters): pnew, err, npairs, uv = fasticp.fast_icp(rimg, pnew, 1000, dist=0.005) if iters % mod == 0: # print '%d iterations, [%d] RMS: %.3f' % (iters, npairs, np.sqrt(err)) window.Refresh() pylab.waitforbuttonpress(0.02) pnew = pm window.Refresh() pylab.waitforbuttonpress(0.02)
def metric_points(depth, points): assert len(points) == 3 assert depth.dtype == np.uint16 rimg = RangeImage(depth, kinect_camera()) rimg.compute_points() pts = [] for x,y in points: pts.append(rimg.xyz[y,x,:]) pts = np.concatenate((pts,pts)) assert pts.shape == (6,3) return pts
def once(): global range_image, points_range, RTold, RTguess ts, M = seqiter.next() # Take the image from an alternate camera location obj.RT = np.dot(RTold, M) range_image = obj.range_render(camera.kinect_camera()) points_range = range_image.point_model() if RTguess is None: range_image.camera.RT = np.eye(4, dtype='f') else: range_image.camera.RT = RTguess p = model.add(range_image) if p: points_range = p RTguess = p.RT window.Refresh() pylab.waitforbuttonpress(0.02)
def load_obj(name='blueghost'): global obj, points, points_p, range_image, points_range, pnew #obj = mesh.load_random() window.canvas.SetCurrent() obj = mesh.load(name) obj.calculate_area_lookup() obj.RT = np.eye(4, dtype='f') obj.RT[:3,3] = -obj.vertices[:,:3].mean(0) obj.RT[:3,3] += [0,0,-3.0] points = obj.sample_point_cloud(10000) # Range image of the original points range_image = obj.range_render(camera.kinect_camera()) range_image.compute_normals() points_range = range_image.point_model() pnew = points_p = points #range_image = obj.range_render(np.dot(camera.kinect_camera(), rp)) window.lookat = obj.RT[:3,3] + obj.vertices[:,:3].mean(0) window.Refresh()
import numpy as np import os import shutil import cv2 import datasetfull as dataset import pylab from wxpy3d import PointWindow from OpenGL.GL import * from rtmodel import pointmodel from rtmodel.rangeimage import RangeImage from rtmodel.camera import kinect_camera cam = kinect_camera() if not 'window' in globals(): window = PointWindow(size=(640,480)) def show_rgb(name, image): cv2.imshow(name, np.ascontiguousarray(image[:,:,::-1])) def show_depth(name, depth): cv2.imshow(name, 1024./depth) @window.event def pre_draw(): glLightfv(GL_LIGHT0, GL_POSITION, (-40, 200, 100, 0.0)) glLightfv(GL_LIGHT0, GL_AMBIENT, (0.3, 0.3, 0.3, 0.0)) glLightfv(GL_LIGHT0, GL_DIFFUSE, (0.3, 0.3, 0.3, 0.0)) glEnable(GL_LIGHT0) glEnable(GL_LIGHTING) #glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE) glEnable(GL_COLOR_MATERIAL)
def fwd_cam(): cam = camera.kinect_camera() cam.RT[:3,3] += [0, 0, 3.0] return cam