Returns 2 coordinates for the baseline for 2d bbs in world coordinates (distance behind trigger volume, as seen from camera) Args: obstacle (Actor): obstacle with distance (int, optional): Distance behind trigger volume. Defaults to 2. Returns:
(self, obstacle, distance=2, cam='seg_front')
| 569 | return obst |
| 570 | |
| 571 | def _get_2d_bb_baseline(self, obstacle, distance=2, cam='seg_front'): |
| 572 | """Returns 2 coordinates for the baseline for 2d bbs in world coordinates |
| 573 | (distance behind trigger volume, as seen from camera) |
| 574 | |
| 575 | Args: |
| 576 | obstacle (Actor): obstacle with |
| 577 | distance (int, optional): Distance behind trigger volume. Defaults to 2. |
| 578 | |
| 579 | Returns: |
| 580 | np.ndarray: Baseline |
| 581 | """ |
| 582 | trigger = obstacle.trigger_volume |
| 583 | bb = self._create_2d_bb_points(trigger) |
| 584 | trig_loc_world = self._trig_to_world(bb, obstacle, trigger) |
| 585 | #self._draw_line(trig_loc_world[:,0], trig_loc_world[:,3], 0.7, color=(0, 255, 255)) |
| 586 | |
| 587 | |
| 588 | cords_x_y_z = np.array(self._world_to_sensor(trig_loc_world, self._get_sensor_position(cam))) |
| 589 | indices = (-cords_x_y_z[0]).argsort() |
| 590 | |
| 591 | # check crooked up boxes |
| 592 | if self._get_dist(cords_x_y_z[:,indices[0]],cords_x_y_z[:,indices[1]]) < self._get_dist(cords_x_y_z[:,indices[0]],cords_x_y_z[:,indices[2]]): |
| 593 | cords = cords_x_y_z[:, [indices[0],indices[2]]] + np.array([[distance],[0],[0],[0]]) |
| 594 | else: |
| 595 | cords = cords_x_y_z[:, [indices[0],indices[1]]] + np.array([[distance],[0],[0],[0]]) |
| 596 | |
| 597 | sensor_world_matrix = self.get_matrix(self._get_sensor_position(cam)) |
| 598 | baseline = np.dot(sensor_world_matrix, cords) |
| 599 | |
| 600 | return baseline |
| 601 | |
| 602 | def _baseline_to_box(self, baseline, cam, height=1): |
| 603 | """Transforms a baseline (in world coords) into a 2d box (in sensor coords) |
no test coverage detected