Apollo 11.0
自动驾驶开放平台
trans_tools.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2020 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
18
19#include <limits>
20
21namespace apollo {
22namespace v2x {
23namespace ft {
24
25void Pb2Object(const PerceptionObstacle &obstacle, base::Object *object,
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;
34 switch (obstacle.sub_type()) {
36 object->sub_type = base::ObjectSubType::UNKNOWN;
37 variance = variance * 3;
38 break;
39
41 object->sub_type = base::ObjectSubType::UNKNOWN_MOVABLE;
42 variance = variance * 2;
43 break;
44
46 object->sub_type = base::ObjectSubType::CAR;
47 variance = variance * 0.8;
48 break;
49
51 object->sub_type = base::ObjectSubType::VAN;
52 variance = variance * 1;
53 break;
54
56 object->sub_type = base::ObjectSubType::TRUCK;
57 variance = variance * 3;
58 break;
59
61 object->sub_type = base::ObjectSubType::BUS;
62 variance = variance * 3;
63 break;
64
66 object->sub_type = base::ObjectSubType::CYCLIST;
67 variance = variance * 0.8;
68 break;
69
71 object->sub_type = base::ObjectSubType::MOTORCYCLIST;
72 variance = variance * 0.8;
73 break;
74
76 object->sub_type = base::ObjectSubType::TRICYCLIST;
77 variance = variance * 0.8;
78 break;
79
81 object->sub_type = base::ObjectSubType::PEDESTRIAN;
82 variance = variance * 0.8;
83 break;
84
86 object->sub_type = base::ObjectSubType::TRAFFICCONE;
87 variance = variance * 0.8;
88 break;
89
90 default:
91 break;
92 }
93 if (obstacle.has_timestamp()) {
94 object->timestamp = obstacle.timestamp();
95 } else {
96 object->timestamp = timestamp_object;
97 }
98 value << obstacle.position().x(), obstacle.position().y(), 0.0;
99 object->position.Set(value, variance);
100 value << obstacle.velocity().x(), obstacle.velocity().y(),
101 obstacle.velocity().z();
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();
108 value << obstacle.length(), obstacle.width(), obstacle.height();
109 object->size.Set(value, variance);
110 std::vector<base::Info3d> polygon_info3d;
111 for (auto &polygon_point : obstacle.polygon_point()) {
112 base::Info3d point;
113 value << polygon_point.x(), polygon_point.y(), polygon_point.z();
114 point.Set(value, variance);
115 polygon_info3d.push_back(point);
116 }
117 object->polygon = polygon_info3d;
118}
119
121 base::Object *object, const std::string &frame_id,
122 double timestamp_object) {
123 Pb2Object(obstacle.perception_obstacle(), object, frame_id, timestamp_object);
124 if (obstacle.has_v2x_info() && obstacle.v2x_info().v2x_type_size() > 0 &&
125 obstacle.v2x_info().v2x_type(0) ==
127 object->v2x_type = base::V2xType::ZOMBIES_CAR;
128 }
129}
130
132 const std::string &frame_id) {
133 base::Object object;
134 Eigen::Vector3d value;
135 Eigen::Matrix3d variance;
136 variance.setIdentity();
137 object.timestamp = obstacle.timestamp();
138 // object
139 value << obstacle.position().x(), obstacle.position().y(),
140 obstacle.position().z();
141
142 object.position.Set(value, variance);
143 value << obstacle.velocity().x(), obstacle.velocity().y(),
144 obstacle.velocity().z();
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;
150 value << obstacle.length(), obstacle.width(), obstacle.height();
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());
155 switch (obstacle.sub_type()) {
157 object.sub_type = base::ObjectSubType::UNKNOWN;
158 break;
159
161 object.sub_type = base::ObjectSubType::UNKNOWN_MOVABLE;
162 break;
163
165 object.sub_type = base::ObjectSubType::CAR;
166 break;
167
169 object.sub_type = base::ObjectSubType::VAN;
170 break;
171
173 object.sub_type = base::ObjectSubType::TRUCK;
174 break;
175
177 object.sub_type = base::ObjectSubType::BUS;
178 break;
179
181 object.sub_type = base::ObjectSubType::CYCLIST;
182 break;
183
185 object.sub_type = base::ObjectSubType::MOTORCYCLIST;
186 break;
187
189 object.sub_type = base::ObjectSubType::TRICYCLIST;
190 break;
191
193 object.sub_type = base::ObjectSubType::PEDESTRIAN;
194 break;
195
197 object.sub_type = base::ObjectSubType::TRAFFICCONE;
198 break;
199
200 default:
201 break;
202 }
203 return object;
204}
205
207 PerceptionObstacle obstacle;
208 // times
209 obstacle.set_timestamp(object.timestamp);
210 // id
211 obstacle.set_id(object.track_id);
212 // position
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());
216 // velocity
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());
220 // yaw
221 obstacle.set_theta(object.theta.Value());
222 // lwh
223 obstacle.set_length(object.size.length());
224 obstacle.set_width(object.size.width());
225 obstacle.set_height(object.size.height());
226 obstacle.set_type(static_cast<PerceptionObstacle::Type>(object.type));
227 switch (object.sub_type) {
228 case base::ObjectSubType::UNKNOWN:
229 obstacle.set_sub_type(PerceptionObstacle::ST_UNKNOWN);
230 break;
231
232 case base::ObjectSubType::UNKNOWN_MOVABLE:
233 obstacle.set_sub_type(PerceptionObstacle::ST_UNKNOWN_MOVABLE);
234 break;
235
236 case base::ObjectSubType::CAR:
237 obstacle.set_sub_type(PerceptionObstacle::ST_CAR);
238 break;
239
240 case base::ObjectSubType::VAN:
241 obstacle.set_sub_type(PerceptionObstacle::ST_VAN);
242 break;
243
244 case base::ObjectSubType::TRUCK:
245 obstacle.set_sub_type(PerceptionObstacle::ST_TRUCK);
246 break;
247
248 case base::ObjectSubType::BUS:
249 obstacle.set_sub_type(PerceptionObstacle::ST_BUS);
250 break;
251
252 case base::ObjectSubType::CYCLIST:
253 obstacle.set_sub_type(PerceptionObstacle::ST_CYCLIST);
254 break;
255
256 case base::ObjectSubType::MOTORCYCLIST:
257 obstacle.set_sub_type(PerceptionObstacle::ST_MOTORCYCLIST);
258 break;
259
260 case base::ObjectSubType::TRICYCLIST:
261 obstacle.set_sub_type(PerceptionObstacle::ST_TRICYCLIST);
262 break;
263
264 case base::ObjectSubType::PEDESTRIAN:
265 obstacle.set_sub_type(PerceptionObstacle::ST_PEDESTRIAN);
266 break;
267
268 case base::ObjectSubType::TRAFFICCONE:
269 obstacle.set_sub_type(PerceptionObstacle::ST_TRAFFICCONE);
270 break;
271
272 default:
273 break;
274 }
275 return obstacle;
276}
277
279 struct PolygoPoint {
280 double x = 0.0;
281 double y = 0.0;
282 double z = 0.0;
283 };
284
285 if (!object_ptr) {
286 return;
287 }
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);
312 }
313}
314
315void Object2Pb(const base::Object &object, PerceptionObstacle *obstacle) {
316 // times
317 obstacle->set_timestamp(object.timestamp);
318 // id
319 obstacle->set_id(object.track_id);
320 // position
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());
324 // velocity
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());
328 // yaw
329 obstacle->set_theta(object.theta.Value());
330 // lwh
331 obstacle->set_length(object.size.length());
332 obstacle->set_width(object.size.width());
333 obstacle->set_height(object.size.height());
335 obstacle->set_type(static_cast<PerceptionObstacle::Type>(object.type));
336 switch (object.sub_type) {
337 case base::ObjectSubType::UNKNOWN:
338 obstacle->set_sub_type(PerceptionObstacle::ST_UNKNOWN);
339 break;
340
341 case base::ObjectSubType::UNKNOWN_MOVABLE:
342 obstacle->set_sub_type(PerceptionObstacle::ST_UNKNOWN_MOVABLE);
343 break;
344
345 case base::ObjectSubType::UNKNOWN_UNMOVABLE:
346 obstacle->set_sub_type(PerceptionObstacle::ST_UNKNOWN_UNMOVABLE);
347 break;
348
349 case base::ObjectSubType::CAR:
350 obstacle->set_sub_type(PerceptionObstacle::ST_CAR);
351 break;
352
353 case base::ObjectSubType::VAN:
354 obstacle->set_sub_type(PerceptionObstacle::ST_VAN);
355 break;
356
357 case base::ObjectSubType::TRUCK:
358 obstacle->set_sub_type(PerceptionObstacle::ST_TRUCK);
359 break;
360
361 case base::ObjectSubType::BUS:
362 obstacle->set_sub_type(PerceptionObstacle::ST_BUS);
363 break;
364
365 case base::ObjectSubType::CYCLIST:
366 obstacle->set_sub_type(PerceptionObstacle::ST_CYCLIST);
367 break;
368
369 case base::ObjectSubType::MOTORCYCLIST:
370 obstacle->set_sub_type(PerceptionObstacle::ST_MOTORCYCLIST);
371 break;
372
373 case base::ObjectSubType::TRICYCLIST:
374 obstacle->set_sub_type(PerceptionObstacle::ST_TRICYCLIST);
375 break;
376
377 case base::ObjectSubType::PEDESTRIAN:
378 obstacle->set_sub_type(PerceptionObstacle::ST_PEDESTRIAN);
379 break;
380
381 case base::ObjectSubType::TRAFFICCONE:
382 obstacle->set_sub_type(PerceptionObstacle::ST_TRAFFICCONE);
383 break;
384
385 default:
386 break;
387 }
388 obstacle->set_source(PerceptionObstacle::HOST_VEHICLE);
389 if (object.v2x_type == base::V2xType::ZOMBIES_CAR) {
390 obstacle->mutable_v2x_info()->add_v2x_type(
392 obstacle->set_source(PerceptionObstacle::V2X);
393 }
394 if (object.v2x_type == base::V2xType::BLIND_ZONE) {
395 obstacle->mutable_v2x_info()->add_v2x_type(
397 obstacle->set_source(PerceptionObstacle::V2X);
398 }
399}
400
401void Object2V2xPb(const base::Object &object, V2XObstacle *obstacle) {
402 PerceptionObstacle perception_obstacle;
403 Object2Pb(object, &perception_obstacle);
404 obstacle->mutable_perception_obstacle()->CopyFrom(perception_obstacle);
405}
406
407double Pbs2Objects(const PerceptionObstacles &obstacles,
408 std::vector<base::Object> *objects,
409 const std::string &frame_id) {
410 double timestamp = std::numeric_limits<double>::max();
411 objects->clear();
412 double timestamp_object = 0.0;
413 if (obstacles.perception_obstacle_size() > 0 &&
414 obstacles.perception_obstacle(0).has_timestamp() == false) {
415 if (obstacles.header().has_camera_timestamp() &&
416 obstacles.header().camera_timestamp() > 10.0) {
417 timestamp_object = obstacles.header().camera_timestamp() / 1.0e9;
418 } else {
419 timestamp_object = obstacles.header().lidar_timestamp() / 1.0e9;
420 }
421 }
422 for (int j = 0; j < obstacles.perception_obstacle_size(); ++j) {
423 base::Object object;
424 Pb2Object(obstacles.perception_obstacle(j), &object, frame_id,
425 timestamp_object);
426 objects->push_back(object);
427 if (timestamp > object.timestamp) {
428 timestamp = object.timestamp;
429 }
430 }
431
432 return timestamp;
433}
434
436 base::Object *object, const std::string &frame_id) {
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;
444 object->v2x_type = base::V2xType::HOST_VEHICLE;
445 variance = variance * 0.8;
446 value << carstatus.pose().position().x(), carstatus.pose().position().y(),
447 0.0;
448 object->position.Set(value, variance);
449 value << carstatus.pose().linear_velocity().x(),
450 carstatus.pose().linear_velocity().y(),
451 carstatus.pose().linear_velocity().z();
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);
460 object->timestamp = carstatus.header().timestamp_sec();
461}
462
463double V2xPbs2Objects(const V2XObstacles &obstacles,
464 std::vector<base::Object> *objects,
465 const std::string &frame_id) {
466 double timestamp = std::numeric_limits<double>::max();
467 objects->clear();
468 double timestamp_object = 0.0;
469 if (obstacles.v2x_obstacle_size() > 0 &&
470 obstacles.v2x_obstacle(0).perception_obstacle().has_timestamp() ==
471 false) {
472 if (obstacles.header().has_camera_timestamp()) {
473 timestamp_object = obstacles.header().camera_timestamp() / 1000000000.0;
474 } else {
475 timestamp_object = obstacles.header().lidar_timestamp() / 1000000000.0;
476 }
477 }
478 for (int j = 0; j < obstacles.v2x_obstacle_size(); ++j) {
479 base::Object object;
480 V2xPb2Object(obstacles.v2x_obstacle(j), &object, frame_id,
481 timestamp_object);
482 objects->push_back(object);
483 if (timestamp > object.timestamp) {
484 timestamp = object.timestamp;
485 }
486 }
487
488 return timestamp;
489}
490
491void Objects2Pbs(const std::vector<base::Object> &objects,
492 std::shared_ptr<PerceptionObstacles> obstacles) {
493 obstacles->mutable_perception_obstacle()->Clear();
494 if (objects.size() < 1) {
495 return;
496 }
497 // obstacles->mutable_header()->set_frame_id(objects[0].frame_id);
498 for (const auto &object : objects) {
499 if (object.v2x_type == base::V2xType::HOST_VEHICLE) {
500 continue;
501 }
502 PerceptionObstacle obstacle;
503 Object2Pb(object, &obstacle);
504 obstacles->add_perception_obstacle()->CopyFrom(obstacle);
505 }
506}
507
508void Objects2V2xPbs(const std::vector<base::Object> &objects,
509 std::shared_ptr<V2XObstacles> obstacles) {
510 obstacles->mutable_v2x_obstacle()->Clear();
511 if (objects.size() < 1) {
512 return;
513 }
514 for (const auto &object : objects) {
515 if (object.v2x_type == base::V2xType::HOST_VEHICLE) {
516 continue;
517 }
518 V2XObstacle obstacle;
519 Object2V2xPb(object, &obstacle);
520 obstacles->add_v2x_obstacle()->CopyFrom(obstacle);
521 }
522}
523
524} // namespace ft
525} // namespace v2x
526} // namespace apollo
void Set(Val value, Var variance)
Definition v2x_object.h:77
PerceptionObstacle Object2Pb(const base::Object &object)
void Object2V2xPb(const base::Object &object, V2XObstacle *obstacle)
void Objects2V2xPbs(const std::vector< base::Object > &objects, std::shared_ptr< V2XObstacles > obstacles)
void FillObjectPolygonFromBBox3D(PerceptionObstacle *object_ptr)
void V2xPb2Object(const apollo::v2x::V2XObstacle &obstacle, base::Object *object, const std::string &frame_id, double timestamp_object)
double V2xPbs2Objects(const V2XObstacles &obstacles, std::vector< base::Object > *objects, const std::string &frame_id)
void CarstatusPb2Object(const LocalizationEstimate &carstatus, base::Object *object, const std::string &frame_id)
void Objects2Pbs(const std::vector< base::Object > &objects, std::shared_ptr< PerceptionObstacles > obstacles)
double Pbs2Objects(const PerceptionObstacles &obstacles, std::vector< base::Object > *objects, const std::string &frame_id)
void Pb2Object(const PerceptionObstacle &obstacle, base::Object *object, const std::string &frame_id, double timestamp_object)
class register implement
Definition arena_queue.h:37
optional double timestamp_sec
Definition header.proto:9
optional uint64 camera_timestamp
Definition header.proto:22
optional uint64 lidar_timestamp
Definition header.proto:19
optional apollo::localization::Pose pose
optional apollo::common::Header header
optional apollo::common::Point3D linear_velocity
Definition pose.proto:35
optional double heading
Definition pose.proto:48
optional apollo::common::PointENU position
Definition pose.proto:26
repeated apollo::common::Point3D polygon_point
optional apollo::common::Point3D velocity
optional apollo::common::Point3D position
optional apollo::perception::PerceptionObstacle perception_obstacle
optional V2XInformation v2x_info
optional apollo::common::Header header
repeated V2XObstacle v2x_obstacle