26 const std::string &frame_id,
double timestamp_object) {
27 Eigen::Vector3d value;
28 Eigen::Matrix3d variance;
29 variance.setIdentity();
30 object->type_probs.push_back(0.85);
31 object->sub_type_probs.push_back(0.85);
32 object->type =
static_cast<base::ObjectType
>(obstacle.
type());
33 variance = variance * 1;
36 object->sub_type = base::ObjectSubType::UNKNOWN;
37 variance = variance * 3;
41 object->sub_type = base::ObjectSubType::UNKNOWN_MOVABLE;
42 variance = variance * 2;
46 object->sub_type = base::ObjectSubType::CAR;
47 variance = variance * 0.8;
51 object->sub_type = base::ObjectSubType::VAN;
52 variance = variance * 1;
56 object->sub_type = base::ObjectSubType::TRUCK;
57 variance = variance * 3;
61 object->sub_type = base::ObjectSubType::BUS;
62 variance = variance * 3;
66 object->sub_type = base::ObjectSubType::CYCLIST;
67 variance = variance * 0.8;
71 object->sub_type = base::ObjectSubType::MOTORCYCLIST;
72 variance = variance * 0.8;
76 object->sub_type = base::ObjectSubType::TRICYCLIST;
77 variance = variance * 0.8;
81 object->sub_type = base::ObjectSubType::PEDESTRIAN;
82 variance = variance * 0.8;
86 object->sub_type = base::ObjectSubType::TRAFFICCONE;
87 variance = variance * 0.8;
93 if (obstacle.has_timestamp()) {
96 object->timestamp = timestamp_object;
99 object->position.Set(value, variance);
102 object->velocity.Set(value, variance);
103 object->theta.Set(obstacle.
theta(), 0.5);
104 object->sensor_type = base::SensorType::MONOCULAR_CAMERA;
105 object->track_id = obstacle.
id();
106 object->frame_id = frame_id;
107 variance.setIdentity();
109 object->size.Set(value, variance);
110 std::vector<base::Info3d> polygon_info3d;
113 value << polygon_point.
x(), polygon_point.y(), polygon_point.z();
114 point.
Set(value, variance);
115 polygon_info3d.push_back(point);
117 object->polygon = polygon_info3d;
132 const std::string &frame_id) {
134 Eigen::Vector3d value;
135 Eigen::Matrix3d variance;
136 variance.setIdentity();
142 object.position.Set(value, variance);
145 object.velocity.Set(value, variance);
146 object.theta.Set(obstacle.
theta(), 0.5);
147 object.sensor_type = base::SensorType::MONOCULAR_CAMERA;
148 object.track_id = obstacle.
id();
149 object.frame_id = frame_id;
151 object.size.Set(value, variance);
152 object.type_probs.push_back(0.85);
153 object.sub_type_probs.push_back(0.85);
154 object.type =
static_cast<base::ObjectType
>(obstacle.
type());
157 object.sub_type = base::ObjectSubType::UNKNOWN;
161 object.sub_type = base::ObjectSubType::UNKNOWN_MOVABLE;
165 object.sub_type = base::ObjectSubType::CAR;
169 object.sub_type = base::ObjectSubType::VAN;
173 object.sub_type = base::ObjectSubType::TRUCK;
177 object.sub_type = base::ObjectSubType::BUS;
181 object.sub_type = base::ObjectSubType::CYCLIST;
185 object.sub_type = base::ObjectSubType::MOTORCYCLIST;
189 object.sub_type = base::ObjectSubType::TRICYCLIST;
193 object.sub_type = base::ObjectSubType::PEDESTRIAN;
197 object.sub_type = base::ObjectSubType::TRAFFICCONE;
209 obstacle.set_timestamp(
object.timestamp);
211 obstacle.set_id(
object.track_id);
213 obstacle.mutable_position()->set_x(
object.position.x());
214 obstacle.mutable_position()->set_y(
object.position.y());
215 obstacle.mutable_position()->set_z(
object.position.z());
217 obstacle.mutable_velocity()->set_x(
object.velocity.x());
218 obstacle.mutable_velocity()->set_y(
object.velocity.y());
219 obstacle.mutable_velocity()->set_z(
object.velocity.z());
221 obstacle.set_theta(
object.theta.Value());
223 obstacle.set_length(
object.size.length());
224 obstacle.set_width(
object.size.width());
225 obstacle.set_height(
object.size.height());
227 switch (
object.sub_type) {
228 case base::ObjectSubType::UNKNOWN:
232 case base::ObjectSubType::UNKNOWN_MOVABLE:
236 case base::ObjectSubType::CAR:
240 case base::ObjectSubType::VAN:
244 case base::ObjectSubType::TRUCK:
248 case base::ObjectSubType::BUS:
252 case base::ObjectSubType::CYCLIST:
256 case base::ObjectSubType::MOTORCYCLIST:
260 case base::ObjectSubType::TRICYCLIST:
264 case base::ObjectSubType::PEDESTRIAN:
268 case base::ObjectSubType::TRAFFICCONE:
288 const double length = object_ptr->
length();
289 const double width = object_ptr->
width();
290 double hl = length / 2;
291 double hw = width / 2;
292 double cos_theta = std::cos(object_ptr->
theta());
293 double sin_theta = std::sin(object_ptr->
theta());
294 PolygoPoint polygon[4];
295 polygon[0].x = hl * cos_theta - hw * sin_theta + object_ptr->
position().
x();
296 polygon[0].y = hl * sin_theta + hw * cos_theta + object_ptr->
position().
y();
297 polygon[0].z = object_ptr->
position().
z();
298 polygon[1].x = hl * cos_theta + hw * sin_theta + object_ptr->
position().
x();
299 polygon[1].y = hl * sin_theta - hw * cos_theta + object_ptr->
position().
y();
300 polygon[1].z = object_ptr->
position().
z();
301 polygon[2].x = -hl * cos_theta + hw * sin_theta + object_ptr->
position().
x();
302 polygon[2].y = -hl * sin_theta - hw * cos_theta + object_ptr->
position().
y();
303 polygon[2].z = object_ptr->
position().
z();
304 polygon[3].x = -hl * cos_theta - hw * sin_theta + object_ptr->
position().
x();
305 polygon[3].y = -hl * sin_theta + hw * cos_theta + object_ptr->
position().
y();
306 polygon[3].z = object_ptr->
position().
z();
307 for (PolygoPoint point : polygon) {
308 auto polygon_point = object_ptr->add_polygon_point();
309 polygon_point->set_x(point.x);
310 polygon_point->set_y(point.y);
311 polygon_point->set_z(point.z);
317 obstacle->set_timestamp(
object.timestamp);
319 obstacle->set_id(
object.track_id);
321 obstacle->mutable_position()->set_x(
object.position.x());
322 obstacle->mutable_position()->set_y(
object.position.y());
323 obstacle->mutable_position()->set_z(
object.position.z());
325 obstacle->mutable_velocity()->set_x(
object.velocity.x());
326 obstacle->mutable_velocity()->set_y(
object.velocity.y());
327 obstacle->mutable_velocity()->set_z(
object.velocity.z());
329 obstacle->set_theta(
object.theta.Value());
331 obstacle->set_length(
object.size.length());
332 obstacle->set_width(
object.size.width());
333 obstacle->set_height(
object.size.height());
336 switch (
object.sub_type) {
337 case base::ObjectSubType::UNKNOWN:
341 case base::ObjectSubType::UNKNOWN_MOVABLE:
345 case base::ObjectSubType::UNKNOWN_UNMOVABLE:
349 case base::ObjectSubType::CAR:
353 case base::ObjectSubType::VAN:
357 case base::ObjectSubType::TRUCK:
361 case base::ObjectSubType::BUS:
365 case base::ObjectSubType::CYCLIST:
369 case base::ObjectSubType::MOTORCYCLIST:
373 case base::ObjectSubType::TRICYCLIST:
377 case base::ObjectSubType::PEDESTRIAN:
381 case base::ObjectSubType::TRAFFICCONE:
390 obstacle->mutable_v2x_info()->add_v2x_type(
395 obstacle->mutable_v2x_info()->add_v2x_type(
408 std::vector<base::Object> *objects,
409 const std::string &frame_id) {
410 double timestamp = std::numeric_limits<double>::max();
412 double timestamp_object = 0.0;
413 if (obstacles.perception_obstacle_size() > 0 &&
415 if (obstacles.
header().has_camera_timestamp() &&
422 for (
int j = 0; j < obstacles.perception_obstacle_size(); ++j) {
426 objects->push_back(
object);
427 if (timestamp >
object.timestamp) {
428 timestamp =
object.timestamp;
437 Eigen::Vector3d value;
438 Eigen::Matrix3d variance;
439 variance.setIdentity();
440 object->type_probs.push_back(0.85);
441 object->sub_type_probs.push_back(0.85);
442 object->type = base::ObjectType::VEHICLE;
443 object->sub_type = base::ObjectSubType::CAR;
445 variance = variance * 0.8;
448 object->position.Set(value, variance);
452 object->velocity.Set(value, variance);
453 object->theta.Set(carstatus.
pose().
heading(), 0.5);
454 object->sensor_type = base::SensorType::MONOCULAR_CAMERA;
455 object->track_id = 0;
456 object->frame_id = frame_id;
457 variance.setIdentity();
458 value << 5.02203, 2.13135, 2.17711;
459 object->size.Set(value, variance);
464 std::vector<base::Object> *objects,
465 const std::string &frame_id) {
466 double timestamp = std::numeric_limits<double>::max();
468 double timestamp_object = 0.0;
469 if (obstacles.v2x_obstacle_size() > 0 &&
472 if (obstacles.
header().has_camera_timestamp()) {
478 for (
int j = 0; j < obstacles.v2x_obstacle_size(); ++j) {
482 objects->push_back(
object);
483 if (timestamp >
object.timestamp) {
484 timestamp =
object.timestamp;