Why is my 3D Vector Field Histogram plot not displaying correctly??

Question

Gavin Halford 2023-12-27

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2064536-why-is-my-3d-vector-field-histogram-plot-not-displaying-correctly

评论： Gavin Halford 2024-1-5

I am streaming in data from a Velodyne Puck Lidar and feeding it to the controllerVFH3D function. When i try to plot the histogram with show(vfh3D). The plot is completely filled in even outside the sensors field of vision so i cant see the UAV or steerring vectors as shown in the example.

This is the plot i get:

This is the example plot:

Here is my code:

clc, clear
% Initialize the Velodyne LiDAR
lidar = velodynelidar('VLP16'); 
%Set up the 3D VFH controller
vfh3D = controllerVFH3D('DistanceLimits', [0.2, 100], ...
                        'HorizontalSensorFOV', [-359, 360], ...
                        'VerticalSensorFOV', [-15, 15],...
                        'SensorOrientation', [-180, 0, 0], ...
                        'HistogramResolution', 10, ...
                        'WindowSize', 3, ...
                        'HistogramThreshold', 4, ...
                        'MaxAge', 0, ...
                        'TargetDirectionWeight',5);
% Create a figure outside the loop
fig = figure;
ax = axes(Parent=fig);
while true
    % Check if the figure is still open
    if ~isvalid(fig)
        break; % Exit the loop if the figure is closed
    end
    % Read data from the LiDAR sensor
    ptCloud = read(lidar);
    x = ptCloud.Location(:,:,1); % X coordinates
    y = ptCloud.Location(:,:,2); % Y coordinates
    z = ptCloud.Location(:,:,3); % Z coordinates
    %Get rid of NaN values
    validIndices = ~isnan(x) & ~isnan(y) & ~isnan(z);
    X = x(validIndices);
    Y = y(validIndices);
    Z = z(validIndices);
    sensorPoints = [X Y Z];
    
    % Update the VFH3D controller with new data
    uavPosition = [1; 0; 0];
    uavOrientation = [1; 0; 0; 0]; % Example values, replace with actual data
    targetPosition = [-1.5; 1; 0]; % Example values, replace with actual data
    % Process data with VFH3D
    [desiredDirection, desiredYaw, status] = vfh3D(uavPosition, ...
                                                   uavOrientation, ...
                                                   sensorPoints, ...
                                                   targetPosition)
  % Plot on the dedicated axes
    show(vfh3D, "Parent",ax);    
    axis equal;
    
end
%Disconnect the LiDAR
disconnectLidar(lidar);

2 个评论
显示无隐藏无

R 2024-1-3

Hi Gavin,

I am unble to reproduce the issue as I don't have access to a lidar sensor. Can you share the sensor data and the version of MATLAB you are using so that contributors can investigate the issue in detail?

Gavin Halford 2024-1-3

ptCloud.mat

Hi Raghvi,

Here is some point cloud data from my Lidar along with a simplified program to pass it to the 3DVFH, I am using R2023b:

%Set up the 3D VFH controller

vfh3D = controllerVFH3D('DistanceLimits', [0.1, 100], ...

'HorizontalSensorFOV', [-180 180], ...

'VerticalSensorFOV', [-20, 20],...

'SensorOrientation', [0, 0, 0], ...

'HistogramResolution', 5, ...

'WindowSize', 1, ...

'HistogramThreshold', 1, ...

'MaxAge', 0, ...

'TargetDirectionWeight',10);

% Create a figure outside the loop

fig = figure;

ax = axes(Parent=fig);

sensorPoints = reshape(ptCloud.Location, [], 3);

% Update the VFH3D controller with new data

uavPosition = [0; 0; 0];

uavOrientation = [1; 0; 0; 0];

targetPosition = [20; 0; 0];

% Process data with VFH3D

[desiredDirection, desiredYaw, status] = vfh3D(uavPosition, ...

uavOrientation, ...

sensorPoints, ...

targetPosition)

% Plot on the dedicated axes

show(vfh3D, "Parent",ax);

axis equal;

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Dhananjay Narayanachar 2024-1-5

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2064536-why-is-my-3d-vector-field-histogram-plot-not-displaying-correctly#answer_1384136

Hi Gavin,

Since the VehicleRadius + SafetyDistance is comparable to the UAV's distance to obstacles, the entire spherical region is considered as filled with obstacles. Please try to reduce the controllerVFH3D object's 'VehicleRadius' and 'SafetyDistance' propeties to a smaller value like 0.1.