Skip to content

Commit bc818a0

Browse files
ClemensLinnhoffClemensLinnhoff
authored
Change Tracking Coordinate System to Virtual Sensor Frame (#19)
* Check input mounting position; set virtual mounting position to 0,0,0 Signed-off-by: ClemensLinnhoff <Clemens.Linnhoff@partner.bmw.de> * Check if sensor specific view config with mounting position exists. If not, replace with mounting position from profile Signed-off-by: ClemensLinnhoff <Clemens.Linnhoff@partner.bmw.de> * Replace profile mounting position in front end and data extraction Signed-off-by: ClemensLinnhoff <Clemens.Linnhoff@partner.bmw.de> * Perform Tracking in virtual sensor coordinates instead of ego vehicle coordinates Signed-off-by: ClemensLinnhoff <Clemens.Linnhoff@partner.bmw.de> * Replace smoke test trace file due to missing bbcenter2rear Signed-off-by: ClemensLinnhoff <Clemens.Linnhoff@partner.bmw.de> * Added trace file for regression test Signed-off-by: ClemensLinnhoff <Clemens.Linnhoff@partner.bmw.de> * Added analysis script Signed-off-by: ClemensLinnhoff <Clemens.Linnhoff@partner.bmw.de> --------- Signed-off-by: ClemensLinnhoff <Clemens.Linnhoff@partner.bmw.de>
1 parent 3776386 commit bc818a0

File tree

20 files changed

+971
-50
lines changed

20 files changed

+971
-50
lines changed

‎.clang-tidy‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
---
2-
Checks: 'readability-*,cppcoreguidelines-*,-cppcoreguidelines-init-variables,-cppcoreguidelines-avoid-magic-numbers,-readability-magic-numbers'
2+
Checks: 'readability-*,cppcoreguidelines-*,-cppcoreguidelines-pro-type-vararg,-cppcoreguidelines-pro-type-cstyle-cast,-cppcoreguidelines-avoid-c-arrays,-cppcoreguidelines-non-private-member-variables-in-classes,-cppcoreguidelines-macro-usage,-cppcoreguidelines-pro-bounds-pointer-arithmetic,-cppcoreguidelines-pro-bounds-array-to-pointer-decay,-cppcoreguidelines-owning-memory,-readability-useless-return-value,-readability-const-return-type,-cppcoreguidelines-init-variables,-cppcoreguidelines-avoid-magic-numbers,-readability-magic-numbers'
33
HeaderFilterRegex: '.*/src/(?:adp|build_system|legacy)/.*'
44
AnalyzeTemporaryDtors: false
55
CheckOptions:
@@ -263,5 +263,7 @@ CheckOptions:
263263
value: 'x|y|z|id|mu|a|b|c'
264264
- key: readability-identifier-length.IgnoredParameterNames
265265
value: 'x|y|z|id'
266+
- key: readability-function-cognitive-complexity.Threshold
267+
value: 100
266268
...
267269

‎.github/workflows/cl2.yml‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -143,6 +143,7 @@ jobs:
143143

144144
- name: Check For Python Scripts
145145
id: check-for-python-scripts
146+
if: steps.check-for-trace-files.outputs.found_trace == 1
146147
working-directory: ./test/integration/${{ matrix.path }}
147148
run: |
148149
( count=`ls -1 *.py 2>/dev/null | wc -l`
@@ -164,14 +165,14 @@ jobs:
164165
run: sudo make install && sudo ldconfig
165166

166167
- name: Install Analysis Dependencies
167-
if: steps.check-for-python-scripts.outputs.found_script == 1
168+
if: steps.check-for-python-scripts.outputs.found_script == 1 && steps.check-for-trace-files.outputs.found_trace == 1
168169
working-directory: ./lib/open-simulation-interface
169170
run: |
170171
python3 -m pip install protobuf==4.21.12
171172
python3 -m pip install .
172173
173174
- name: Run Trace File Analysis
174-
if: steps.check-for-python-scripts.outputs.found_script == 1
175+
if: steps.check-for-python-scripts.outputs.found_script == 1 && steps.check-for-trace-files.outputs.found_trace == 1
175176
working-directory: ./test/integration/${{ matrix.path }}
176177
run: python3 ${{ steps.check-for-python-scripts.outputs.python_file_name }} output/${{ steps.check-for-trace-files.outputs.trace_file_name }}
177178

‎.gitignore‎

Lines changed: 41 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,43 @@
1-
cmake*
2-
build/
3-
.vscode/
4-
.editorconfig
5-
out/
1+
# Prerequisites
2+
*.d
3+
4+
# Compiled Object files
5+
*.slo
6+
*.lo
7+
*.o
8+
*.obj
9+
10+
# Precompiled Headers
11+
*.gch
12+
*.pch
13+
14+
# Compiled Dynamic libraries
15+
*.so
16+
*.dylib
17+
*.dll
18+
19+
# Fortran module files
20+
*.mod
21+
*.smod
22+
23+
# Compiled Static libraries
24+
*.lai
25+
*.la
26+
*.a
27+
*.lib
28+
29+
# Executables
30+
*.exe
31+
*.out
32+
*.app
33+
34+
# Build folder
35+
/build*
36+
/cmake-build*
37+
38+
# IDE configs
639
.idea/
740
.vs/
8-
*.bat
9-
*.sh
41+
.vscode/
42+
.editorconfig
43+
__pycache__

‎src/model/profiles/profile_Ibeo_LUX_2010.hpp.in‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,9 +27,9 @@ namespace model::profile::Ibeo_LUX_2010 {
2727
Ibeo_LUX_2010.sensor_view_configuration.set_range(400.0);
2828
Ibeo_LUX_2010.sensor_view_configuration.set_field_of_view_horizontal(180.0 / 180 * M_PI);
2929
Ibeo_LUX_2010.sensor_view_configuration.set_field_of_view_vertical(170.0 / 180 * M_PI);
30-
Ibeo_LUX_2010.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_x(5.0);
30+
Ibeo_LUX_2010.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_x(0.0);
3131
Ibeo_LUX_2010.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_y(0.0);
32-
Ibeo_LUX_2010.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_z(0.489);
32+
Ibeo_LUX_2010.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_z(0.0);
3333
Ibeo_LUX_2010.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_roll(0.0 / 180 * M_PI);
3434
Ibeo_LUX_2010.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_pitch(-0.5 / 180 * M_PI);
3535
Ibeo_LUX_2010.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_yaw(0-0.25 / 180 * M_PI);

‎src/model/profiles/profile_LongRange_Radar.hpp.in‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -27,9 +27,9 @@ namespace model::profile::LongRange_Radar {
2727
LongRange_Radar.sensor_view_configuration.set_range(400.0);
2828
LongRange_Radar.sensor_view_configuration.set_field_of_view_horizontal(180.0 / 180 * M_PI);
2929
LongRange_Radar.sensor_view_configuration.set_field_of_view_vertical(170.0 / 180 * M_PI);
30-
LongRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_x(3.82);
31-
LongRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_y(0.35);
32-
LongRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_z(0.24);
30+
LongRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_x(0.0);
31+
LongRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_y(0.0);
32+
LongRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_z(0.0);
3333
LongRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_roll(0.0 / 180 * M_PI);
3434
LongRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_pitch(0.0 / 180 * M_PI);
3535
LongRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_yaw(0.0 / 180 * M_PI);

‎src/model/profiles/profile_MidRange_Radar.hpp.in‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -27,9 +27,9 @@ namespace model::profile::MidRange_Radar {
2727
MidRange_Radar.sensor_view_configuration.set_range(200.0);
2828
MidRange_Radar.sensor_view_configuration.set_field_of_view_horizontal(180.0 / 180 * M_PI);
2929
MidRange_Radar.sensor_view_configuration.set_field_of_view_vertical(170.0 / 180 * M_PI);
30-
MidRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_x(3.82);
31-
MidRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_y(0.35);
32-
MidRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_z(0.24);
30+
MidRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_x(0.0);
31+
MidRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_y(0.0);
32+
MidRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_z(0.0);
3333
MidRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_roll(0.0 / 180 * M_PI);
3434
MidRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_pitch(0.0 / 180 * M_PI);
3535
MidRange_Radar.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_yaw(0.0 / 180 * M_PI);

‎src/model/profiles/profile_SCALA_1.hpp.in‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -27,9 +27,9 @@ namespace model::profile::SCALA_1 {
2727
SCALA_1.sensor_view_configuration.set_range(400.0);
2828
SCALA_1.sensor_view_configuration.set_field_of_view_horizontal(180.0 / 180 * M_PI);
2929
SCALA_1.sensor_view_configuration.set_field_of_view_vertical(170.0 / 180 * M_PI);
30-
SCALA_1.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_x(3.82);
31-
SCALA_1.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_y(-0.35);
32-
SCALA_1.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_z(0.24);
30+
SCALA_1.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_x(0.0);
31+
SCALA_1.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_y(0.0);
32+
SCALA_1.sensor_view_configuration.mutable_mounting_position()->mutable_position()->set_z(0.0);
3333
SCALA_1.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_roll(0.0 / 180 * M_PI);
3434
SCALA_1.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_pitch(0.0 / 180 * M_PI);
3535
SCALA_1.sensor_view_configuration.mutable_mounting_position()->mutable_orientation()->set_yaw(0.0 / 180 * M_PI);

‎src/model/strategies/data-extraction-strategy/src/DataExtractionStrategy.cpp‎

Lines changed: 15 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -254,17 +254,18 @@ double DataExtractionStrategy::calculate_irradiation_gain(double azimuth_angle_r
254254

255255
void DataExtractionStrategy::transform_detections_to_logical_detections(osi3::SensorData& sensor_data, const osi3::LidarDetectionData& detection_data) const
256256
{
257-
osi3::Vector3d mounting_position;
258-
osi3::Orientation3d mounting_orientation;
259-
if (!profile.sensor_view_configuration.radar_sensor_view_configuration().empty())
257+
MountingPosition mounting_pose;
258+
if (sensor_data.sensor_view(0).radar_sensor_view_size() > 0)
260259
{ // radar
261-
mounting_position.CopyFrom(profile.sensor_view_configuration.radar_sensor_view_configuration(0).mounting_position().position());
262-
mounting_orientation.CopyFrom(profile.sensor_view_configuration.radar_sensor_view_configuration(0).mounting_position().orientation());
260+
mounting_pose.CopyFrom(sensor_data.sensor_view(0).radar_sensor_view(0).view_configuration().mounting_position());
263261
}
264-
else
262+
else if (sensor_data.sensor_view(0).lidar_sensor_view_size() > 0)
265263
{ // lidar
266-
mounting_position.CopyFrom(profile.sensor_view_configuration.lidar_sensor_view_configuration(0).mounting_position().position());
267-
mounting_orientation.CopyFrom(profile.sensor_view_configuration.lidar_sensor_view_configuration(0).mounting_position().orientation());
264+
mounting_pose.CopyFrom(sensor_data.sensor_view(0).lidar_sensor_view(0).view_configuration().mounting_position());
265+
}
266+
else
267+
{
268+
alert("No lidar or radar sensor view found!");
268269
}
269270

270271
for (const auto& current_detection : detection_data.detection())
@@ -277,12 +278,14 @@ void DataExtractionStrategy::transform_detections_to_logical_detections(osi3::Se
277278
point_cartesian_sensor.set_x(distance * cos(elevation) * cos(azimuth));
278279
point_cartesian_sensor.set_y(distance * cos(elevation) * sin(azimuth));
279280
point_cartesian_sensor.set_z(distance * sin(elevation));
280-
osi3::Vector3d point_cartesian_vehicle = TF::transform_from_local_coordinates(point_cartesian_sensor, mounting_orientation, mounting_position);
281+
osi3::Vector3d point_cartesian_vehicle = TF::transform_from_local_coordinates(point_cartesian_sensor, mounting_pose.orientation(), mounting_pose.position());
282+
osi3::Vector3d point_cartesian_virtual_sensor = TF::transform_to_local_coordinates(
283+
point_cartesian_vehicle, sensor_data.sensor_view(0).mounting_position().orientation(), sensor_data.sensor_view(0).mounting_position().position());
281284

282285
auto* current_logical_detection = sensor_data.mutable_logical_detection_data()->add_logical_detection();
283-
current_logical_detection->mutable_position()->set_x(point_cartesian_vehicle.x());
284-
current_logical_detection->mutable_position()->set_y(point_cartesian_vehicle.y());
285-
current_logical_detection->mutable_position()->set_z(point_cartesian_vehicle.z());
286+
current_logical_detection->mutable_position()->set_x(point_cartesian_virtual_sensor.x());
287+
current_logical_detection->mutable_position()->set_y(point_cartesian_virtual_sensor.y());
288+
current_logical_detection->mutable_position()->set_z(point_cartesian_virtual_sensor.z());
286289
current_logical_detection->set_intensity(current_detection.intensity());
287290
current_logical_detection->mutable_object_id()->set_value(current_detection.object_id().value());
288291
}

‎src/model/strategies/front-end-strategy/include/frontendstrategy/FrontEndStrategy.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ class FrontEndStrategy : public Strategy
5757
void apply(osi3::SensorData& sensor_data) override;
5858

5959
private:
60-
static void check_sensor_data_input(const osi3::SensorData& sensor_data, const Alert& alert);
60+
void check_sensor_data_input(osi3::SensorData& sensor_data, const Alert& alert);
6161
static void set_sensor_data_timestamp(osi3::SensorData& sensor_data, const osi3::SensorView& input_sensor_view, const Alert& alert);
6262
static bool simulate_sensor_failure(osi3::SensorData& sensor_data, const Profile& profile, const Log& log);
6363
static std::vector<GroundTruthObject> bring_ground_truth_objects_to_unified_format(const osi3::SensorView& input_sensor_view,

0 commit comments

Comments
 (0)