Kinect 사용하기 2

매트랩에서 키넥트를 사용해서 Depth image를 받아온 뒤에 이를 레이져 range finder sensor와 같이 사용하는 방법에 대해서 알아보자. 

간단데스텝

1. 뎁스 이미지를 받아온다. 

2. 원하는 영역 (초록색 수평 선 사이)에서 각 column에 대해서 최소 값을 가져온다. 

3. 이 최소값을 삼차원 공간 속의 point cloud로 바꾼다.

4. 너무 멀리 있는 값은 신경쓰지 않는다. 

아래 동영상과 같은 일을 할 수 있다. 

 - 맨위의 그림은 뎁스 이미지

 - 중간의 그림은 위에서 바라보았을 때 그림

 - 맨아래 그림은 카메라 시각에서 바라보았을 때 그림

코드

메인 (이걸 실행하면 된다.)

%%

clc; clear; close all;

imaqreset;

depthVid = videoinput('kinect', 2);

triggerconfig(depthVid, 'manual');

depthVid.FramesPerTrigger = 1;

depthVid.TriggerRepeat = inf;

% set( getselectedsource(depthVid), 'TrackingMode', 'Skeleton');

set( getselectedsource(depthVid));

fprintf('Kinect Initialize Done. \n');

start(depthVid);

fprintf('Start Kinect. \n');

kinect_period_sec = 0.15;

kinect_prev_sec = 0;

global key_pressed; key_pressed = '';

fig_depth = figure(1);

set(fig_depth, 'Position', [400 200 600 900], 'KeyPressFcn', @keyDownListener);

init_clock = clock;

while 1

    curr_sec = etime(clock, init_clock);

    if curr_sec - kinect_prev_sec > kinect_period_sec

        kinect_prev_sec = kinect_prev_sec + kinect_period_sec;

        

        tic;

        % kinect_period_sec 초에 한번 씩 들어온다.

        depthImage = getsnapshot(depthVid);

        depthImage = fliplr(depthImage);

        % resize를 한다. (96 * 128) for speed up

        % depthImage_resize = imresize(depthImage, [96 128], 'box');

        depthImage_resize = depthImage;

        

        % min을 구한다.

        depthImage_resize_temp = depthImage_resize;

        depthImage_resize_temp(~depthImage_resize_temp) = 4096;

        hmin = 140;

        hmax = 340;

        [min_depth_array, min_idx] = min(depthImage_resize_temp(hmin:hmax, :));

        min_depth_val = min(min_depth_array);

        min_idx_nz = find(min_idx ~= 1 & min_depth_array < 4000);

        % Depth image화면에서 최소점을 찾는다. 

        min_x_idx_nz = min_idx_nz;

        min_y_idx_nz = min_idx(min_idx_nz)+hmin;

        

        % xyz를 구한다.

        max_dist_tr = 4000;

        xyzs = get_xyz_kinect(depthImage_resize_temp, [min_x_idx_nz' min_y_idx_nz']);

        exc_idx = find(xyzs(:, 3) > max_dist_tr);

        xyzs(exc_idx, :) = [];

        etime_ms = toc;

        

        % 그림을 그린다.

        % figure(1);

        clf;

        subplot(5, 1, 1:2);

        hold on;

        imshow(depthImage_resize_temp, [0 4096], 'InitialMagnification', 500); colorbar;

        plot(min_x_idx_nz, min_y_idx_nz, 'bo');

        plot(1:640, hmin*ones(1, 640), 'g-');

        plot(1:640, hmax*ones(1, 640), 'g-');

        hold off; axis([1 size(depthImage_resize_temp, 2) 1 size(depthImage_resize_temp, 1)]);

        title(sprintf('%.2f sec mindepth: %.1f (%.1f msec)', curr_sec, min_depth_val, etime_ms*1000), 'FontSize', 15);

        subplot(5, 1, 3:4);

        plot3(xyzs(:, 1), xyzs(:, 2), xyzs(:, 3), 'ro');

        fov3 = [0 0 0 ; 2500 0 5000; -2500 0 4000];

        xlabel('X [mm]'); ylabel('Y [mm]'); zlabel('Depth Z [mm]');

        axis equal; axis([-3000 3000 -600 600 0 5000]); grid on; view(0, 0);

        title('Birds eye view', 'FontSize', 13);

        subplot(5, 1, 5);

        plot3(xyzs(:, 1), xyzs(:, 2), xyzs(:, 3), 'ro');

        xlabel('X [mm]'); ylabel('Y [mm]'); zlabel('Depth Z [mm]');

        axis equal; axis([-3000 3000 -600 600 0 5000]); grid on; view(0, -80);

        title('Camera view', 'FontSize', 13);

        drawnow;

        

        

        switch key_pressed

            case 'q'

                key_pressed = '';

                break;

        end

    end

end

% 키넥트를 종료

stop(depthVid);

title('Stoped', 'FontSize', 13);

fprintf('Stop Kinect. \n');

%%

stop(depthVid);

함수1 (뎁스 이미지를 삼차원 공간 속의 점으로 변환)

function xyz = get_xyz_kinect( depth_image, idxs )

nr_data  = size(idxs, 1); 

nr_r = size(depth_image, 1);

idx_list = (nr_r-idxs(:, 1)+1) + (idxs(:, 2)-1)*nr_r;

fx = 525.0;  % focal length x

fy = 525.0;  % focal length y

cx = 319.5;  % optical center x

cy = 239.5;  % optical center y

% factor = 5000; % for the 16-bit PNG files

factor = 1; % for the 32-bit float images in the ROS bag files

u_list = idxs(:, 1);

v_list = idxs(:, 2);

% if u > 640, u = 640; end;

% if v > 480, v = 480; end;

% if u < 1, u = 1; end;

% if v < 1, v = 1; end;

% Z = double(depth_image(idx_list)) / factor;

Z = zeros(nr_data, 1);

for i = 1:nr_data

    Z(i) = double(depth_image(idxs(i, 2), idxs(i, 1))) / factor;

end

X = (u_list - cx*ones(nr_data, 1)) .* Z / fx;

Y = (v_list - cy*ones(nr_data, 1)) .* Z / fy;

xyz = [X Y Z]; 

함수2 (키보드 입력을 읽어오는 핸들러)

function keyDownListener(~, event)

global key_pressed;

key_pressed = event.Key;

% disp(key_pressed);