我正在尝试绘制从 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')