# Curb Detection in 3-D Lidar Point Cloud

This example shows how to detect curbs in lidar point clouds. A curb is a line of stone or concrete, that connects the roadway to the sidewalk. Curbs act as delimiters for the drivable area of the road. In the absence of GPS signals, detecting the curb position is important for path planning and safe navigation in autonomous driving applications.

This example uses a subset of the PandaSet data set from Hesai and Scale. The PandaSet contains lidar point cloud scans of various city scenes captured using the Pandar64 sensor.

### Introduction

Curb detection in lidar point cloud involves the detecting the left and right curbs of a road with respect to the ego vehicle. The lidar sensor is mounted on the top of the vehicle.

Use these steps to detect curbs in the point cloud data captured by the lidar sensor:

1. Extract a region of interest(ROI)

4. Detect candidate curb points using the on-road points

The size of the data set is 5.2 GB, and it contains 2560 preprocessed organized point clouds. Out of these, this example uses a subset of 27 point clouds. Each point cloud is specified as a 64-by-1856-by-3 array. The ground truth data contains the semantic segmentation labels for 13 classes. The point clouds are stored in PCD format, and the ground truth data is stored in PNG format.

% Set random seed to generate reproducible results
rng("default");

Select the first frame of the data set for further processing. Visualize the point cloud.

ptCloud = ptCloudData{1};
labels = labelsData{1};

% Visualize the input point cloud with corresponding ground truth labels
figure
pcshow(ptCloud.Location,labels)
view(2)
title("Input Lidar Point Cloud")

### Preprocess Data

As a preprocessing step for detecting curb points, first extract a region of interest from the point cloud and classify the points within it as on-road or off-road points.

Based on the installed position of the lidar sensor, the point cloud data is sparse beyond a certain distance. To ensure the point cloud you extract is dense enough for further processing, specify an ROI within a limited distance of the sensor.

% Define ROI in meters
xLim = [-35 20];
yLim = [-25 25];

roiIdx = ptCloud.Location(:,:,1) >= xLim(1) & ...
ptCloud.Location(:,:,1) <= xLim(2) & ...
ptCloud.Location(:,:,2) >= yLim(1) & ...
ptCloud.Location(:,:,2) <= yLim(2);

Classify the point cloud into on-road and off-road points using these labels:

• 1 — Unlabeled

• 2 — Vegetation

• 3 — Ground

• 6 —Side Walk

• 7 — Car

• 8 — Truck

• 9 — Other Vehicle

• 10 — Pedestrian

• 12 — Signs

• 13 — Buildings

labels(~roiIdx) = nan;
onRoadLabelsIdx = (labels == 3 | labels == 4 | labels == 6);

% The off-road points consist of buildings and other objects
offRoadLabelsIdx = (labels == 2 | labels == 11 | ...
labels == 12 | labels == 13);

figure
subplot(1,3,1)
pcshow(select(ptCloud,roiIdx))
view(2)
title("Cropped Input Point Cloud")

subplot(1,3,2)
view(2)

subplot(1,3,3)
view(2)

If semantic labels are not available with your data set, you can compute them using a deep learning network. For more information, see the Lidar Point Cloud Semantic Segmentation Using SqueezeSegV2 Deep Learning Network example.

1. Launch a sequence of beams from a launching point. In this example, the launching point is the location of the lidar sensor on the ego vehicle.

2. Divide the beams into beam zones according to the angular resolution of the sensor. In this example, the angular resolution is 1 degree.

3. Determine the beam angles and beam lengths with respect to the launching point.

4. For each beam zone, determine the distance between the launching point and closest point in that zone and the distance between the launching point and the farthest point in that zone.

5. Compute the normalized beam length of a beam zone as the ratio of the shortest and longest distance between the launching point and a point in that zone.

6. Apply a customized version of the toe-finding algorithm [3] to the normalized beam lengths to find the road shape.

7. The center angle of each toe slice is the road segmentation angle with respect to the launching point.

% Select the off-road point cloud

% Use the helperRoadSegmentationAngles helper function defined at the end
% of this example to compute the road segmentation angles

% Visualize the off-road points and the toe slices indicating the road shape
figure
subplot(1,2,1)
view(2)
subplot(1,2,2)
polarplot(dk)

Extract the feature curb points from the on-road points. Process each scan line of the on-road point cloud individually to extract the points satisfying the threshold on these features:

• Smoothness Feature [2] — This feature checks for the smoothness of the area around a point. A higher smoothness value indicates that the point is an edge point, and a lower values indicates that the point is a plane point. Use the points with smoothness value greater than the specified smoothness threshold for further computation.

• Height Difference Feature [2] — This feature checks for the standard deviation and the maximum difference of height around a point. Use the points with these values within the specified bounds for further computation.

• Horizontal and Vertical Continuity Feature [1] — This feature checks for the horizontal and vertical continuity of a point with respect to its immediate neighboring points. Use the points with horizontal and vertical distances greater than the specified threshold for further computation.

% Set up the parameters for detecting the curbs
numScanLines = 64;
numNeighborsOnEachSide = 5;

% Define the threshold for the smoothness feature
smoothnessThreshold = 0.001;

% Define the thresholds for the height difference feature (in meters)
heightThreshold.heightMin = 0.02;
heightThreshold.heightMax = 0.25;
heightThreshold.heightSDMin = 0.02;
heightThreshold.heightSDMax = 0.07;

% Define the threshold for the horizontal and vertical continuity feature
% (in degrees)
angleResolutionThreshold = 0.24;

ptInfluence = -2*numNeighborsOnEachSide;

Apply the height difference, smoothness, and horizontal and vertical continuity feature thresholds to the on-road points.

% Use the helperDetectFeatureCurbs helper function defined at the
% end of this example to detect the feature curb points
[featureCurbPoints,validSmoothnessIndices,validHeightDifferenceIndices, ...
validHorizontalVerticalContinuityIndices] = helperDetectFeatureCurbs( ...
ptInfluence,smoothnessThreshold,heightThreshold,angleResolutionThreshold);

Visualize the detected features.

figure
subplot(2,2,1)
validSmoothnessIndices))
view(2)
title("Smoothness Feature")

subplot(2,2,2)
validHeightDifferenceIndices))
view(2)
title("Height Difference Feature")

subplot(2,2,3)
validHorizontalVerticalContinuityIndices))
view(2)
title("Horizontal and Vertical Continuity Feature")

subplot(2,2,4)
pcshow(featureCurbPoints)
view(2)
title("Feature Curb Points")

Next, compute the candidate curb points from the feature curb points:

1. Classify the feature curb points into different segments based on the road segmentation angles.

2. In each segment, define the points closest to the road segmentation lines as seed points. These seed points can be noisy and contain false positive points.

3. To improve the results, and to compute the candidate curb points, parabolically model the seed curbs in each segment, and use the RANSAC algorithm to remove the false positives that do not match the parabolic model.

% Define the threshold for RANSAC based filtering
maxDistance = 0.1;

% Define the segment size to find the seed curb points along the road
% segmentation lines
stepSegment = 0.1;

% Define the length of the road segmentation lines to search for the seed
% curb points

% Use the helperDetectCandidateCurbs helper function, attached to this
% example as a supporting file, to compute the candidate curb points from
% the feature curb points
featureCurbPoints,stepSegment,maxDistance);

% Visualize the point cloud overlaid with the candidate curb points
figure
pcshow(select(ptCloud,roiIdx).Location,"w")
view(2)
hold on
pcshow(candidateCurbPoints,"r",MarkerSize=200)
view(2)
hold off
title("Candidate Curb Points Detected")

### Track Curb Points

Loop through and process the lidar data to extract and track the candidate curb points. Tracking curb points improves the robustness of curb detection. You can halt the curb tracking at the segmented roads, and restart it when the ego vehicle leaves the segmented roads. Curb tracking involves polynomial fitting on the XY-points using a 2-degree polynomial represented as $\mathit{y}=\mathit{a}{\mathit{x}}^{2}+\mathit{bx}+\mathit{c}$, where a, b, and c are the polynomial parameters. Curb tracking is a two-step process:

1. Track curb polynomial parameters a and b to control the curvature of the polynomial.

2. Track curb polynomial parameter c to control the drift of the polynomial.

Update these parameters by using a Kalman filter with a constant acceleration motion model. Use the configureKalmanFilter function to initialize a Kalman filter.

player = pcplayer([-35 20],[-25 25],[-10 10],MarkerSize=6);

% Initial values for Kalman filter
initParams.initialEstimateError = [1 1 1]*1e-1;
initParams.motionNoise = [1 1 1]*1e-7;
initParams.measurementNoise = 10;

leftTracker.drift = [];
leftTracker.filteredDrift = [];
leftTracker.curveSmoothness = [];
leftTracker.filteredCurveSmoothness = [];

rightTracker.drift = [];
rightTracker.filteredDrift = [];
rightTracker.curveSmoothness = [];
rightTracker.filteredCurveSmoothness = [];

isTracking = false;
isLeftCurbTrackingInitialised = false;
isRightCurbTrackingInitialised = false;
isPrevAngleSet = false;

for fileIdx = 1:numel(ptCloudData)
% Load the point cloud and it's ground truth labels
ptCloud = ptCloudData{fileIdx};
labels = labelsData{fileIdx};

% Find the points lying within ROI
xLim = [-35 20];
yLim = [-25 25];
roiIdx = ptCloud.Location(:,:,1) >= xLim(1) & ...
ptCloud.Location(:,:,1) <= xLim(2) & ...
ptCloud.Location(:,:,2) >= yLim(1) & ...
ptCloud.Location(:,:,2) <= yLim(2);

labels(~roiIdx) = nan;

onRoadLabelsIdx = (labels == 3| labels == 4 | labels == 6);
offRoadLabelsIdx = (labels == 2 | labels == 11 | labels == 12 | ...
labels == 13);

if ~isPrevAngleSet && numel(roadAngles) == 3
isPrevAngleSet = true;
end
end

% Compute number of road segments in front and behind the
% ego vehicle

% Check if curb tracking should be enabled or disabled
if ~isTracking
isTracking = true;
end
else
isTracking = false;
isLeftCurbTrackingInitialised = false;
isRightCurbTrackingInitialised = false;
isPrevAngleSet = false;
end
if numel(roadAngles) < 3 && isPrevAngleSet
end
end

% Compute the candidate curb points using on-road points
numScanLines,ptCloud,numNeighborsOnEachSide,ptInfluence, ...
smoothnessThreshold,heightThreshold,angleResolutionThreshold);

diff(featureCurbPoints.YLimits))/2;

if isempty(candidateCurbPoints)
continue;
end

% Divide the candidate curb points into front and rear curb points
% based on the ego vehicle position
frontCandidateCurbPoints = candidateCurbPoints( ...
candidateCurbPoints(:,1) >= 0,:);
rearCandidateCurbPoints = candidateCurbPoints( ...
candidateCurbPoints(:,1) < 0,:);

xVal = linspace(min(frontCandidateCurbPoints(:,1)),xLim(2),80);
if isTracking
leftCurb = [];
rightCurb = [];

[0 0 1]);
modelParams = model.Parameters;

candidateAngles = atan2d(frontCandidateCurbPoints(:,2), ...
frontCandidateCurbPoints(:,1));
candidateAngles(candidateAngles < 0) = candidateAngles( ...
candidateAngles < 0) + 360;

% Divide the candidate curb points, into left and right curb points
idx = candidateAngles >= roadAngles(1) & ...
else
idx = candidateAngles >= roadAngles(2) | ...
end
leftCandidateCurbPoints = frontCandidateCurbPoints(idx,:);
rightCandidateCurbPoints = frontCandidateCurbPoints(~idx,:);

% Compute polynomials to track left and right curbs
leftPolynomial = fitPolynomialRANSAC( ...
leftCandidateCurbPoints(:,1:2),2,0.1);
rightPolynomial = fitPolynomialRANSAC( ...
rightCandidateCurbPoints(:,1:2),2,0.1);

if ~isempty(leftPolynomial)
if ~isLeftCurbTrackingInitialised
[leftFilter,leftCurb] = helperTrackCandidateCurbs( ...
leftPolynomial,xVal,modelParams,initParams);
isLeftCurbTrackingInitialised = true;
else
[leftPolynomial,leftCurb,leftTracker] = helperTrackCandidateCurbs( ...
leftPolynomial,xVal,modelParams, ...
leftFilter,leftTracker);
end
end
if ~isempty(rightPolynomial)
if ~isRightCurbTrackingInitialised
[rightFilter,rightCurb] = helperTrackCandidateCurbs( ...
rightPolynomial,xVal,modelParams,initParams);
isRightCurbTrackingInitialised = true;
else
[rightPolynomial,rightCurb,rightTracker] = helperTrackCandidateCurbs( ...
rightPolynomial,xVal,modelParams, ...
rightFilter,rightTracker);
end
end
ptCloud = select(ptCloud,roiIdx);

% Use the helperVisualiseOutput helper function, attached to this
% example as a supporting file to visualise the final output
outputPtCloud = helperVisualiseOutput(ptCloud,leftCurb,rightCurb, ...
rearCandidateCurbPoints);
else
ptCloud = select(ptCloud,roiIdx);

% Use the helperVisualiseOutput helper function, attached to this
% example as a supporting file to visualise the final output
outputPtCloud = helperVisualiseOutput(ptCloud,candidateCurbPoints);
end
if isOpen(player)
view(player,outputPtCloud);
view(player.Axes,[0 90])
camva(player.Axes,0)
end
end

### Results

To analyze the curb detection results, compare them against the tracked curb polynomials by plotting them in figures. Each plot compares the parameters with and without the Kalman filter. The first figure compares the drift of curbs along the y-axis, and the second figure compares the smoothness of the curb polynomials. Smoothness is the rate of change of the slope of the curb curve.

figure
ax = subplot(2,1,1);
plot(leftTracker.drift)
hold on
plot(leftTracker.filteredDrift)
hold off
title("Left Curb Drift Along Y-axis")
ax.Position(2) = ax.Position(2) - 0.08;
ax = subplot(2,1,2);
plot(rightTracker.drift)
hold on
plot(rightTracker.filteredDrift)
hold off
title("Right Curb Drift Along Y-axis")
ax.Position(2) = ax.Position(2) - 0.06;
Lgnd = legend("Drift Values","Filtered Drift Values", ...
"Orientation","Horizontal");
Lgnd.Position(1:2) = [0.29 0.9];

figure
ax = subplot(2,1,1);
plot(leftTracker.curveSmoothness);
hold on
plot(leftTracker.filteredCurveSmoothness)
hold off
title("Left Curve Smoothness")
ax.Position(2) = ax.Position(2) - 0.08;
ax = subplot(2,1,2);
plot(rightTracker.curveSmoothness);
hold on
plot(rightTracker.filteredCurveSmoothness)
hold off
title("Right Curve Smoothness")
ax.Position(2) = ax.Position(2) - 0.06;
Lgnd = legend("Curve Smoothness","Filtered Curve Smoothness", ...
"Orientation","Horizontal");
Lgnd.Position(1:2) = [0.23 0.9];

### Helper Functions

"data/Pandaset_LidarData.tar.gz");
filepath = fileparts(lidarDataTarFile);
outputFolder = fullfile(filepath,"Pandaset");

if (~exist(fullfile(outputFolder,"Lidar"),"file")) ...
&&(~exist(fullfile(outputFolder,"semanticLabels"),"file"))
untar(lidarDataTarFile,outputFolder);
end

lidarData =  fullfile(outputFolder,"Lidar");
labelsFolder = fullfile(outputFolder,"semanticLabels");

ptCloudData = cell(27,1);
labelsData = cell(27,1);
count = 1;
for fileIdx = 1041:1067
% The example follows the convention that ego vehicle motion is along
% the positive x-axis, hence, transform the point cloud
theta = -90;
rot = [cosd(theta) sind(theta) 0; ...
-sind(theta) cosd(theta) 0; ...
0            0       1];
trans = [0 0 0];
tform = rigid3d(rot,trans);
ptCloud = pctransform(ptCloud,tform);
ptCloudData{count} = ptCloud;
count = count + 1;
end
end

% Compute the angles of all the points in the off-road point cloud with
% respect to the launching point (origin)

minBeamLength = ones(360,1);
maxBeamLength = ones(360,1);
for k = 1:360
% Use the points in the kth zone to compute the normalized beam length
zonePtsIdx = idx >= (k-1) & idx < k;
if any(zonePtsIdx)
% Shortest distance between the zone points and the launching point
minBeamLength(k) = min(hypot( ...

% Longest distance between the zone points and the launching point
maxBeamLength(k) = max(hypot( ...
end
end
% Compute the normalized beam length
dk = minBeamLength./maxBeamLength;
dk(dk < 0.25) = 0;
dk(dk < mean(dk)) = 0;

peaksStart = find(diff(dk == 1) == 1) + 1;
peaksEnd = find(diff(dk == 1) == -1);

% Find indices of peaks to be merged
idx = (peaksStart - peaksEnd) < 15;
peaksStart(idx) = [];
peaksEnd(idx) = [];
if isempty(peaksStart) || isempty(peaksEnd)
angles = [];
else
if peaksStart(1) ~= 1
peaksStart = [1; peaksStart];
end
if peaksEnd(end) ~= 360
peaksEnd = [peaksEnd; 360];
end
peaks = [peaksStart peaksEnd];

% Remove the outliers in peaks computation
peaks(diff(peaks,[],2) <= 3,:) = [];

% Compute the road segmentation angles from the peaks
angles = mean(peaks,2);
if abs(angles(1) + angles(end) - 360) > 0 && ...
abs(angles(1) + angles(end) - 360) < 20
angles(1) = angles(1) + angles(end);
if angles(1) > 360
angles(1) = angles(1) - 360;
end
angles(end) = [];
end

% Update the peaks information
dk = zeros(360,1);
for i = 1:size(peaks,1)
dk(peaks(i,1):peaks(i,2)) = 1;
end
end
end

The helperCheckSmoothness helper function returns the indices of points satisfying the smoothnessThreshold condition.

function isSmoothnessValid = helperCheckSmoothness(neighborPtCloud, ...
currentPt,ptInfluence,numNeighborsOnEachSide,smoothnessThreshold)
isSmoothnessValid = false(size(neighborPtCloud,1),1);

diffX = ptInfluence*currentPt(:,1) + sum(neighborPtCloud(:,1,:),3);
diffY = ptInfluence*currentPt(:,2) + sum(neighborPtCloud(:,2,:),3);
diffZ = ptInfluence*currentPt(:,3) + sum(neighborPtCloud(:,3,:),3);
numerator = vecnorm([diffX diffY diffZ]')';
denominator = numNeighborsOnEachSide*vecnorm(currentPt')';

isSmoothnessValid(numerator./denominator > smoothnessThreshold) = true;
end

The helperCheckHeightDifference helper function returns the indices of points satisfying the heightThreshold conditions.

function isHeightDifferenceValid = helperCheckHeightDifference( ...
neighborPtCloud,heightThreshold)
isHeightDifferenceValid = false(size(neighborPtCloud,1),1);

heightDifference = max(squeeze(neighborPtCloud(:,3,:)),[],2) - ...
min(squeeze(neighborPtCloud(:,3,:)),[],2);
heightStdDev = std(squeeze(neighborPtCloud(:,3,:)),0,2);

isHeightDifferenceValid( ...
heightStdDev > heightThreshold.heightSDMin & ...
heightStdDev <= heightThreshold.heightSDMax & ...
heightDifference >= heightThreshold.heightMin & ...
heightDifference <= heightThreshold.heightMax) = true;
end

The helperCheckHorizontalVerticalContinuity helper function returns the indices of points satisfying the horizontalContinuityThreshold and verticalContinuityThreshold conditions.

function isHorizontalVerticalContinuityValid = helperCheckHorizontalVerticalContinuity( ...
leftPt,currentPt,rightPt,angleResolutionThreshold)
isHorizontalVerticalContinuityValid = false(size(currentPt,1),1);

verticalAngle = atan2d(currentPt(:,3),hypot(currentPt(:,1), ...
currentPt(:,2)));
horizontalContinuityThreshold = hypot(currentPt(:,1),currentPt(:,2))* ...
angleResolutionThreshold*(pi/180);
verticalContinuityThreshold = horizontalContinuityThreshold.* ...
sind(verticalAngle);

distanceLeftHorizontal = sqrt(sum((currentPt(:,1:2) - leftPt(:,1:2)).^2,2));
distanceRightHorizontal = sqrt(sum((currentPt(:,1:2) - rightPt(:,1:2)).^2,2));

distanceLeftVertical = abs(currentPt(:,3) - leftPt(:,3));
distanceRightVertical = abs(currentPt(:,3) - rightPt(:,3));

isHorizontalVerticalContinuityValid(distanceLeftHorizontal >= ...
horizontalContinuityThreshold & ...
distanceRightHorizontal >= horizontalContinuityThreshold & ...
distanceLeftVertical >= verticalContinuityThreshold & ...
distanceRightVertical >= verticalContinuityThreshold) = true;
end

The helperDetectFeatureCurbs helper function detects the feature curb points from the on-road points.

function [featureCurbPoints,validSmoothnessIndices, ...
validHeightDifferenceIndices, ...
validHorizontalVerticalContinuityIndices] = helperDetectFeatureCurbs( ...
ptInfluence,smoothnessThreshold,heightThreshold,angleResolutionThreshold)

% Declare and initialise the variables to store the indices of the points
% satisfying the feature thresholds

for i = 1:numScanLines
% Extract the points lying in a scan line
scanLine = squeeze(ptCloud.Location(i,:,:));

% Extract the on-road points from the scan line points

if size(onRoadPtsInScanLine,1) >= 2*numNeighborsOnEachSide + 1

numNeighborsOnEachSide);
numNeighborsOnEachSide);

for j = numNeighborsOnEachSide+1: ...
% Select the left neighbor points of each point
leftPoints(j-numNeighborsOnEachSide,:,:) = reshape( ...
1,3,numNeighborsOnEachSide);

% Select the right neighbor points of each point
rightPoints(j-numNeighborsOnEachSide,:,:) = reshape( ...
1,3,numNeighborsOnEachSide);
end
leftPoints(isnan(leftPoints(:,1,1)),:,:) = [];
rightPoints(isnan(rightPoints(:,1,1)),:,:) = [];

% Use the helperCheckSmoothness helper function to find the points
% satisfying the smoothness feature
validSmoothnessIndicesInScanLine = helperCheckSmoothness( ...
ptInfluence,numNeighborsOnEachSide, ...
smoothnessThreshold).*validPoints';

validSmoothnessIndices(i, ...
validSmoothnessIndicesInScanLine( ...
validSmoothnessIndicesInScanLine ~= 0)) = true;

% Use the helperCheckHeightDifference helper function to find the
% points satisfying the height difference feature
validHeightDifferenceIndicesInScanLine = helperCheckHeightDifference( ...
cat(3,leftPoints,rightPoints),heightThreshold).*validPoints';

validHeightDifferenceIndices(i, ...
validHeightDifferenceIndicesInScanLine( ...
validHeightDifferenceIndicesInScanLine ~= 0)) = true;

% Use the helperCheckHorizontalVerticalContinuity helper function
% to find the points satisfying the horizontal and vertical
% continuity feature
validHorizontalVerticalContinuityIndicesInScanLine = ...
helperCheckHorizontalVerticalContinuity( ...
rightPoints(:,:,1),angleResolutionThreshold).*validPoints';

validHorizontalVerticalContinuityIndices(i, ...
validHorizontalVerticalContinuityIndicesInScanLine( ...
validHorizontalVerticalContinuityIndicesInScanLine ~= 0)) = true;
end
end
validHorizontalVerticalContinuityIndices & ...
validSmoothnessIndices & ...
validHeightDifferenceIndices);
featureCurbPoints = pcdenoise(featureCurbPoints);
end

The helperTrackCandidateCurbs helper function tracks the candidate curb points using the Kalman filter.

function varargout = helperTrackCandidateCurbs(varargin)
polynomial = varargin{1};
xVal = varargin{2};
modelParams = varargin{3};
if numel(varargin) == 4
initParams = varargin{4};
curveInitialParameters = polynomial(1:2);
driftInitialParameters = polynomial(3);

% Configure Kalman filter
filter.curveFilter = configureKalmanFilter( ...
"ConstantAcceleration",curveInitialParameters, ...
initParams.initialEstimateError, ...
initParams.motionNoise, ...
initParams.measurementNoise);
filter.driftFilter = configureKalmanFilter( ...
"ConstantAcceleration",driftInitialParameters, ...
initParams.initialEstimateError, ...
initParams.motionNoise, ...
initParams.measurementNoise);

varargout{1} = filter;
else
filter = varargin{4};
tracker = varargin{5};

predict(filter.curveFilter);
predict(filter.driftFilter);

tracker.drift = [tracker.drift;polynomial(3)];
tracker.curveSmoothness = [tracker.curveSmoothness;
polynomial(1)];

% Correct polynomial using Kalman filter
updatedCurveParams = correct(filter.curveFilter, ...
polynomial(1:2));
updatedDriftParams = correct(filter.driftFilter, ...
polynomial(3));

% Update the polynomial
polynomial = [updatedCurveParams,updatedDriftParams];
tracker.filteredDrift = [tracker.filteredDrift;polynomial(3)];
tracker.filteredCurveSmoothness = [tracker.filteredCurveSmoothness;
polynomial(1)];
varargout{1} = polynomial;
varargout{3} = tracker;
end
% Y-coordinate estimation
yVal = polyval(polynomial,xVal);

% Z-coordinate estimation
zVal = (-modelParams(1)*xVal - ...
modelParams(2)*yVal - modelParams(4))/modelParams(3);

% Final Curb
curb = [xVal' yVal' zVal'];
varargout{2} = curb;
end

### References

[1]Zhang, Yihuan, Jun Wang, Xiaonian Wang, and John M. Dolan. "Road-Segmentation-Based Curb Detection Method for Self-Driving via a 3D-LiDAR Sensor." IEEE Transactions on Intelligent Transportation Systems 19, no. 12 (December 2018): 3981–91. https://doi.org/10.1109/TITS.2018.2789462.

[2] Wang, Guojun, Jian Wu, Rui He, and Bin Tian. "Speed and Accuracy Tradeoff for LiDAR Data Based Road Boundary Detection." IEEE/CAA Journal of Automatica Sinica 8, no. 6 (June 2021): 1210–20. https://doi.org/10.1109/JAS.2020.1003414.

[3] Hu, Jiuxiang, Anshuman Razdan, John C. Femiani, Ming Cui, and Peter Wonka. "Road Network Extraction and Intersection Detection From Aerial Images by Tracking Road Footprints." IEEE Transactions on Geoscience and Remote Sensing 45, no. 12 (December 2007): 4144–57. https://doi.org/10.1109/TGRS.2007.906107.