0

我正在尝试绘制从 Garmin LiDAR lite V3 获得的 3D 点云图。matlab 脚本正在运行，数据变量正在从激光雷达收集 [x, y, z] 点云球坐标，但 3D 绘图不起作用。我正在使用 plot3() 函数来绘制点云。

a = arduino(); %Make sure you have 'Servo' and 'I2C' Libraries included.
s1 = servo(a, 'D3');%Creates a servo object on pin D3.
s2 = servo(a, 'D5');%Creates a servo object on pin D5.
garmin = device(a, 'I2CAddress', '0x62')%Connects to the lidar device.

%Zero the servo position.
writePosition(s1, 0);
writePosition(s2, 0);

%Allow time for the servos to move.
pause(1);

%servoAngleT is for the "tilt" servo (up/down).
%servoAngleP is for the "pan" servo (left/right).
%The servos have a range of 0 to 1, Right-Left, Down-Up.

jj = 1;
ij = 1;
servoAngleT = 0;
clear data
%"data" is the matrix where two angles and the distance value will be
%stored each loop.

while servoAngleT <= 1
    
    %Move the tilt servo up and reset the pan servo.
    writePosition(s2, servoAngleT);
    pause(0.2)
    servoAngleP = 0;
    writePosition(s1, servoAngleP)
    pause(2)
    ii = 1;

    while servoAngleP <= 1
        
        %Move the pan servo to the left.
        writePosition(s1, servoAngleP);
        
        writeRegister(garmin, 0, 4);
        
        while true
        one = readRegister(garmin, '0x01');
        if mod(one,2) == 0; break; end
        end
        
        
        DL = readRegister(garmin, '0x10', 'uint8');%Low byte.
        DH = readRegister(garmin, '0x0f', 'uint8');%High byte.
        %Combine into int16.
        DC = uint8([DH DL]);
        DC = uint16(DC);
        D = typecast(bitor(bitshift(DC(1), 8), DC(2)), 'int16');
        
        %Storing distance and servo position in a matrix.
        data(1, ij) = servoAngleP;
        data(2, ij) = servoAngleT;
        data(3, ij) = D;
        
        ii = ii + 1;
        ij = ij + 1;
        servoAngleP = servoAngleP + 0.01;%Specifies pan resolution.
    end
    servoAngleT = servoAngleT + 0.01;%Specifies tilt resolution.
end
    
%Moving the servos to the center when scan is complete.
writePosition(s1, 0.5);
writePosition(s2, 0.5);

%Extracting 3 seperate lines of data from the matrix.
theta = data(1, :);
phi = data(2, :);
radius = data(3, :);

%Using matlabs built in 'pi' is less precise, so I defined one with more
%decimal places.
pii = 3.1415926535;

%Converting servo values into radians.
phi = phi * pii;
theta = theta * pii;

%Converting from spherical coordinates into cartesian coordinates for
%plotting on a 3d graph.
x = radius .* cos(theta) .* sin(phi);
y = radius .* sin(theta) .* sin(phi);
z = radius .* cos(phi);

%creating a point cloud.
plot3(x, y, z, 'r.')

%Making the axes all the same ensures that the data won't appear
axis([-600 600 -600 600 -600 600])
hold on
%plotting a marker at 0, 0, 0 to show the center.
plot3(0, 0, 0, 'dg')
4

0 回答 0