first commit

.gitattributes

@@ -0,0 +1,6 @@
+# Auto detect text files and perform LF normalization
+# https://stackoverflow.com/questions/21472971/what-is-the-purpose-of-text-auto-in-gitattributes-file
+# https://code.visualstudio.com/docs/remote/troubleshooting#_resolving-git-line-ending-issues-in-wsl-resulting-in-many-modified-files
+* text=auto eol=lf
+*.{cmd,[cC][mM][dD]} text eol=crlf
+*.{bat,[bB][aA][tT]} text eol=crlf

.gitignore

@@ -0,0 +1,70 @@
+# cuixingxing 定义的.gitgnore文件,参考：在.gitignore中如何写可以忽略掉无扩展名的Unix可执行文件？ https://www.zhihu.com/question/26247816/answer/71915337 ，以后项目依据此文件适当修改，2021.9.26
+# 文件夹前面必须要用"/"才奏效，2022.3.8 
+# 2022.10.14 根据最新参考修改模板
+
+#忽略所有文件和目录(当前目录不能忽略)
+* 
+
+# 根据自己项目工程修改,增加指定目录和下面所有目录
+!/Assets/
+!/Assets/**
+
+!/Project/
+!/Project/**
+
+!/Scenarios/
+!/Scenarios/**
+
+!/Scenes/
+!/Scenes/**
+
+!images/
+!images/**
+
+!Exports/
+!Exports/**
+
+# 增加指定扩展名文件#忽略特殊文件，一般是当前目录下的文件(当前目录不能忽略)
+!.gitignore
+!.gitattributes
+!*.json
+!*.md
+
+# license
+!LICENSE
+
+# python
+!*.py
+!*.ipynb
+
+# matlab
+!*.m
+!*.mlx
+!*.slx
+!*.p
+
+# c/c++
+!*.cpp
+!*.hpp
+!*.h
+!*.c
+
+# cmake
+!CMakeLists.txt
+!Makefile
+
+# 根据自己项目工程修改
+!*.prj
+!*.txt
+!*.cgt
+!*.osm
+
+#忽略特殊文件，一般是当前目录下的文件(当前目录不能忽略)
+/codegen_custom_cpp/codegen/
+/yourSpecify.cpp
+/yourSpecify.h
+
+
+
+
+

Exports/README.md

@@ -0,0 +1,2 @@
+
+本目录存储的为RoadRunner导出的fbx文件和一些资源文件

README.md

@@ -0,0 +1,72 @@
+# 基于Simulink/RoadRunner的3D-HD map建图仿真和深度语义地图生成
+
+[![RoadRunner Scenario Parking Valet](https://res.cloudinary.com/marcomontalbano/image/upload/v1720923105/video_to_markdown/images/youtube--NYIECLVFOF0-c05b58ac6eb4c4700831b2b3070cd403.jpg)](https://youtu.be/NYIECLVFOF0 "RoadRunner Scenario Parking Valet")
+
+>本Repo基于Simulink/RoadRunner构建了一个HD map，高逼格近似3D地图，可用于Scenario各种工况下的仿真，包括但不限于车位检测，车道线检测，可行驶区域检测，交通标志检测等一体的高度融合环境。地图格式支持导出AutoCAD®, Filmbox, glTF™, OpenFlight, OpenSceneGraph, ASAM OpenDRIVE®, USD, Apollo, CARLA, Metamoto®, Unity®, Unreal®, GeoJSON, and VTD等各类格式。
+
+[RoadRunner®](https://www.mathworks.com/help/roadrunner/index.html)可根据`OpenStreetMap`、`Apollo HD Map`、`HERE HD Live Map`等导入地图数据或者直接从头开始逐一搭建，本项目所有3D目标均手工搭建，地图地面信息表达充分，特别是泊车位尽可能与实际表达一致，道路路面材质几乎与实际保持一致，通过自主/算法设计路线泊车，另外还可以根据速度profile+ego/non-ego vehicle自定制动态场景仿真+碰撞检测，高度逼近任意Scenario，适合各类感知算法corner case测试、深度学习groundTruth数据生成，也可以为下游的规控算法提供支持。
+
+## Requirements
+
+MathWorks Products (<https://www.mathworks.com>)
+
+- Requires MATLAB release R2023b or newer.
+- Simulink™
+- Automated Driving Toolbox™.
+- RoadRunner™
+- RoadRunner Asset Library™
+- RoadRunner Scenario™
+- Sensor Fusion and Tracking Toolbox™ or Navigation Toolbox™ or UAV Toolbox™,([waypointTrajectory](https://www.mathworks.com/help/nav/ref/waypointtrajectory-system-object.html))
+- Navigation Toolbox™ or Robotics System Toolbox™ or UAV Toolbox™,([quat2rotm](https://www.mathworks.com/help/nav/ref/quat2rotm.html))
+
+Optional Products(<https://www.mathworks.com>)
+
+- Deep Learning Toolbox™
+- Computer Vision Toolbox™
+
+## 工业厂区3D场景图一角（Scene）
+
+由于地图是3D格式，为方便在此处一览，摘取了部分视角图像进行浏览如下，**源文件**位于`Scenes/IndustryZone.rrscene`，需要可前往下载。
+
+![scenario1](images/road.jpeg)
+![scenario2](images/road2.jpeg)
+![scenario3](images/road3.jpeg)
+
+## 记忆泊车仿真(Scenario)
+
+Scenario为动态设计的场景逻辑，源文件位于`Scenarios/IndustryZone.rrscenario`，需要可前往下载。该场景设计了从工业区侧边一条路东北门进入到U型广场泊车，红色泊车道路全长448.11米，在泊车区域内人为任意布置17辆不同车型，外观，位置的车辆。并设计一辆SUV为ego vehicle,车速3米/秒，视角选择"follow"，图像生成如下：
+![scenario](images/scenario.PNG)
+![scenario](images/scenarioRoad.JPG)
+![roadrunner](images/RoadRunnerDemo.gif)
+
+## 深度语义地图生成
+
+上述Scenario动态产生的场景可直接联合Simulink仿真生成语义地图数据，为简化地图映射表示说明，只对环形道路一圈(约350米)进行语义地图映射，对不同路段的路面尽可能用原始材质表示，不同车位及先前已经停好的各类车。这里仅**搭建简易的Simulink模型**如下所示：
+
+![SegmentModel](images/SegmentModel.JPG)
+
+1. 选取waypoints
+
+针对ego vehicle在BEV地图上选取waypoints并导入到MATLAB中生成trajectory，设计以经过所有waypoints的匀速行驶，到达各个waypoints时间可以自动推理计算得到。
+
+![waypoints](images/waypoints.PNG)
+
+在MATLAB中绘制的轨迹路线如下：
+![trajectory](images/trajectorySimple.png)
+
+2. 布局各类已经停好的车
+
+设计若干不同车类型，颜色，泊车位停放的车辆，见下图：
+![birdeyeView](images/birdeyeView.PNG)
+
+3. 语义映射地图
+
+以下为仿真的地图原图和映射实时对比。
+![origin](images/origin.PNG)
+![segment](images/segment.PNG)
+
+再看下俯视图BEV地图映射效果：
+
+![DEMO](images/DEMO.gif)
+
+大量在任意视角下适用各类场景的深度学习语义图片可根据自己需求进行更改，上述简单模型产生的语义图片在`writeimages`模块下设置。

getMyTrajectory.m

@@ -0,0 +1,16 @@
+% 以下为通过编程接口方式控制RoadRunner仿真场景
+% 但目前版本暂时无法通过编程方式获得roadrunner scenario车辆轨迹路径点坐标，改
+% 用方法是RoadRunner Scene中的测量工具获取wayPoints3D,然后再导入到MATLAB中使用
+% 
+rrInstallationPath = "C:\Program Files\RoadRunner R2023b\bin\win64";
+rrProjectPath = "E:\RoadRunner_work\myscene\New RoadRunner Project";
+
+s = settings;
+s.roadrunner.application.InstallationFolder.PersonalValue = rrInstallationPath;
+rrApp = roadrunner(rrProjectPath);
+openScenario(rrApp,"TrajectoryCutIn")
+rrSim = createSimulation(rrApp);
+simStepSize = 0.1;
+set(rrSim,"StepSize",simStepSize);
+
+set(rrSim,"SimulationCommand","Start")

logSensorSubFcn.m

@@ -0,0 +1,71 @@
+function logSensorSubFcn(timeStamp,location, orientation,bevImage,bevSemanticImage)
+% Brief: 用于simulink matlab function模块的调用函数，记录传感器数据，便于C++使用其数据
+% Details:
+%    子函数，undefined function时候，可以独立为m文件使用
+% 
+% Syntax:  
+%     logSensorSubFcn(timeStamp,egoVelocity,poseEgoInGlobalWorld,positionRotationEgoInGloableWorld,imgFrontSurround,imgRearSurround,imgLeftSurround,imgRightSurround,imageImgFrontWindshield,ultrasonicData)
+% 
+% Inputs:
+%    timeStamp - [1,1] size,[double] type,Description
+%    egoVelocity - [1,1] size,[double] type,Description
+%    poseEgoInGlobalWorld - [1,3] size,[double] type,Description
+%    positionRotationEgoInGloableWorld - [1,6] size,[double] type,Description
+%    imgFrontSurround - [m,n] size,[None] type,Description
+%    imgRearSurround - [m,n] size,[None] type,Description
+%    imgLeftSurround - [m,n] size,[None] type,Description
+%    imgRightSurround - [m,n] size,[None] type,Description
+%    imageImgFrontWindshield - [M,N] size,[None] type,Description
+%    ultrasonicData - [1,12] size,[double] type,Description
+% 
+% Outputs:
+%    None
+% 
+% Example: 
+%    None
+% 
+% See also: None
+
+% Author:                          cuixingxing
+% Email:                           cuixingxing150@gmail.com
+% Created:                         26-Jul-2022 13:43:21
+% Version history revision notes:
+%                                  None
+% Implementation In Matlab R2022a
+% Copyright © 2022 TheMatrix.All Rights Reserved.
+%
+arguments
+    % required
+    timeStamp (1,1) double
+   location
+   orientation
+    bevImage
+    bevSemanticImage
+end
+
+% step1: initialize variables 
+persistent numStep allTT 
+if isempty(numStep)
+    numStep = 1;
+    timeStamp = duration(seconds(timeStamp));
+    currTT = timetable(timeStamp,location,orientation);% corresponding to C++ mat2BinStruct type
+    allTT = currTT;
+else
+    timeStamp = duration(seconds(timeStamp));
+    currTT = timetable(timeStamp,location,orientation);% corresponding to C++ mat2BinStruct type
+    allTT = [allTT;currTT];
+end
+
+% step2: write current images
+img1Name = sprintf("./results/original/%05d.jpg",numStep);
+img2Name = sprintf("./results/semantic/%05d.png",numStep);
+
+imwrite(bevImage,img1Name);
+imwrite(bevSemanticImage,img2Name);
+
+% step3: write csv sensor data
+writetimetable(allTT,"./results/sensorData.csv");
+
+% step4: numStep plus one
+numStep  = numStep+1;
+end

main.m

@@ -0,0 +1,101 @@
+%% 直接从RoadRunner Scene中的测量工具获取wayPoints3D,然后再导入到MATLAB中使用
+% smoothPathSpline/waypointTrajectory函数进行平滑处理得到trajectory
+% 参考：
+% 1. Select Waypoints for Unreal Engine Simulation
+% 2. Import RoadRunner Scene into Unreal Engine Using Simulink
+% 注意：
+% 由于仿真是调用的Unreal Engine，R2023b只能在windows上仿真查看结果，后续版本
+% 支持win,linux,mac。
+%
+% 测试通过，效果较好
+
+% waypoints是从roadrunner中鼠标选点得到的坐标，导入到simulink也是世界坐标点
+isUseParkingGarage = 0; % 为1时使用simulinkParkGarage.slx，为0时使用simulinkPark.slx
+if isUseParkingGarage % RoadRunner自带地下停车场路径控制点
+    wayPoints3D = [-26.52,12.12,6.0;
+        -26.70,-2.43,6.0;
+        -21.16,-11.55,6.0;
+        -18.17,-11.64,6.0;
+        -2.10,-11.64,6.0;
+        13.49,-11.57,6.0;
+        17.66,-11.64,6.0;
+        23.83,-7.15,6.0;
+        26.23,-2.21,6.0;
+        18.54,6.97,6.0;
+        13.25,6.97,5.72;
+        -14.93,7.19,3.72;
+        -24.83,6.17,3.03;
+        -28.26,1.48,3.0;
+        -25.91,-9.49,3.0;
+        -22.63,-11.71,3.0;
+        -20.26,-11.65,3.0;
+        16.41,-11.22,3.0;
+
+        21.42,-10.17,3.0;
+        26.49,-4.53,3.0;
+        26.35,3.07,3.0;
+        23.10,6.26,3.0;
+        17.09,6.55,2.78;
+        12.85,6.86,2.46;
+        -12.17,7.72,0.53;
+        -17.32,7.52,0.19;
+        -26.52,10.17,0.0;
+        -35.61,8.83,0.0];
+    modelName = "simulinkParkGarage";
+else %自己设计的一个回圈停车场
+    wayPoints3D = [1.81,-29.07,0;
+        5.9,14.61,0;
+        11.63,22.09,0;
+        41.11,21.10,0;
+        50.27,12.05,0;
+        50.52,-4.05,0;
+        50.25,-50.45,0;
+        44.18,-55.77,0;
+        33.04,-55.77,0;
+        4.55,-50.22,0;
+        2.07,-46.23,0;
+        1.53,-33.31,0;
+        1.70,-31.25,0];
+    modelName = "simulinkPark";
+end
+nums = size(wayPoints3D,1);
+
+trajectory = waypointTrajectory(wayPoints3D,GroundSpeed=20*ones(nums,1));
+
+% 以匀速17m/s进行前进，获得起始，终止时间点
+t0 = trajectory.TimeOfArrival(1);
+tf = trajectory.TimeOfArrival(end);
+
+% 时间均匀采样，看每个点的ego vehicle姿态
+sampleTimes = t0:1/60:tf; %取步长1/60是因为与Simulation 3D Scene Configuration一致，保障图像连续
+
+[position,ori,velocity,acceleration,~] = lookupPose(trajectory,sampleTimes);
+eul = quat2eul(ori);
+yaw = rad2deg(eul(:,1));
+
+% 拟传递给simulink的控制位姿
+posX = [sampleTimes',position(:,1)];
+posY = [sampleTimes',position(:,2)];
+posYaw = [sampleTimes',yaw];
+
+%
+figure()
+plot3(position(:,1),position(:,2),position(:,3),LineWidth=2)
+hold on
+plot3(wayPoints3D(:,1),wayPoints3D(:,2),wayPoints3D(:,3),'ro-',LineWidth=2)
+xlabel("x (m)")
+ylabel("y (m)")
+zlabel("z (m)")
+title("Trajectory")
+legend(["trajectory","waypoints"])
+grid on;
+
+%% 3D仿真+地图映射
+
+open_system(modelName);
+set_param(gcs, 'StopTime', num2str(sampleTimes(end)));
+sim(modelName)
+close_system(modelName)
+
+
+