Question

我有一个三维地图或矩阵，我想从中构建点云。我已经使用此代码完成了这项工作：

function [pcloud, distance] = depthToCloud(depth, topleft)
% depthToCloud.m - Convert depth image into 3D point cloud
% Author: Liefeng Bo and Kevin Lai
%
% Input: 
% depth - the depth image
% topleft - the position of the top-left corner of depth in the original depth image. Assumes depth is uncropped if this is not provided
%
% Output:
% pcloud - the point cloud, where each channel is the x, y, and z euclidean coordinates respectively. Missing values are NaN.
% distance - euclidean distance from the sensor to each point
%

if nargin < 2
    topleft = [1 1];
end

depth= double(depth);
depth(depth == 0) = nan;

% RGB-D camera constants
center = [320 240];
[imh, imw] = size(depth);
constant = 570.3;
MM_PER_M = 1000;

% convert depth image to 3d point clouds
pcloud = zeros(imh,imw,3);
xgrid = ones(imh,1)*(1:imw) + (topleft(1)-1) - center(1);
ygrid = (1:imh)'*ones(1,imw) + (topleft(2)-1) - center(2);
pcloud(:,:,1) = xgrid.*depth/constant/MM_PER_M;
pcloud(:,:,2) = ygrid.*depth/constant/MM_PER_M;
pcloud(:,:,3) = depth/MM_PER_M;
%distance = sqrt(sum(pcloud.^2,3));

但我问的是否有更有效的方法？在构建点云之后，我想得到每个点的法线，并且我使用了内置的matlab函数surfnorm，但是它需要大量的处理时间。所以，如果有人能帮助我，那么这样做会更好，更有效。

将3d地图更改为点云matlab的有效方法

0 个答案: