MS Kinect V2 から実世界の座標を生成しようとしています。
pyqt + opengl 散布図をつなぎ合わせ、pylibfreenect2 を使用して Kinect からの深度データを表示することができました。
深度データが点群データと同じではないことにすぐに気付きました。私の部屋の天井が非常に歪んでいることに注意してください (平らなはずの天井がホッケースティック グラフのようになり始めています)。
深度フレームをプロットした結果
ソースファイルを読んで掘り下げた後、非常に有望な関数を見つけることができました。
getPointXYZ - 点群で 3D ポイントを構築します。
一度に 1 つのピクセルでしか機能しないため、単純なネストされた for ループを作成しました。以下のコードでは、次の行が表示されます。
out = np.zeros((d.shape[0]*d.shape[1], 3)) #shape = (217088, 3)
for row in range(d.shape[0]):
for col in range(d.shape[1]):
world = registration.getPointXYZ(undistorted, row, col) #convert depth pixel to real-world coordinate
out[row + col] = world
getPointXYZ() からの座標の結果
何が起こっているのかわからない。それは直線のように見えますが、時には長方形に似ていて、非常に平らです (それでも、3 次元すべてで任意の角度に配置されています)。センサーの前で手を動かすと、いくつかのポイントが動き回るのがわかりますが、宣言可能な形状は表示されません。すべてのポイントが詰め込まれているようです。
以下は、openGL 散布図を含む pyQt アプリケーション ウィンドウを表示する Python スクリプトです。フレームは pylibfreenect2 を介して Kinect センサーから受信され、散布図のポイントは深度データの各行と列を反復処理し、getPointXYZ を介して送信することによって生成されます (これは非常に遅く、機能しません...)。
# coding: utf-8
# An example using startStreams
from pyqtgraph.Qt import QtCore, QtGui
import pyqtgraph.opengl as gl
import numpy as np
import cv2
import sys
from pylibfreenect2 import Freenect2, SyncMultiFrameListener
from pylibfreenect2 import FrameType, Registration, Frame, libfreenect2
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial)
types = 0
types |= FrameType.Color
types |= (FrameType.Ir | FrameType.Depth)
listener = SyncMultiFrameListener(types)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# NOTE: must be called after device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
#QT app
app = QtGui.QApplication([])
w = gl.GLViewWidget()
w.show()
g = gl.GLGridItem()
w.addItem(g)
#initialize some points data
pos = np.zeros((1,3))
sp2 = gl.GLScatterPlotItem(pos=pos)
w.addItem(sp2)
def update():
frames = listener.waitForNewFrame()
ir = frames["ir"]
color = frames["color"]
depth = frames["depth"]
d = depth.asarray()
registration.apply(color, depth, undistorted, registered)
#There are 3 optionally commented methods for generating points data (the last one is not commented here).
#First will generate points using depth data only.
#Second will generate colored points and pointcloud xyz coordinates.
#Third is simply the pointcloud xyz coordinates without the color mapping.
"""
#Format depth data to be displayed
m, n = d.shape
R, C = np.mgrid[:m, :n]
out = np.column_stack((d.ravel() / 4500, C.ravel()/m, (-R.ravel()/n)+1))
"""
"""
#Format undistorted and regisered data to real-world coordinates with mapped colors (dont forget color=out_col in setData)
out = np.zeros((d.shape[0]*d.shape[1], 3)) #shape = (217088, 3)
out_col = np.zeros((d.shape[0]*d.shape[1], 3)) #shape = (217088, 3)
for row in range(d.shape[0]):
for col in range(d.shape[1]):
world = registration.getPointXYZRGB(undistorted, registered, row, col)
out[row + col] = world[0:3]
out_col[row + col] = np.array(world[3:6]) / 255
"""
# Format undistorted data to real-world coordinates
out = np.zeros((d.shape[0]*d.shape[1], 3)) #shape = (217088, 3)
for row in range(d.shape[0]):
for col in range(d.shape[1]):
world = registration.getPointXYZ(undistorted, row, col)
out[row + col] = world
sp2.setData(pos=out, size=2)
listener.release(frames)
t = QtCore.QTimer()
t.timeout.connect(update)
t.start(50)
## Start Qt event loop unless running in interactive mode.
if __name__ == '__main__':
import sys
if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
QtGui.QApplication.instance().exec_()
device.stop()
device.close()
sys.exit(0)
実際の点群座標データを取得するために次に何をすべきかわかりません。
私が間違っていることについて何か提案はありますか？
私のオペレーティング システムは、Python 3.5 を使用した Ubuntu 16.0.4 です。
ありがとう。
