Skip to content

Commit 8d5ded1

Browse files
rty727433198rty727433198
committed
move l2v to cameras.yaml
1 parent 30e5528 commit 8d5ded1

3 files changed

Lines changed: 69 additions & 75 deletions

File tree

‎GEMstack/knowledge/calibration/cameras.yaml‎

Lines changed: 32 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -1,25 +1,35 @@
11
cameras:
2-
front:
3-
K:
4-
- [684.83331299, 0.0, 573.37109375]
5-
- [0.0, 684.60968018, 363.70092773]
6-
- [0.0, 0.0, 1.0]
7-
D: [0.0, 0.0, 0.0, 0.0, 0.0]
8-
T_l2c:
9-
- [ 0.0289748006, -0.999580136, 0.0000368439, -0.0307300513]
10-
- [-0.0094993062, -0.0003122155, -0.999954834, -0.386689354 ]
11-
- [ 0.999534999, 0.0289731321, -0.0095043721, -0.671425124 ]
12-
- [ 0.0, 0.0, 0.0, 1.0 ]
2+
front:
3+
K:
4+
- [684.83331299, 0.0, 573.37109375]
5+
- [0.0, 684.60968018, 363.70092773]
6+
- [0.0, 0.0, 1.0]
7+
D: [0.0, 0.0, 0.0, 0.0, 0.0]
8+
T_l2c:
9+
- [ 0.0289748006, -0.999580136, 0.0000368439, -0.0307300513]
10+
- [-0.0094993062, -0.0003122155, -0.999954834, -0.386689354 ]
11+
- [ 0.999534999, 0.0289731321, -0.0095043721, -0.671425124 ]
12+
- [ 0.0, 0.0, 0.0, 1.0 ]
13+
T_l2v:
14+
- [ 0.99939639, 0.02547917, 0.023615, 1.1 ]
15+
- [ -0.02530848, 0.99965156, -0.00749882, 0.03773583 ]
16+
- [ -0.02379784, 0.00689664, 0.999693, 1.95320223 ]
17+
- [ 0.0, 0.0, 0.0, 1.0 ]
1318

14-
front_right:
15-
K:
16-
- [1176.25545, 0.0, 966.432645]
17-
- [0.0, 1175.14569, 608.580326]
18-
- [0.0, 0.0, 1.0 ]
19-
D: [-0.270136325, 0.164393255, -0.00160720782, -0.0000741246708, -0.0619939758]
20-
T_l2c:
21-
- [-0.71836368, -0.69527204, -0.02346088, 0.05718003]
22-
- [-0.09720448, 0.13371206, -0.98624154, -0.15983010]
23-
- [ 0.68884317, -0.70619960, -0.16363744, -1.04767285]
24-
- [ 0.0, 0.0, 0.0, 1.0 ]
19+
front_right:
20+
K:
21+
- [1176.25545, 0.0, 966.432645]
22+
- [0.0, 1175.14569, 608.580326]
23+
- [0.0, 0.0, 1.0 ]
24+
D: [-0.270136325, 0.164393255, -0.00160720782, -0.0000741246708, -0.0619939758]
25+
T_l2c:
26+
- [-0.71836368, -0.69527204, -0.02346088, 0.05718003]
27+
- [-0.09720448, 0.13371206, -0.98624154, -0.15983010]
28+
- [ 0.68884317, -0.70619960, -0.16363744, -1.04767285]
29+
- [ 0.0, 0.0, 0.0, 1.0 ]
30+
T_l2v:
31+
- [0.99939639, 0.02547917, 0.023615, 1.1]
32+
- [-0.02530848, 0.99965156, -0.00749882, 0.03773583]
33+
- [-0.02379784, 0.00689664, 0.999693, 1.95320223]
34+
- [0.0, 0.0, 0.0, 1.0 ]
2535

‎GEMstack/onboard/perception/cone_detection.py‎

Lines changed: 37 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
1-
from ...state import AllState, VehicleState, ObjectPose, ObjectFrameEnum, ObstacleState, ObstacleMaterialEnum, ObstacleStateEnum
1+
from ...state import AllState, VehicleState, ObjectPose, ObjectFrameEnum, ObstacleState, ObstacleMaterialEnum, \
2+
ObstacleStateEnum
23
from ..interface.gem import GEMInterface
34
from ..component import Component
45
from .perception_utils import *
@@ -29,50 +30,38 @@ class ConeDetector3D(Component):
2930
"""
3031

3132
def __init__(
32-
self,
33-
vehicle_interface: GEMInterface,
34-
camera_name: str,
35-
camera_calib_file: str,
36-
enable_tracking: bool = True,
37-
visualize_2d: bool = False,
38-
use_cyl_roi: bool = False,
39-
T_l2v: list = None,
40-
save_data: bool = True,
41-
orientation: bool = True,
42-
use_start_frame: bool = True,
43-
**kwargs
33+
self,
34+
vehicle_interface: GEMInterface,
35+
camera_name: str,
36+
camera_calib_file: str,
37+
enable_tracking: bool = True,
38+
visualize_2d: bool = False,
39+
use_cyl_roi: bool = False,
40+
save_data: bool = True,
41+
orientation: bool = True,
42+
use_start_frame: bool = True,
43+
**kwargs
4444
):
4545
# Core interfaces and state
46-
self.vehicle_interface = vehicle_interface
47-
self.current_obstacles = {}
48-
self.tracked_obstacles = {}
49-
self.cone_counter = 0
50-
self.latest_image = None
51-
self.latest_lidar = None
52-
self.bridge = CvBridge()
53-
self.start_pose_abs = None
54-
self.start_time = None
46+
self.vehicle_interface = vehicle_interface
47+
self.current_obstacles = {}
48+
self.tracked_obstacles = {}
49+
self.cone_counter = 0
50+
self.latest_image = None
51+
self.latest_lidar = None
52+
self.bridge = CvBridge()
53+
self.start_pose_abs = None
54+
self.start_time = None
5555

5656
# Config flags
57-
self.camera_name = camera_name
57+
self.camera_name = camera_name
5858
self.enable_tracking = enable_tracking
59-
self.visualize_2d = visualize_2d
60-
self.use_cyl_roi = use_cyl_roi
61-
self.save_data = save_data
62-
self.orientation = orientation
59+
self.visualize_2d = visualize_2d
60+
self.use_cyl_roi = use_cyl_roi
61+
self.save_data = save_data
62+
self.orientation = orientation
6363
self.use_start_frame = use_start_frame
6464

65-
# 1) Load lidar→vehicle transform
66-
if T_l2v is not None:
67-
self.T_l2v = np.array(T_l2v)
68-
else:
69-
self.T_l2v = np.array([
70-
[0.99939639, 0.02547917, 0.023615, 1.1],
71-
[-0.02530848, 0.99965156, -0.00749882, 0.03773583],
72-
[-0.02379784, 0.00689664, 0.999693, 1.95320223],
73-
[0.0, 0.0, 0.0, 1.0]
74-
])
75-
7665
# 2) Load camera intrinsics/extrinsics from the supplied YAML
7766
with open(camera_calib_file, 'r') as f:
7867
calib = yaml.safe_load(f)
@@ -84,15 +73,16 @@ def __init__(
8473
# D: [...]
8574
# T_l2c: [[...], ..., [...]]
8675
cam_cfg = calib['cameras'][camera_name]
87-
self.K = np.array(cam_cfg['K'])
88-
self.D = np.array(cam_cfg['D'])
76+
self.K = np.array(cam_cfg['K'])
77+
self.D = np.array(cam_cfg['D'])
8978
self.T_l2c = np.array(cam_cfg['T_l2c'])
79+
self.T_l2v = np.array(cam_cfg['T_l2v'])
9080

9181
# Derived transforms
9282

9383
self.undistort_map1 = None
9484
self.undistort_map2 = None
95-
self.camera_front = (camera_name=='front')
85+
self.camera_front = (camera_name == 'front')
9686

9787
def rate(self) -> float:
9888
return 8
@@ -126,8 +116,8 @@ def initialize(self):
126116
# Initialize the YOLO detector
127117
self.detector = YOLO('GEMstack/knowledge/detection/cone.pt')
128118
self.detector.to('cuda')
129-
self.T_c2l = np.linalg.inv(self.T_l2c)
130-
self.R_c2l = self.T_c2l[:3, :3]
119+
self.T_c2l = np.linalg.inv(self.T_l2c)
120+
self.R_c2l = self.T_c2l[:3, :3]
131121
self.camera_origin_in_lidar = self.T_c2l[:3, 3]
132122

133123
def synchronized_callback(self, image_msg, lidar_msg):
@@ -162,7 +152,7 @@ def update(self, vehicle: VehicleState) -> Dict[str, ObstacleState]:
162152
if self.latest_image is None or self.latest_lidar is None:
163153
return {}
164154
latest_image = self.latest_image.copy()
165-
155+
166156
# Set up current time variables
167157
start = time.time()
168158
current_time = self.vehicle_interface.time()
@@ -179,7 +169,7 @@ def update(self, vehicle: VehicleState) -> Dict[str, ObstacleState]:
179169
# Ensure data/ exists and build timestamp
180170
if self.save_data:
181171
self.save_sensor_data(vehicle=vehicle, latest_image=latest_image)
182-
172+
183173
if self.camera_front == False:
184174
start = time.time()
185175
undistorted_img, current_K = self.undistort_image(latest_image, self.K, self.D)
@@ -508,8 +498,8 @@ def box_to_fake_obstacle(box):
508498
pose = ObjectPose(t=0, x=x + w / 2, y=y + h / 2, z=0, yaw=0, pitch=0, roll=0, frame=ObjectFrameEnum.CURRENT)
509499
dims = (w, h, 0)
510500
return ObstacleState(pose=pose, dimensions=dims, outline=None,
511-
type=ObstacleMaterialEnum.TRAFFIC_CONE, activity=ObstacleStateEnum.STANDING,
512-
velocity=(0, 0, 0), yaw_rate=0)
501+
type=ObstacleMaterialEnum.TRAFFIC_CONE, activity=ObstacleStateEnum.STANDING,
502+
velocity=(0, 0, 0), yaw_rate=0)
513503

514504

515505
if __name__ == '__main__':

‎launch/cone_detection.yaml‎

Lines changed: 0 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,6 @@ drive:
1717
args:
1818
camera_name: front #[front, front_right]
1919
camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml
20-
21-
T_l2v:
22-
- [0.99939639, 0.02547917, 0.023615, 1.1]
23-
- [-0.02530848, 0.99965156, -0.00749882, 0.03773583]
24-
- [-0.02379784, 0.00689664, 0.999693, 1.95320223]
25-
- [0.0, 0.0, 0.0, 1.0]
2620
enable_tracking: True # True if you want to enable tracking
2721
visualize_2d: False # True to see 2D detection visualization
2822
use_cyl_roi: False # True to use a cylinder ROI

0 commit comments

Comments
 (0)