Skip to content

Commit 3310de6

Browse files
tobifalkcassava
tobifalk
authored and
cassava
committed
vtd: Obtain OSI lane boundaries from ground truth
1 parent f24216c commit 3310de6

8 files changed

+225
-107
lines changed

‎plugins/vtd/src/osi_omni_sensor.cpp

+126-55
Original file line numberDiff line numberDiff line change
@@ -22,15 +22,18 @@
2222

2323
#include "osi_omni_sensor.hpp"
2424

25-
#include <cassert> // for assert
26-
#include <map> // for map<>
25+
#include <math.h> // for atan
26+
#include <algorithm> // for max
27+
#include <cassert> // for assert
28+
#include <map> // for map<>
2729

2830
#include <Eigen/Geometry> // for Isometry3d, Vector3d
2931

30-
#include <cloe/component/object.hpp> // for Object
31-
#include <cloe/core.hpp> // for Duration
32-
#include <cloe/simulator.hpp> // for ModelError
33-
#include <cloe/utility/geometry.hpp> // for quaternion_from_rpy
32+
#include <cloe/component/lane_boundary.hpp> // for LaneBoundary
33+
#include <cloe/component/object.hpp> // for Object
34+
#include <cloe/core.hpp> // for Duration
35+
#include <cloe/simulator.hpp> // for ModelError
36+
#include <cloe/utility/geometry.hpp> // for quaternion_from_rpy
3437

3538
#include "osi_common.pb.h" // for Timestamp, Identifier, BaseMoving, ..
3639
#include "osi_detectedobject.pb.h" // for DetectedMovingObject
@@ -101,6 +104,44 @@ const std::map<osi3::MovingObject::VehicleClassification::Type, cloe::Object::Cl
101104
cloe::Object::Class::Unknown},
102105
};
103106

107+
/**
108+
* Convert from OSI lane boundary types to Cloe types.
109+
*/
110+
const std::map<osi3::LaneBoundary_Classification_Type, cloe::LaneBoundary::Type>
111+
osi_lane_bdry_type_map = {
112+
// clang-format off
113+
{osi3::LaneBoundary_Classification_Type_TYPE_UNKNOWN, cloe::LaneBoundary::Type::Unknown},
114+
{osi3::LaneBoundary_Classification_Type_TYPE_OTHER, cloe::LaneBoundary::Type::Unknown},
115+
{osi3::LaneBoundary_Classification_Type_TYPE_NO_LINE, cloe::LaneBoundary::Type::Unknown},
116+
{osi3::LaneBoundary_Classification_Type_TYPE_SOLID_LINE, cloe::LaneBoundary::Type::Solid},
117+
{osi3::LaneBoundary_Classification_Type_TYPE_DASHED_LINE, cloe::LaneBoundary::Type::Dashed},
118+
{osi3::LaneBoundary_Classification_Type_TYPE_BOTTS_DOTS, cloe::LaneBoundary::Type::Unknown},
119+
{osi3::LaneBoundary_Classification_Type_TYPE_ROAD_EDGE, cloe::LaneBoundary::Type::Unknown},
120+
{osi3::LaneBoundary_Classification_Type_TYPE_SNOW_EDGE, cloe::LaneBoundary::Type::Unknown},
121+
{osi3::LaneBoundary_Classification_Type_TYPE_GRASS_EDGE, cloe::LaneBoundary::Type::Grass},
122+
{osi3::LaneBoundary_Classification_Type_TYPE_GRAVEL_EDGE, cloe::LaneBoundary::Type::Unknown},
123+
{osi3::LaneBoundary_Classification_Type_TYPE_SOIL_EDGE, cloe::LaneBoundary::Type::Unknown},
124+
{osi3::LaneBoundary_Classification_Type_TYPE_GUARD_RAIL, cloe::LaneBoundary::Type::Unknown},
125+
{osi3::LaneBoundary_Classification_Type_TYPE_CURB, cloe::LaneBoundary::Type::Curb},
126+
{osi3::LaneBoundary_Classification_Type_TYPE_STRUCTURE, cloe::LaneBoundary::Type::Unknown},
127+
// clang-format on
128+
};
129+
130+
/**
131+
* Convert from OSI lane boundary colors to Cloe colors.
132+
*/
133+
const std::map<int, cloe::LaneBoundary::Color> osi_lane_bdry_color_map = {
134+
{osi3::LaneBoundary_Classification_Color_COLOR_UNKNOWN, cloe::LaneBoundary::Color::Unknown},
135+
{osi3::LaneBoundary_Classification_Color_COLOR_OTHER, cloe::LaneBoundary::Color::Unknown},
136+
{osi3::LaneBoundary_Classification_Color_COLOR_NONE, cloe::LaneBoundary::Color::Unknown},
137+
{osi3::LaneBoundary_Classification_Color_COLOR_WHITE, cloe::LaneBoundary::Color::White},
138+
{osi3::LaneBoundary_Classification_Color_COLOR_YELLOW, cloe::LaneBoundary::Color::Yellow},
139+
{osi3::LaneBoundary_Classification_Color_COLOR_RED, cloe::LaneBoundary::Color::Red},
140+
{osi3::LaneBoundary_Classification_Color_COLOR_BLUE, cloe::LaneBoundary::Color::Blue},
141+
{osi3::LaneBoundary_Classification_Color_COLOR_GREEN, cloe::LaneBoundary::Color::Green},
142+
{osi3::LaneBoundary_Classification_Color_COLOR_VIOLET, cloe::LaneBoundary::Color::Unknown},
143+
};
144+
104145
cloe::Duration osi_timestamp_to_time(const osi3::Timestamp& timestamp) {
105146
return std::chrono::duration_cast<cloe::Duration>(std::chrono::seconds(timestamp.seconds()) +
106147
std::chrono::nanoseconds(timestamp.nanos()));
@@ -395,7 +436,8 @@ void OsiOmniSensor::process(osi3::SensorData* osi_sd, cloe::Duration& sim_time)
395436
if (mnt_pos) {
396437
osi_sensor_pose_ = osi_position_orientation_to_pose(*mnt_pos);
397438
} else {
398-
if (this->get_mock_level(MockTarget::MountingPosition) != MockLevel::Zero) {
439+
if (this->get_mock_level(SensorMockTarget::MountingPosition) !=
440+
SensorMockLevel::OverwriteNone) {
399441
osi_sensor_pose_ =
400442
get_static_mounting_position(ground_truth_->get_veh_coord_sys_info(owner_id_),
401443
ground_truth_->get_mov_obj_dimensions(owner_id_));
@@ -410,59 +452,31 @@ void OsiOmniSensor::process(osi3::SensorData* osi_sd, cloe::Duration& sim_time)
410452
throw cloe::ModelError("OSI host_vehicle_location handling is not yet available");
411453
}
412454

413-
// Detected moving objects
455+
// Process detected moving objects.
414456
for (int i_mo = 0; i_mo < osi_sd->moving_object_size(); ++i_mo) {
415457
this->process(osi_sd->has_moving_object_header(), osi_sd->moving_object_header(),
416458
osi_sd->moving_object(i_mo));
417459
}
418460

419-
#if 0
420-
// Detected stationary objects
421-
for (int i_so = 0; i_so < osi_sd->stationary_object_size(); ++i_so) {
422-
this->process(osi_sd->has_stationary_object_header(),
423-
osi_sd->stationary_object_header(), osi_sd->stationary_object(i_so));
424-
}
425-
#endif
461+
// TODO(tobias): Process detected stationary objects.
426462

427-
#if 0
428-
// Detected traffic signs
429-
for (int i_ts = 0; i_ts < osi_sd->traffic_sign_size(); ++i_ts) {
430-
this->process(osi_sd->has_traffic_sign_header(),
431-
osi_sd->traffic_sign_header(), osi_sd->traffic_sign(i_ts));
463+
// Process lane boundaries.
464+
switch (this->get_mock_level(SensorMockTarget::DetectedLaneBoundary)) {
465+
case SensorMockLevel::OverwriteAll: {
466+
mock_detected_lane_boundaries();
467+
break;
432468
}
433-
#endif
434-
435-
#if 0
436-
// Detected road markings
437-
for (int i_rm = 0; i_rm < osi_sd->road_marking_size(); ++i_rm) {
438-
this->process(osi_sd->has_road_marking_header(),
439-
osi_sd->road_marking_header(), osi_sd->road_marking(i_rm));
469+
default: {
470+
// TODO(tobias): Detected road marking handling is not yet available.
471+
break;
440472
}
441-
#endif
473+
}
442474

443-
#if 0
444-
// Detected lane boundaries
445-
for (int i_lb = 0; i_lb < osi_sd->lane_boundary_size(); ++i_lb) {
446-
this->process(osi_sd->has_lane_boundary_header(),
447-
osi_sd->lane_boundary_header(), osi_sd->lane_boundary(i_lb));
448-
}
449-
#endif
475+
// TODO(tobias): Process detected lanes once supported by Cloe data model.
450476

451-
#if 0
452-
// Detected lanes
453-
for (int i_l = 0; i_l < osi_sd->lane_size(); ++i_l) {
454-
this->process(osi_sd->has_lane_header(),
455-
osi_sd->lane_header(), osi_sd->lane(i_l));
456-
}
457-
#endif
477+
// TODO(tobias): Process detected traffic signs.
458478

459-
#if 0
460-
// Detected traffic lights
461-
for (int i_tl = 0; i_tl < osi_sd->traffic_light_size(); ++i_tl) {
462-
this->process(osi_sd->has_traffic_light_header(),
463-
osi_sd->traffic_light_header(), osi_sd->traffic_light(i_tl));
464-
}
465-
#endif
479+
// TODO(tobias): Process detected traffic lights once supported by Cloe data model.
466480

467481
store_sensor_meta_data(ground_truth_->get_veh_coord_sys_info(owner_id_),
468482
ground_truth_->get_mov_obj_dimensions(owner_id_));
@@ -509,7 +523,6 @@ void OsiOmniSensor::process(const bool has_veh_data, const osi3::HostVehicleData
509523
const osi3::MovingObject& osi_ego) {
510524
auto obj = std::make_shared<cloe::Object>();
511525
obj->exist_prob = 1.0;
512-
513526
// Object id
514527
from_osi_identifier(osi_ego.id(), obj->id);
515528
assert(obj->id == static_cast<int>(owner_id_));
@@ -530,6 +543,10 @@ void OsiOmniSensor::process(const bool has_veh_data, const osi3::HostVehicleData
530543
// - Offset to vehicle frame origin
531544
obj->cog_offset = ground_truth_->get_veh_coord_sys_info(obj->id);
532545

546+
// Store ego pose.
547+
osi_ego_pose_ = obj->pose;
548+
osi_ego_pose_.translation() = obj->pose.translation() + obj->pose.rotation() * obj->cog_offset;
549+
533550
// Object attributes are all set:
534551
// - 1a) osi3::HostVehicleData: "All coordinates and orientations are relative
535552
// to the global ground truth coordinate system."
@@ -560,17 +577,18 @@ void OsiOmniSensor::process(const bool has_eh, const osi3::DetectedEntityHeader&
560577
"VtdOsiSensor: DetectedEntityHeader not yet handled. measurement_time = {}ns",
561578
osi_timestamp_to_simtime(osi_eh.measurement_time()).count());
562579
}
563-
switch (this->get_mock_level(MockTarget::DetectedMovingObject)) {
564-
case MockLevel::Zero: {
580+
switch (this->get_mock_level(SensorMockTarget::DetectedMovingObject)) {
581+
case SensorMockLevel::OverwriteNone: {
565582
from_osi_detected_moving_object(osi_mo, *obj);
566583
break;
567584
}
568-
case MockLevel::MissingData: {
585+
case SensorMockLevel::InterpolateMissing: {
569586
from_osi_detected_moving_object_alt(osi_mo, *ground_truth_, *obj);
570587
break;
571588
}
572-
case MockLevel::All: {
573-
throw cloe::ModelError("OSI MockLevel All not available for DetectedMovingObject");
589+
case SensorMockLevel::OverwriteAll: {
590+
throw cloe::ModelError(
591+
"OSI SensorMockLevel::OverwriteAll not available for DetectedMovingObject");
574592
break;
575593
}
576594
}
@@ -602,4 +620,57 @@ void OsiOmniSensor::process(const bool has_eh, const osi3::DetectedEntityHeader&
602620
store_object(obj);
603621
}
604622

623+
void OsiOmniSensor::from_osi_boundary_points(const osi3::LaneBoundary& osi_lb,
624+
cloe::LaneBoundary& lb) {
625+
assert(osi_lb.boundary_line_size() > 0);
626+
for (int i = 0; i < osi_lb.boundary_line_size(); ++i) {
627+
const auto& osi_pt = osi_lb.boundary_line(i);
628+
Eigen::Vector3d position = osi_vector3d_xyz_to_vector3d(osi_pt.position());
629+
// Transform points from the inertial into the sensor reference frame.
630+
cloe::utility::transform_to_child_frame(osi_ego_pose_, &position);
631+
cloe::utility::transform_to_child_frame(osi_sensor_pose_, &position);
632+
lb.points.push_back(position);
633+
}
634+
// Compute clothoid segment. TODO(tobias): implement curved segments.
635+
lb.dx_start = lb.points.front()(0);
636+
lb.dy_start = lb.points.front()(1);
637+
lb.heading_start = std::atan((lb.points.back()(1) - lb.points.front()(1)) /
638+
(lb.points.back()(0) - lb.points.front()(0)));
639+
lb.curv_hor_start = 0.0;
640+
lb.curv_hor_change = 0.0;
641+
lb.dx_end = lb.points.back()(0);
642+
}
643+
644+
void OsiOmniSensor::mock_detected_lane_boundaries() {
645+
const auto& osi_gt = ground_truth_->get_gt();
646+
int lb_id = 0;
647+
// If some of the OSI data does not have an id, avoid id clashes.
648+
for (const auto& osi_lb : osi_gt.lane_boundary()) {
649+
if (osi_lb.has_classification() && osi_lb.has_id()) {
650+
int id;
651+
from_osi_identifier(osi_lb.id(), id);
652+
lb_id = std::max(lb_id, id + 1);
653+
}
654+
}
655+
// Set lane boundary data.
656+
for (const auto& osi_lb : osi_gt.lane_boundary()) {
657+
if (osi_lb.has_classification()) {
658+
cloe::LaneBoundary lb;
659+
if (osi_lb.has_id()) {
660+
from_osi_identifier(osi_lb.id(), lb.id);
661+
} else {
662+
lb.id = lb_id;
663+
}
664+
lb.exist_prob = 1.0;
665+
lb.prev_id = -1; // no concatenated line segments for now
666+
lb.next_id = -1;
667+
++lb_id;
668+
from_osi_boundary_points(osi_lb, lb);
669+
lb.type = osi_lane_bdry_type_map.at(osi_lb.classification().type());
670+
lb.color = osi_lane_bdry_color_map.at(osi_lb.classification().color());
671+
store_lane_boundary(lb);
672+
}
673+
}
674+
}
675+
605676
} // namespace osii

0 commit comments

Comments
 (0)