根据三维激光xyz坐标计算对应的角度与距离

2024年1月12日 68次阅读 来源: 儿时的窗 
clear;clc;
[file,path] = uigetfile('*.txt');
if isequal(file,0)
   disp('User selected Cancel');
else
   disp(['User selected ', fullfile(path,file)]);
end
sig=importdata( fullfile(path,file) );

x=sig(:,1);
y=sig(:,2);
z=sig(:,3);

n = length(x);
% d = zeros(n,1);
ju = zeros(n,1); %竖直角度
jd = zeros(n,1); %水平角度

d = sqrt(power(x,2)+power(y,2)+power(z,2)) ; %距离
PI = pi/180;

e=inputdlg({'Horizontal Angle range'}); %inputdlg({'a','b','c','d'})
bm=str2num(e{1});
% fprintf('%d \n', bm); 
% x = -d*sin(ju)*cos(jd);
% y =  d*sin(ju)*sin(jd);
% z =  d*cos(ju);
%% 水平角在0-180
if bm == 180
    for i=1:n
        %水平角0-180
        if y(i) > 0 %说明ju在0-180区间
            ju(i) = acosd(z(i)/d(i));%生成的值为角度值；
        else %说明ju在180-360区间
            ju(i) = 360 - acosd(z(i)/d(i));%生成的值为角度值；
        end
        jd(i) = acosd( x(i)/ ( d(i)*sin(ju(i)*PI)*(-1) ) ); %生成的值为角度值； 
        fprintf('%.3f %.3f %.3f\n', jd(i), ju(i) , d(i));       
    end
%% 水平角在180-360
elseif bm == 360
    for i=1:n
        if y(i) < 0 %说明ju在0-180区间
            ju(i) = acosd(z(i)/d(i));%生成的值为角度值；
        else %说明ju在180-360区间
        end
        jd(i) = 360 - acosd( x(i)/ ( d(i)*sin(ju(i)*PI)*(-1) ) ); %生成的值为角度值；
        fprintf('%.3f %.3f %.3f\n', jd(i), ju(i) , d(i));    
    end
else
    fprintf('input error!!!\n');   
end
%%


% figure
% hold on
% subplot(2,1,1),plot(jd,'r.','MarkerSize',20),title('水平角度'),grid
% subplot(2,1,2),plot(ju,'g.','MarkerSize',20),title('竖直角度'),grid

%%

数据格式如下：

8.044360,-0.438343,0.692872
8.045611,-0.438360,0.778929
8.046889,-0.438377,0.866857
8.043083,-0.438326,0.605104
7.997507,-0.505654,0.693540
7.998758,-0.505670,0.779597
8.000037,-0.505687,0.867525
7.996230,-0.505637,0.605773
7.945226,-0.580763,0.694286
7.946477,-0.580779,0.780343
7.947755,-0.580796,0.868271
7.943949,-0.580746,0.606518

运行结果如下：

User selected F:\1.txt
3.119 274.916 8.086
3.119 275.522 8.095
3.118 276.139 8.105
3.119 274.296 8.078
3.618 274.946 8.043
3.617 275.556 8.053
3.617 276.177 8.063
3.618 274.324 8.035
4.181 274.981 7.997
4.180 275.594 8.006
4.180 276.218 8.016
4.181 274.354 7.988

    原文作者：儿时的窗
    原文地址: https://blog.csdn.net/qq_26786109/article/details/111943043
    本文转自网络文章，转载此文章仅为分享知识，如有侵权，请联系博主进行删除。
点赞