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

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);

[R]

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]

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]

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.

[Gavin Halford]

Thank you this is spot on my problem. Some adjustments to the parameters got everything working.