无人驾驶之MATLAB无人驾驶工具箱学习(5)
3 ADST中的函数
constvel
匀速状态更新。
语法
updatedstate = constvel(state)
updatedstate = constvel(state,dt)说明
updatedstete = constvel(state)返回匀速卡尔曼滤波器运动模型的更新状态state，以1s为步长。updatedstete = constvel(state,dt)指定时间步长为dt。
例子
匀速运动状态更新
以1s为时间间隔更新2维匀速运动。
>> state = [1;1;2;1];
>> state = constvel(state)
state =
2
1
3
1以特定时间步长更新匀速匀速状态
以1s为时间间隔更新2维匀速运动。
>> state = [1;1;2;1];
>> state = constvel(state,1.5)
state =
2.5000
1.0000
3.5000
1.0000constveljac
匀速段运动的雅克比矩阵。
语法
jacobian = constveljac(state)
jacobian = constveljac(state,dt)cvmeas
匀速运动从测量方程。
语法
measurement = cvmeas(state)
measurement = cvmeas(state, frame)
measurement = cvmeas(state, frame, sensorpos)
measurement = cvmeas(state, frame, sensorpos, sensorvel)
measurement = cvmeas(state, frame, sensorpos, sensorvel, laxes)
measurement = cvmeas(state, measurementParameters)说明
第一种用法返回直角坐标系中匀速卡尔曼运动模型的测量。state参数为当前跟踪滤波器的状态。
第二组用法指定了测量坐标系，frame。
第三种用法给定了传感器的位置sensorpos。
第四种用法给定了传感器的速度sensorvel。
第五种用法给定了当前传感器的轴线姿态laxes。
第六中用法指定测量参数，measurementParameters。
例子
在直角坐标中从匀速运动物体创建测量
在2D匀速运动中定义一个物体的状态。状态为分别为2维的位置和速度。在直角坐标中的测量为：
>> state = [1;10;2;20];
>> measurement = cvmeas(state)
measurement =
1
2
0z分量的测量为0.
在球面坐标中从匀速物体创建跟踪
>> measurement = cvmeas(state, 'spherical')
measurement =
63.4349
0
2.2361
22.3607仰角（elevation）为0，距离变化率（range rate）为正。该结果表明物体正远离传感器。
在平移后的球面坐标系中从匀速物体创建测量
坐标系位于（20；40；0）m.
>> measurement = cvmeas(state, 'spherical',[20;40;0])
measurement =
-116.5651
0
42.4853
-22.3607距离变化率为负，说明物体正朝着传感器运动。
用测量参数（measurement parameter）从匀速匀速物体中创建测量
>> state2d = [1;10;2;20];
>> frame = 'spherical';
>> sensorpos = [20;40;0];
>> sensorvel = [0;5;0];
>> laxes = eye(3);
>> measurement = cvmeas(state2d, frame, sensorpos, sensorvel, laxes)
measurement =
-116.5651
0
42.4853
-17.8885将测量参数放到结构体中，采用替换语法：
>> measparm = struct('Frame',frame,'OriginPosition',sensorpos,'OriginVelocity',sensorvel,'Orientation',laxes);
>> measurement = cvmeas(state2d, measparm)
measurement =
-116.5651
0
42.4853
-17.8885定义
方位角（Azimuth）和仰角（Elevation）的定义
定义在ADST中应用的方位角和仰角。
向量的方位角为该向量在xy平面上投影与x轴的夹角。从x轴到y轴方向为该角的正方向。方位角的范围为[-180°，180°]。仰角为该向量与其在xy平面上的投影的夹角。从xy平面向z轴正方向为该角的正方向。
configureDetectorMonoCamera
配置用标定过的单目相机进行物体检测。
语法
configuredDetector = configureDetectorMonoCamera(detector, sensor, objectSize)说明
配置ACF、Faster R-CNN或者 Fast R-CNN物体检测器来检测地面已知大小的物体。指定训练好的物体检测器detector，一个相机配置sensor用来将图像坐标系转换到世界坐标系下，以及检测物体的范围（objectSize）宽度和长度。
例子
用标定的单目相机检测车辆
用内置相机函数创建一个monoCamera类，并配置车辆检测器用来检测指定宽度的车辆。在视频上运行该检测器，并显示识别出的物体。
设置视频读入函数。
videoFile = fullfile(toolboxdir('driving'),'drivingdata','caltech_washington1.avi');
reader = vision.VideoFileReader(videoFile,'VideoOutputDataType','uint8');创建视频播放器：
videoPlayer = vision.VideoPlayer('Position',[29 579 643 386]);创建一个单目相机对象，该对象包含相机函数和相机位置。
>> videoPlayer = vision.VideoPlayer('Position',[29 579 643 386]);
>> focalLength = [309.4362 344.2161]; %[fx fy]
>> principlePoint = [318.9034 257.5352]; %[cx cy]
>> imaginSize = [480 640]; %[mrows ncols]
>> height = 2179.8/1000; % height in meters from the ground
>> pitch = 14; % pitch of the camera in degrees
>> intrinsics = cameraIntrinsics(focalLength, principlePoint, imaginSize);
>> sensor = monoCamera(intrinsics, height, 'Pitch', pitch);加载预训练的车辆检测器。
>> detector = vehicleDetectorACF;设置用于识别普通车辆尺寸的宽度为1.5~2.5m。
>> vehicleWith = [1.5 2.5];配置检测器。指定monoamera对象传感器类别，及定义的车辆宽度。
>> detector = configureDetectorMonoCamera(detector,sensor,vehicleWith);与设置的视频播放器一起在循环中运行检测器。插入包围盒和评分到视频中。
>> cont = ~isDone(reader);
while cont
I = reader();
% run the detector
[bboxes, scores] = detect(detector, I);
if ~isempty(bboxes)
I = insertObjectAnnotation(I, 'rectangle', bboxes, scores, 'Color','g');
end
videoPlayer(I)
% 如果视频播放器图片关闭，退出循环。
cont = ~isDone(reader) && isOpen(videoPlayer);
end
cvmeasjac
匀速运动测量函数的雅克比函数。
语法
measurementjac = cvmeasjac(state)
measurementjac = cvmeasjac(state, frame)
measurementjac = cvmeasjac(state, frame, sensorpos)
measurementjac = cvmeasjac(state, frame, sensorpos, sensorvel)
measurementjac = cvmeasjac(state, frame, sensorpos, sensorvel, laxes)
measurementjac = cvmeasjac(state, measurementParameters)说明
与cvmeas相对应。
constacc
匀加速运动模型。
语法
updatestate = constacc(state)
updatestate = constacc(state, dt)constaccjac
匀加速运动的雅克比矩阵。
语法
jacobian = constaccjac(state)
jacobian = constaccjac(state, dt)cameas
匀加速运动的测量函数。
语法
measurement = cameas(state)
measurement = cameas(state, frame)
measurement = cameas(state, frame, sensorpos)
measurement = cameas(state, frame, sensorpos, sensorvel)
measurement = cameas(state, frame, sensorpos, sensorvel, laxes)
measurement = cameas(state, measurementParameters)cameasjac
匀加速模运动测量函数的雅克比矩阵。
语法
measurementjac = cameasjac(state)
measurementjac = cameasjac(state, frame)
measurementjac = cameasjac(state, frame, sensorpos)
measurementjac = cameasjac(state, frame, sensorpos, sensorvel)
measurementjac = cameasjac(state, frame, sensorpos, sensorvel, laxes)
measurementjac = cameasjac(state, measurementParameters)constturn
恒定转弯速率运行模型。
语法
updatedstate = constturn(state)
updatedstate = constturn(state, dt)例子
匀速转弯速率运动状态更新
定义一个2维恒定转弯速率运动初始状态。转弯速率为12°/s。更新该状态到一秒以后。
>> state = [500, 0,0,100,12]';
>> state = constturn(state)
state =
489.5662
-20.7912
99.2705
97.8148
12.0000指定时间间隔的恒定转弯速率运动状态更新
指定时间间隔为0.1s。更新状态到0.1s以后。
>> state = constturn(state,0.1)
state =
487.3849
-22.8351
109.0295
97.3579
12.0000constturnjac
恒定转弯速率运动的雅克比矩阵。
语法
jacobian = constturnjac(state)
jacobian = constturnjac(state, dt)ctmeas
恒定转角速率运动的测量函数。
语法
measurement = ctmeas(state)
measurement = ctmeas(state, frame)
measurement = ctmeas(state, frame, sensorpos)
measurement = ctmeas(state, frame, sensorpos, sensorvel)
measurement = ctmeas(state, frame, sensorpos, sensorvel, laxes)
measurement = ctmeas(state, measurementParameters)ctmeasjac
恒定转角速率测量函数的雅克比矩阵。
语法
measurementjac = ctmeasjac(state)
measurementjac = ctmeasjac(state, frame)
measurementjac = ctmeasjac(state, frame, sensorpos)
measurementjac = ctmeasjac(state, frame, sensorpos, sensorvel)
measurementjac = ctmeasjac(state, frame, sensorpos, sensorvel, laxes)
measurementjac = ctmeasjac(state, measurementParameters)findCubicLaneBoundaries
用三次模型寻找边界。
语法
boundaries = findCubicLaneBoundaries(xyBoundaryPoints, approxBoundaryWidth)
[boundaries, boundaryPoints] = findCubicLaneBoundaries(xyBoundaryPoints, approxBoundaryWidth)
[___] = findCubicLaneBoundaries(____, Name, Value)说明
第一种用法采用随机样本一致性（random sample consensus，RANSAC）算法通过拟合边界点和近似宽度得到三次车道边界模型。每一个cubicBoundary对象的返回数组中的模型包含【A,B,C,D】四个三次多项式方程的系数以及边界估计的长度。
第二种用法先前输入参数返回一个元胞数组。
第三种用法用可选指定一个或多个Name-Value对参数。
例子
在鸟瞰视图中搜索三次车道边界线
导入一个有车道线的道路图片。该图片有安装在车头的相机传感器获得。
>> I = imread('road.png');用预设置传感器对象将图片转换为鸟瞰图。该对象模拟捕获原始图片的传感器。
>> bevSensor = load('birdsEyeConfig.mat');
>> birdsEyeImage = transformImage(bevSensor.birdsEyeConfig,I);
>> imshow(birdsEyeImage)
以m为单位设置近似的车道线宽度。
>> approxBoundaryWidth = 0.25;以黑白图形式检测车道特征。
>> birdsEyeBW = segmentLaneMarkerRidge(rgb2gray(birdsEyeImage), bevSensor.birdsEyeConfig, approxBoundaryWidth);
>> imshow(birdsEyeBW);
获得世界坐标系下车道点。
>> [imageX, imageY] = find(birdsEyeBW);
>> xyBoundaryPoints = imageToVehicle(bevSensor.birdsEyeConfig, [imageY, imageX]);
用findCubicLaneBoundaries函数找到图中车道边界。
>> boundaries = findCubicLaneBoundaries(xyBoundaryPoints, approxBoundaryWidth);
用insertLaneBoundary函数在原始图中显示车道。Xpoints向量以米为单位，代表车道点，并且在自主车辆传感器范围内。用不同看色标记车道。默认地，车道颜色为黄色。
>> Xpoints = 3:30;
>> figure,
>> sensor = bevSensor.birdsEyeConfig.Sensor;
>> lanesI = insertLaneBoundary(I, boundaries(1),sensor, Xpoints);
>> lanesI = insertLaneBoundary(lanesI, boundaries(2), sensor, Xpoints, 'Color', 'green');
>> imshow(lanesI);
在鸟瞰图中显示车道：
>> figure,
>> BEconfig = bevSensor.birdsEyeConfig;
>> lanesBEI = insertLaneBoundary(birdsEyeImage, boundaries(1), BEconfig, Xpoints);
>> lanesBEI = insertLaneBoundary(lanesBEI, boundaries(2), BEconfig, Xpoints,'Color', 'green');
>> imshow(lanesBEI);
findParabolicLaneBoundaries
用抛物线模型寻找边界。
语法
boundaries = findCubicLaneBoundaries(xyBoundaryPoints, approxBoundaryWidth)
[boundaries, boundaryPoints] = findCubicLaneBoundaries(xyBoundaryPoints, approxBoundaryWidth)
[___] = findCubicLaneBoundaries(____, Name, Value)fitPolynomialRANSAC
用RANSAC方法拟合多点的多项式。
语法
P = fitPolynomailRANSAC(xyPoints, N, maxDistance)
[P, inlierIdx] = fitPolynomialRANSAC(____)
[____] = fitPolynomialRANSAC(__, Name, Value)说明
第一种用法通过采样一些列点xyPoints拟合多项式得到多项式的系数P。返回具有最多内点的拟合。如果拟合不成功，返回系数P为空。函数采用M-estimator sample consensus（MSAC），random sample consensus（RANSAC）算法进行拟合数据。
方法二返回一个逻辑数组inlierIdx，指定点的顺序以基于maxDistance拟合多项式。输入参数与第一种方法相同。
第三种方法通过给定一个多多个名称-值对参数指定附加选项。
例子
用RANSAC拟合噪声点多项式
用点集[x,y]构造并绘制一个抛物线。
x = (-10:0.1:10)';
y = (36-x.^2)/9;
figure
plot(x,y)
title('Paraola');
在抛物线点集中添加噪声和异值点。
y = y + rand(length(y), 1);
y([50,150, 99, 199]) = [y(50) + 12,y(150) + 12,y(99) + 33,y(199) - 23];
plot(x,y);
title('Parabola with Outliers and Noise')
用fitPolynomialRANSAC生成二次多项式系数。并得到由maxDistance从拟合多项式确定的内点。
N = 2; % 多项式次数
maxDistance = 1; % 内点距离多项式的最大距离
[P, inlierIdx] = fitPolynomialRANSAC([x,y], N, maxDistance);
% 用多项式的值验证结果。在[x,y]点上画出曲线。用红圈标记处外点
yRecoveredCurve = polyval(P, x);
figure,
plot(x, yRecoveredCurve, '-g', 'LineWidth',3);
hold on;
plot(x(inlierIdx), y(inlierIdx),'.', x(~inlierIdx), y(~inlierIdx), 'ro');
legend('Fit polynomial', 'inlier points', 'Outlier points')
hold off
evaluateLaneBoundaries
验证车道边界模型。
语法
numMatches = evaluateLaneBoundaries(boundaries, worldGroundTruthPoints, threshold)
[numMatches, numMissed, numFalsePositives] = evaluateLaneBoundaries(____)
[____]= evaluateLaneBoundaries(____, xWorld)
[____]= evaluateLaneBoundaries(boundaries, groundTruthBoundaries, threshold)
[____, assignments]= evaluateLaneBoundaries(____)说明
第一种方法通过比较输入边界模型，边界和地面实况数据，返回横向距离阈值内的车道边界匹配（真阳性）的总数。
第二种方法还使用先前的输入返回未命中总数（漏报）和误报。
第三种方法指定执行比较的x轴点。 worldGroundTruthPoints中指定的点在给定的x轴位置进行线性插值。
第四种方法将边界与在车道边界对象数组或数组单元阵列中指定的地面实况模型进行比较。
第五种方法还返回在groundTruthBoundaries中指定的赋值索引。 每个边界都与groundTruthBoundaries中相应的类赋值相匹配。 boundaries中第k个边界与worldGroundTruthPoints的assignments(k)元素匹配。 零表示误报（未找到匹配项）。
例子
比较车道边界模型
创建一组地面实况点，添加噪声以模拟实际车道边界点，并将模拟数据与模型进行比较。
通过使用抛物线参数创建一组表示地面实况的点。
parabolaParams1 = [-0.001 0.01 0.5];
parabolaParams2 = [0.001 0.02 0.52];
x = (0:0.1:20)';
y1 = polyval(parabolaParams1,x);
y2 = polyval(parabolaParams1,x);
% 添加噪声
y1 = y1 + 0.10*parabolaParams1(3)*(rand(length(y1),1)-0.5);
y2 = y2 + 0.10*parabolaParams2(3)*(rand(length(y2),1)-0.5);
% 创建一个测试边界模型集合
testlbs = parabolicLaneBoundary([-0.002 0.01 0.5;
-0.001 0.02 0.45;
-0.001 0.01 0.5;
0.000 0.02 0.52;
-0.001 0.01 0.51]);将边界模型与地面真实点进行比较。 根据匹配、漏报和误报的数量计算模型的精度和灵敏度。
[numMatches,numMisses,numFalsePositives,~] = ...
evaluateLaneBoundaries(testlbs,{[x y1],[x y2]},threshold);
disp('Precision:');
disp(numMatches/(numMatches+numFalsePositives));
disp('Sensitivity/Recall:');
disp(numMatches/(numMatches+numMisses));
Precision:
0.4000
Sensitivity/Recall:
1getTrackPositions
返回更新的跟踪位置和位置协方差矩阵。
语法
position = getTrackPosition(tracks, positionSelector)
[position, positionCovariances] = getTrackPosition(tracks, positionSelector)说明
第一种方法返回跟踪位置矩阵。 每行包含被跟踪对象的位置。
第二种方法返回一个跟踪位置的矩阵。
例子
发现3-D匀加速物体的位置
给一个3D恒定加速运动创建一个扩展卡尔曼滤波器。
>> tracker = multiObjectTracker('FilterInitializationFcn',@initcaekf);
% 通过单次检测更新跟踪器并获取轨道输出。
>> detection = objectDetection(0,[10;-20;4],'ObjectClassID',3);
>> tracks = updateTracks(tracker,detection,0)
tracks =
包含以下字段的 struct:
TrackID: 1
Time: 0
Age: 1
State: [9×1 double]
StateCovariance: [9×9 double]
IsConfirmed: 1
IsCoasted: 0
ObjectClassID: 3
ObjectAttributes: {}
% 从跟踪状态中获得位置向量
>> positionSelector = [1 0 0 0 0 0 0 0 0; 0 0 0 1 0 0 0 0 0; 0 0 0 0 0 0 1 0 0];
>> position = getTrackPositions(tracks, positionSelector)
position =
10 -20 4从3D匀速对象中找到获得位置和协方差矩阵
给一个3D匀速运动创建一个扩展卡尔曼滤波器。
>> tracker = multiObjectTracker('FilterInitializationFcn',@initcvekf);
% 用单次检测更新跟踪并得到跟踪输出
>> detection = objectDetection(0,[10;3;-7],'ObjectClassID',3);
>> tracks = updateTracks(tracker,detection,0)
tracks =
包含以下字段的 struct:
TrackID: 1
Time: 0
Age: 1
State: [6×1 double]
StateCovariance: [6×6 double]
IsConfirmed: 1
IsCoasted: 0
ObjectClassID: 3
ObjectAttributes: {}
% 从跟踪中获得位置向量和位置协方差矩阵
>> positionSelector = [1 0 0 0 0 0; 0 0 1 0 0 0; 0 0 0 0 1 0];
>> [position,positionCovariance] = getTrackPositions(tracks,positionSelector)
position =
10 3 -7
positionCovariance =
1 0 0
0 1 0
0 0 1
>> 定义
2D运动的位置选择：
当状态由位置和速度构成时，2D运动的位置选择矩阵为：
3D运动的位置选择：
当状态由位置和速度构成时，3D运动的位置选择矩阵为：
有加速度的3D运动位置选择：
当状态由位置、速度、加速度构成时，3D运动的位置选择矩阵为：
getTrackVelocites(126)
获得更新的跟踪速度和速度协方差矩阵。
initcaekf(131)
从侦测报告中创建恒定加速度的扩展卡尔曼滤波器。
initcakf(136)
从侦测报告中创建恒定加速度的线性卡尔曼滤波器。
initcaukf(139)
从侦测报告中创建恒定加速度的无迹卡尔曼滤波器。
initctekf(144)
从侦测报告中创建恒定转向速率的扩展卡尔曼滤波器。
initctukf(149)
从侦测报告中创建恒定转向速度的无迹卡尔曼滤波器。
initcvekf(154)
从侦测报告中创建恒定速度的扩展卡尔曼滤波器。
initcvkf(159)
从侦测报告中创建恒定速度的线性卡尔曼滤波器。
initcvukf(164)
从侦测报告中创建匀速无迹卡尔曼滤波器。
insertLaneBoundary(169)
在图像中插入车道。
ransac(180)
从噪声数据中拟合得到模型。
segmentLaneMarkerRidge(188)
在灰度图中检测车道。
segmentLidarData(196)
Segment organized 3-D range data into clusters.雷达数据（点云）分段聚类。
vehicleDetectorACF(204)
使用聚合通道功能(aggregate channel features)加载车辆检测器.
vehicleDetectorFasterRCNN(207)
用faster R-CNN检测车辆。
driving.scenario.TargetsToEgo(210)
将actor的姿态转换到ego坐标系下。
driving.scenario.roadBoundariesToEgo(215)
将道路边界转换到ego坐标系下。
plotOutline(222)
绘制对象轮廓。
plotCoverageArea(229)
绘制覆盖区域的鸟瞰图。
plotDetection(237)
绘制一组检测对象。
plotLaneBoundary(242)
在鸟瞰图中绘制车道边界。
plotLaneMarking(248)
在鸟瞰图中绘制车道标志。
plotPath(255)
在鸟瞰图中绘制车道边界。
plotTrack(259)
绘制一系列检测跟踪。
checkPathValidity(264)
检测规划的车辆路径是否有效。