def __init__(self):
self.k = Kinect(debug=True)
self.k.start()
self.k.wait_for_init()
self.pcd = open3d.PointCloud()
self.vis = open3d.Visualizer()
self.vis.create_window()
def __init__(self, model_file):
"""
@param model_file: String with path to model file.
"""
self.model_file = model_file
print("Loading model...")
self.load_model()
print("Model loaded.")
# Start the kinect
self.k = Kinect(debug=True, pointcloud=False, color=True)
self.k.start()
self.k.wait_for_init()
def cb_test():
k = Kinect()
k.start()
k.wait_for_init()
p = k.get_pointcloud()
p[:,3:] = p[:,3:] / 255.0
k.stop()
return p
def test():
k = Kinect()
k.start()
k.wait_for_init()
p = k.get_pointcloud()
p[:,3:] = p[:,3:] / 255.0
k.stop()
v = Visualize(p)
v.run()
def __init__(self):
self.k = Kinect(debug=True)
self.k.start()
self.k.wait_for_init()
self.is_running = True
self.pcd1 = open3d.PointCloud()
self.pcd2 = open3d.PointCloud()
self.vis = open3d.Visualizer()
#self.vis = open3d.VisualizerWithKeyCallback()
#self.vis.register_key_callback(ord('k'), self.callback_test)
self.vis.create_window()
self.vis.add_geometry(self.pcd1)
self.vis.add_geometry(self.pcd2)
class NonBlockVis(object):
def __init__(self):
self.k = Kinect(debug=True)
self.k.start()
self.k.wait_for_init()
self.pcd = open3d.PointCloud()
self.vis = open3d.Visualizer()
self.vis.create_window()
#self.vis.add_geometry(self.pcd)
#self.vis.run()
def show(self):
pointcloud = self.k.get_pointcloud()
print(f"pointcloud shape: {pointcloud.shape}")
d = pointcloud[:, :3]
c = pointcloud[:, 3:] / 255.0
self.pcd.points = open3d.Vector3dVector(d)
self.pcd.colors = open3d.Vector3dVector(c)
self.vis.add_geometry(self.pcd)
self.vis.update_geometry()
self.vis.poll_events()
self.vis.update_renderer()
self.vis.run()
def stop(self):
self.k.stop()
class Open3DVisualize():
def __init__(self):
self.k = Kinect(debug=True)
self.k.start()
self.k.wait_for_init()
self.is_running = True
self.pcd1 = open3d.PointCloud()
self.pcd2 = open3d.PointCloud()
self.vis = open3d.Visualizer()
#self.vis = open3d.VisualizerWithKeyCallback()
#self.vis.register_key_callback(ord('k'), self.callback_test)
self.vis.create_window()
self.vis.add_geometry(self.pcd1)
self.vis.add_geometry(self.pcd2)
#self.display_pc()
#print(f"GEOMS: {len(self.geoms)}")
#self.draw()
def end(self):
self.is_running = False
self.k.stop()
def run(self):
#while self.is_running:
# time.sleep(1)
# print("ite")
#print("Stopping...")
#self.end()
self.draw()
def draw(self):
key_to_callback = {}
key_to_callback[ord("K")] = self.callback_test
#open3d.draw_geometries_with_key_callbacks(self.geoms, key_to_callback)
#open3d.draw_geometries_with_key_callbacks([self.pcd1, self.pcd2], key_to_callback)
def display_pc(self):
print("Displayng PC")
pointcloud = self.k.get_pointcloud()
d = pointcloud[:, :3]
c = pointcloud[:, 3:] / 255.0
#pcd = open3d.PointCloud()
self.pcd1.points = open3d.Vector3dVector(d)
self.pcd1.colors = open3d.Vector3dVector(c)
#self.geoms.append(pcd)
#self.vis.add_geometry(pcd)
#self.geoms.append(pcd)
self.vis.update_geometry()
self.vis.poll_events()
self.vis.update_renderer()
def get_pc(self):
print("Grabbing pointcloud")
return self.k.get_pointcloud()
def visualize_animation(self):
self.vis.update_geometry()
self.vis.reset_view_point(True)
self.vis.poll_events()
self.vis.update_renderer()
def callback_test(self, vis):
self.is_running = False
print("Callback!")
print(dir(vis))
#d = np.asarray(vis.capture_depth_float_buffer())
#print(d.shape)
def visualize(self):
pointcloud = self.k.get_pointcloud()
d = pointcloud[:, :3]
c = pointcloud[:, 3:] / 255.0
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(d)
pcd.colors = open3d.Vector3dVector(c)
"""
vis = open3d.Visualizer()
vis.create_window()
vis.add_geometry(pcd)
vis.run()
vis.destroy_window()
"""
key_to_callback = {}
key_to_callback[ord("K")] = self.callback_test
open3d.draw_geometries_with_key_callbacks([pcd], key_to_callback)
#import Open3DVisualize as vis
from SimpleNonBlocking import O3DVisualize
def example_help_function():
import open3d
help(open3d)
help(open3d.PointCloud)
help(open3d.read_point_cloud)
if __name__ == "__main__":
#example_help_function()
k = Kinect()
k.start()
k.wait_for_init()
o3d_vis = O3DVisualize()
for i in range(10):
o3d_vis.update_pointcloud(k.get_pointcloud())
#time.sleep(0.5)
o3d_vis.stop()
k.stop()
#vis.visualize_animation()
#v = vis.Open3DVisualize()
#v.visualize()
imgproc.COLOR_MAX[2] = value
# 1 - Webcam
# 2 - File
action = 2
imgproc = None
if action == 1:
imgproc = Imgproc(0)
else:
imgproc = Imgproc(-1)
kinect = None
#if len(sys.argv) < 1:
kinect = Kinect()
cv2.namedWindow('Original, HSV, Thresh, Rects')
cv2.namedWindow('Tuning Window')
cv.CreateTrackbar("Lower H", 'Tuning Window', imgproc.COLOR_MIN[0], 255,
update_lowerH)
cv.CreateTrackbar("Lower S", 'Tuning Window', imgproc.COLOR_MIN[1], 255,
update_lowerS)
cv.CreateTrackbar("Lower V", 'Tuning Window', imgproc.COLOR_MIN[2], 255,
update_lowerV)
cv.CreateTrackbar("Upper H", 'Tuning Window', imgproc.COLOR_MAX[0], 255,
update_upperH)
cv.CreateTrackbar("Upper S", 'Tuning Window', imgproc.COLOR_MAX[1], 255,
update_upperS)
cv.CreateTrackbar("Upper V", 'Tuning Window', imgproc.COLOR_MAX[2], 255,
update_upperV)
import numpy as np import pandas as pd import time import cv2 import matplotlib.pyplot as plt from scipy.linalg import lstsq from pyntcloud import PyntCloud as pc from tqdm import tqdm_notebook as tqdm import numba as nb from Kinect import Kinect from Planes import find_plane, distance_from_plane from watershed import watershed k = Kinect(debug=True) k.start() k.wait_for_init() point_cloud = k.get_pointcloud() k.stop() points = pd.DataFrame(point_cloud.astype(np.float32), columns=['x', 'y', 'z', 'red', 'green', 'blue']) cloud = pc(points) #cloud.plot(IFrame_shape=(1200, 700), point_size=0.001) cloud.plot(width=900, initial_point_size=0.01) time.sleep(5)
def update_upperV(value):
imgproc.COLOR_MAX[2] = value
# 1 - Webcam
# 2 - File
action = 2
imgproc = None
if action == 1:
imgproc = Imgproc(0)
else:
imgproc = Imgproc(-1)
kinect = None
#if len(sys.argv) < 1:
kinect = Kinect()
cv2.namedWindow('Original, HSV, Thresh, Rects')
cv2.namedWindow('Tuning Window')
cv.CreateTrackbar("Lower H", 'Tuning Window', imgproc.COLOR_MIN[0], 255, update_lowerH)
cv.CreateTrackbar("Lower S", 'Tuning Window', imgproc.COLOR_MIN[1], 255, update_lowerS)
cv.CreateTrackbar("Lower V", 'Tuning Window', imgproc.COLOR_MIN[2], 255, update_lowerV)
cv.CreateTrackbar("Upper H", 'Tuning Window', imgproc.COLOR_MAX[0], 255, update_upperH)
cv.CreateTrackbar("Upper S", 'Tuning Window', imgproc.COLOR_MAX[1], 255, update_upperS)
cv.CreateTrackbar("Upper V", 'Tuning Window', imgproc.COLOR_MAX[2], 255, update_upperV)
while 1:
# Load the image from the camera (or a static file for testing)
if len(sys.argv) > 1:
if action == 2:
pos = p[:, :3]
colors = p[:, 3:] / 255.0
self.scatter.set_data(pos,
face_color=colors,
edge_color=colors,
size=self._point_size,
edge_width=self._edge_width,
symbol=self._symbol,
scaling=True)
self.view.camera = 'turntable'
self.view.camera.fov = 45
#self.view.camera.distance = 1
if __name__ == "__main__":
from Kinect import Kinect
k = Kinect()
k.start()
k.wait_for_init()
v = Vispy(k.get_pointcloud)
#pc = k.get_pointcloud()
v.run()
k.stop()
rep = np.repeat(
np.array([[width_ratio, height_ratio, width_ratio, height_ratio]]),
fbboxes.shape[0], axis=0)
rscaled_bboxes = fbboxes * rep
out_img = img
else:
rscaled_bboxes = fbboxes
out_img = timg
box_list = BboxList.from_arrays(fids, fscores, rscaled_bboxes, self.classes, th=threshold)
return box_list, timg
rospy.init_node('talker', anonymous=True)
cam=Kinect()
rospack=rospkg.RosPack()
path=rospack.get_path("ssggcnn_ur5_grasping")
det=Detector(path + "/scripts/detection_pkg/model.params")
arraypub = rospy.Publisher('sdd_points_array', Int32MultiArray, queue_size=10)
ssd_img_pub = rospy.Publisher('ssd/img/bouding_box', Image, queue_size=1)
rospy.Subscriber("/camera/color/image_raw", Image, cam.set_image)
rate = rospy.Rate(10) # 10hz
points_to_send = Int32MultiArray()
while not rospy.is_shutdown():
if cam.has_image > 0:
im=cam.image
with TimeIt('ssd_process_time'):
[caixas,timag]= det.detect(im)
size = len(caixas)
#!/usr/bin/python2
import sys
import os
sys.path.append('lib')
import cv2
from cv2 import cv
import numpy as np
import freenect
from imgproc import *
from Kinect import Kinect
import Lock
imgproc = Imgproc(-1)
kinect = Kinect()
params = [cv.CV_IMWRITE_PNG_COMPRESSION, 8]
SCALE_FACTOR = 3
msg = ""
def getTargets():
image = kinect.get_IR_image()
depth = kinect.get_depth()
targets = imgproc.getRect(image)
targets = imgproc.filterRects(targets)
imgproc.labelRects(image, targets)
return targets, image
class Demo(object):
"""
Demo showcasing gesture recognistion model.
"""
def __init__(self, model_file):
"""
@param model_file: String with path to model file.
"""
self.model_file = model_file
print("Loading model...")
self.load_model()
print("Model loaded.")
# Start the kinect
self.k = Kinect(debug=True, pointcloud=False, color=True)
self.k.start()
self.k.wait_for_init()
def load_model(self):
"""
Method for loading a keras model
"""
# Due to the way Keras handles custom loss functions, we need to create a loss function here.
# While some sort of stub could be used, it's easier to just use the one used when training the model
l = create_loss_function(20, 20, LABEL_WEIGHT, OFFSET_LOSS_WEIGHT,
NUM_CLASSES, EPSILON, BATCHSIZE)
self.model = load_model(self.model_file,
custom_objects={'loss_function': l})
def resize_image(self, im, target_size):
"""
Resize an image. Cuts size of any dimension too large after resize.
@param im: Image to resize
@param target_size: Target size of image
"""
# Find the resize factor
resize_x_factor = im.shape[0] / target_size[0]
resize_y_factor = im.shape[1] / target_size[1]
resize_factor = np.min([resize_x_factor, resize_y_factor])
# Find the new size of the image
new_size = (int(im.shape[1] / resize_factor),
int(im.shape[0] / resize_factor))
# Resize
im_resize = cv2.resize(im, new_size)
# Init array for output image
im_out = np.zeros(target_size)
# Cut width, if needed
if im_out.shape[0] > target_size[0]:
extra_pixels = im_out.shape[0] - target_size[0]
remove_from_each_side = int(extra_pixels / 2)
width_min = remove_from_each_side
width_max = im_out.shape[0] - remove_from_each_side
else:
height_min = 0
height_max = target_size[0]
# Cut height, if needed
if im_out.shape[1] > target_size[1]:
extra_pixels = im_out.shape[1] - target_size[1]
remove_from_each_side = int(extra_pixels / 2)
width_min = remove_from_each_side
width_max = im_out.shape[1] - remove_from_each_side
else:
width_min = 0
width_max = target_size[1]
# Slize image, and load into result
im_out = im_resize[width_min:width_max, height_min:height_max]
return im_out
def preprocess(self, im):
"""
Preprocess an image, so it's ready for inputting the the model.
@param im: Image to preprocess
"""
#print(f"shape:{im.shape}")
im_pre = cv2.cvtColor(im, cv2.COLOR_RGB2GRAY).astype(np.float32)
# Mean subtraction
im_pre -= np.mean(im_pre)
# Normalization
im_pre /= np.std(im_pre)
return im_pre.astype(np.float32)
def get_image(self):
"""
Get an image from the Kinect
"""
im = self.k.get_color_image()
return im
def stop(self):
"""
Stop execution, cleanup.
"""
print("Stopping...")
self.k.stop()
def predict_points(self, im):
"""
Predict points in an image.
@param im: Image to predict points from
"""
# Preprocess the image
im_pre = self.preprocess(im)
# Make prediction
res = self.model.predict(im_pre.reshape(1, WIDTH, HEIGHT, CHANNELS))
# Get anchor matrix
anchors = get_anchors(WIDTH, HEIGHT, 20, 20)
# Loop over all points/classes
for i in range(NUM_CLASSES):
# Initialize a prediction matrix for single point
pred = np.zeros((20, 20, 3))
# Load with data
pred[:, :, 0] = res[0, :, :, i]
pred[:, :, 1] = res[0, :, :, NUM_CLASSES + i * 2]
pred[:, :, 2] = res[0, :, :, NUM_CLASSES + 1 + i * 2]
# And get all the points from this single prediction
pred_point = get_all_points_from_prediction(
pred,
anchors,
threshold=THRESHOLD,
offset_weight=int(320 / 20) / 2,
is_label=False)
# If any points found, get the mean value as result
if len(pred_point) > 0:
x_points = np.zeros(len(pred_point))
y_points = np.zeros(len(pred_point))
for counter, p in enumerate(pred_point):
x_points[counter] = p[0] + p[2]
y_points[counter] = p[1] + p[2]
x = np.mean(x_points)
y = np.mean(y_points)
# And draw it in the image
cv2.circle(im, (int(x), int(y)), 1, (255, 0, 0), thickness=2)
def run(self):
"""
Start the demo
"""
while (True):
# Get an image
im = self.get_image()
# Resize the imaee
im = self.resize_image(im, (WIDTH, HEIGHT))
# And make prediction
self.predict_points(im)
im = cv2.resize(im, dsize=(0, 0), fx=2, fy=2)
cv2.imshow("Main", im)
# Run till q is pressed
if cv2.waitKey(1) == ord('q'):
self.stop()
break
point_cloud = k.get_pointcloud()
point_cloud = ransac.ransac(point_cloud, 300, 10, 8000)
#with self.cloud_lock:
# self.pc = point_cloud
return point_cloud
def get_pointcloud(self, ransac):
if ransac:
return self.do_ransac()
else:
return k.get_pointcloud()
#with self.cloud_lock:
# return self.pc
k = Kinect(debug=False)
#k.scale_size(200)
k.scale_rgb(True)
k.start()
ri = RansacInterface(k)
#ri.start()
vis = Vispy(ri.get_pointcloud, point_size=0.004, edge_width=0.004, symbol='o')
#vis = Vispy(k.get_pointcloud)
vis.run()
#ri.stop()
#ri.join()
k.stop()