Skip to content

Commit

Permalink
modify yaw calculation
Browse files Browse the repository at this point in the history
  • Loading branch information
gaoyi committed Dec 15, 2023
1 parent 0cd1ecf commit 925a63c
Show file tree
Hide file tree
Showing 3 changed files with 71 additions and 55 deletions.
20 changes: 10 additions & 10 deletions Config/category2id_hashmap.txt
View file
Original file line number Diff line number Diff line change
@@ -1,31 +1,31 @@
crosswalk 56421536c7a368a0bacb9fd125c0be4d96e187b3cbfa264cde540025e0efc865 1
TFL_red ccbf596a4032cc173bd6bd58e84ece0021033216f0697216f90ffbb4a954a203 1
crosswalk 56421536c7a368a0bacb9fd125c0be4d96e187b3cbfa264cde540025e0efc865 1
car 2b2961a431b23c9007efe270c1d7eb79c19d4192d7cd2d924176eb0b19e7d2a1 1
truck 24611dec0a0a3ae6f9f3a63c27323a5615c029c22a8e472e2bc6ae3ca65a6293 2
stop_line 586e3a72234c910a8728987363317ee6f5f758e4e55d1d45e5d5571c4172d8ae 2
TFL_green 22b22910bc2dd34d0d9791e968fb63634bd13cccc3bf2c54b5510f87e1b97697 2
truck 24611dec0a0a3ae6f9f3a63c27323a5615c029c22a8e472e2bc6ae3ca65a6293 2
cyclelist eee7d058d1407d0b476963d43cd044209987be186fb28d5a703633bd55d43ed3 3
RoadArrow_straight bed2ffc1cc3cab4848a291fbbbe6953b611e10a53001f9f8ac7bbfdcea9de809 3
TFL_black 77e4925888286cf6c011cea16de72435f5d2197a596ab404e0709c3b045ac0f1 3
TFL_yellow 9abd69285c634186f124603ef566593a3d9bd6a52b29ed8e4102b0102ec43880 4
RoadArrow_straight bed2ffc1cc3cab4848a291fbbbe6953b611e10a53001f9f8ac7bbfdcea9de809 3
pedestrian 750cfe5c94c6b2f63d3b293bdf763cd678b99a0fada8cc984e5c046681b49057 4
TFL_yellow 9abd69285c634186f124603ef566593a3d9bd6a52b29ed8e4102b0102ec43880 4
RoadArrow_straight_right ac5d9bfe379d87fd2c56f3cf22190d16df532701368f207cb24a0a566ee511a7 4
tricycle 247a1cd4f4ac7b02c20b31fe458438cbdab6a13ff8b40fbaa6d2d7600bb4312d 5
RoadArrow_left b2158be770e88df8d8b6ebcc8ee35cb80d2ede6a21047f31a61e37fc2db4917c 5
RoadArrow_straight_left 9f04c3f993a2b1419d5cc428b121a2cd9276eb27e28e622365bc5e0289aaf301 6
bus 04e027e4990a203f4899f7e87c2d5ff6b9019e9565795619a59ce06c099560d4 6
RoadArrow_right a86a6c5680f213a6d8569f946fea835bfdc15e27111c5e1b86875d54b7fb8346 7
RoadArrow_straight_left 9f04c3f993a2b1419d5cc428b121a2cd9276eb27e28e622365bc5e0289aaf301 6
bicycle d29af8a2e160dd867fe45a75f70bf805b1b6c1cf92017cf53cdf1d5cf390c916 7
RoadArrow_left_bend 462e66e0caf46eac3b569cd7d0c27a117ea1905c9312e7cd6502576b8d911de8 8
RoadArrow_right a86a6c5680f213a6d8569f946fea835bfdc15e27111c5e1b86875d54b7fb8346 7
traffic 075f4ab854e2a33c8ac11c0758796af4ae994ec4e37e36b6f11b503492c68289 8
RoadArrow_left_right 00b24704bb8b83ff053fbd48b5a2dc5842616a6da3aebd075f321e84afbfbbfa 9
RoadArrow_left_bend 462e66e0caf46eac3b569cd7d0c27a117ea1905c9312e7cd6502576b8d911de8 8
vehicle b404ed3c370c8c264e53b1867929db93e94012bb618bb263f514e32fd9e5bc29 9
RoadArrow_left_u-turn 5e613ff31f74d5d33a9b23b580a4e8311963eab1d1f57e8274834d925441301c 10
RoadArrow_left_right 00b24704bb8b83ff053fbd48b5a2dc5842616a6da3aebd075f321e84afbfbbfa 9
construction 7a01adea4b8a1084a7aebd7f3256080c31a3013aa5ad7638add2136d6f3e5371 10
RoadArrow_left_u-turn 5e613ff31f74d5d33a9b23b580a4e8311963eab1d1f57e8274834d925441301c 10
RoadArrow_u-turn 5abf1241122624bfa94b4368f13116a02501a8380376690172967973ecf87307 11
others 01db91d06032cc64162c16f8e35725b0e632beebdde8b2c2459979d04fb1e20c 11
barrier f0dc3075f012ecffa9225a29f5dda815bb4aac0c0f97edfb894152c1e7e0ec1d 12
RoadArrow_straight_u-turn 4f8bc2abac43ed80480b0aa5eaa8aa1839139d091a8e7284e727bb5cee05d3b2 12
portable 01e782826ae5182220bd6158f883d01ceb1bce659dc020e7c511f802a9aa7737 13
RoadArrow_right_bend 459f311afb46616e48a9c80aab7ab80e91156c063a10ace3044d85a621f5a171 13
portable 01e782826ae5182220bd6158f883d01ceb1bce659dc020e7c511f802a9aa7737 13
bendy 69a4fc72339ed264135236864a2c20d74cc8e8a87bd199f8e41dfba8615c15fa 14
63 changes: 35 additions & 28 deletions Sources/Models/python_util.swift
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,28 +2,18 @@ import Foundation
import PythonKit

func pytest(frameItem: AxeraFrame.FrameItems,
FrontCameraMatrix: PythonObject,
FrontCameraMatrix: inout PythonObject,
fov_w: Int,
frontCamIntrinsics: PythonObject) -> [(x: Double, y: Double)]
frontCamIntrinsics: inout PythonObject,
np: inout PythonObject,
tan: inout PythonObject,
pi: inout PythonObject,
yaml _: inout PythonObject) -> [(x: Double, y: Double)]
{
let sys = Python.import("sys")
// print("Python Path \(sys.path)")
// print("Python \(sys.version_info.major).\(sys.version_info.minor)")
// print("Python Version: \(sys.version)")
// print("Python Encoding: \(sys.getdefaultencoding().upper())")
let np = Python.import("numpy")
let torch = Python.import("torch")
let Tensor = torch.Tensor
let cv2 = Python.import("cv2")
let Rotation = Python.import("scipy.spatial.transform").Rotation
let Quaternion = Python.import("pyquaternion.quaternion").Quaternion
let tan = Python.import("math").tan
let pi = Python.import("math").pi
let yaml = Python.import("yaml")
let Path = Python.import("pathlib").Path

// read yaml file
let FrontCameraMatrix_np = np.array(FrontCameraMatrix)
// x is front axis, y is left axis, z is up axis
// x, y, z, l, w, h, yaw
var bbox3d_for_cam = [
frameItem.position.x,
Expand All @@ -36,15 +26,16 @@ func pytest(frameItem: AxeraFrame.FrameItems,
]
let sensor2ego_rotation = FrontCameraMatrix_np[..<3, ..<3]
let sensor2ego_translation = FrontCameraMatrix_np[..<3, 3]

let cam_coord = np.dot(
np.linalg.inv(np.array(sensor2ego_rotation)),
np.array(bbox3d_for_cam)[..<3] - np.array(sensor2ego_translation)
)
for i in 0 ..< 3 {
for i in 0 ... 2 {
bbox3d_for_cam[i] = Double(cam_coord[i])!
}

// yaw to [-pi, pi]
// // yaw to [-pi, pi]
let nppi = Double(np.pi)!
var yaw = bbox3d_for_cam[6]
yaw = nppi * 1.5 - yaw
Expand All @@ -58,14 +49,18 @@ func pytest(frameItem: AxeraFrame.FrameItems,
}
}
assert(yaw > -nppi - 1e-4 && yaw < nppi + 1e-4, "yaw: \(yaw)")
// TODO：前视相机应该不需要 * -1
yaw = -1 * yaw
bbox3d_for_cam[6] = yaw

// x is front axis, y is left axis, z is up axis
let x = bbox3d_for_cam[0]
let y = bbox3d_for_cam[1]
let z = bbox3d_for_cam[2]
let l = bbox3d_for_cam[3]
let w = bbox3d_for_cam[4]
let h = bbox3d_for_cam[5]
let depth = bbox3d_for_cam[0]
// let depth = bbox3d_for_cam[0]
// calculate 8 corners
let halfLength = l / 2.0
let halfWidth = w / 2.0
Expand All @@ -85,30 +80,42 @@ func pytest(frameItem: AxeraFrame.FrameItems,

// Translate corners to the actual position using the center coordinates
// var corners = [(Double, Double, Double)]()
var rotatedCorners: [(x: Double, y: Double, z: Double)] = []
var corners: [(x: Double, y: Double, z: Double)] = []
for corner in cornersRelativeToCenter {
let rotatedCorner = (
x: corner.0 * cos(yaw) - corner.1 * sin(yaw),
z: corner.0 * sin(yaw) + corner.1 * cos(yaw),
y: corner.2
)
rotatedCorners.append(rotatedCorner)
}

for corner in rotatedCorners {
let translatedCorner = (
x: corner.0 + x,
y: corner.1 + y,
z: corner.2 + z
x: corner.x + x,
y: corner.y + y,
z: corner.z + z
)
corners.append(translatedCorner)
}

// clip
let clipDepth = abs(depth) / Double(tan(Double(fov_w) / 2.0 / 180.0 * Double(pi)!))! // Replace with your actual clip depth
let clipDepth = abs(bbox3d_for_cam[0]) / Double(tan(Double(fov_w) / 2.0 / 180.0 * Double(pi)!))!
// let clipDepth = 0.0

// Check if any corner's x is greater than the clip depth
let shouldClip = corners.contains { $0.x > clipDepth }
let shouldClip = corners.contains { $0.z > clipDepth }

if shouldClip {
// At least one corner has x greater than clip depth, so update all corners
for i in 0 ..< corners.count {
corners[i].x = max(clipDepth, corners[i].x)
corners[i].z = max(clipDepth, corners[i].z)
}
} else {
return [(x: Double, y: Double)]()
}

// Camera intrinsic parameters
let fx = Double(frontCamIntrinsics["fx"])!
let fy = Double(frontCamIntrinsics["fy"])!
Expand All @@ -120,8 +127,8 @@ func pytest(frameItem: AxeraFrame.FrameItems,

for corner in corners {
// Apply the perspective projection formula
let x2D = (fx * corner.x) / corner.z + cx
let y2D = (fy * corner.y) / corner.z + cy
let x2D = (fx * corner.x / corner.z) + cx
let y2D = (fy * corner.y / corner.z) + cy

// Append the projected 2D point to the corners2D array
corners2D.append((x: x2D, y: y2D))
Expand Down
43 changes: 26 additions & 17 deletions Sources/cli.swift
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,21 @@ struct SwiftCOCO: ParsableCommand {
print("Error creating parent directory for output: \(error)")
}

var np = Python.import("numpy")
var tan = Python.import("math").tan
var pi = Python.import("math").pi
var yaml = Python.import("yaml")
var FrontCameraMatrix = PythonObject([])
var frontCamIntrinsics = PythonObject([])
let cameraYaml = try! String(contentsOf: URL(fileURLWithPath: cameraYamlPath))
let cameraYamlDict = try! yaml.safe_load(cameraYaml)
for camera in cameraYamlDict["camera"] {
let camera_config = camera["camera_config"]
if camera_config["topic"] == "/camera/XFV/FRONT/compressed_image" {
FrontCameraMatrix = camera_config["tovcs"]
frontCamIntrinsics = camera_config["intrinsics"]
}
}
// load category2id_hashmap
var category2id_hashmap: [String: (String, Int)] = [:]
if let category2id_hashmapData = try? String(contentsOf: category2id_hashmapURL) {
Expand All @@ -76,14 +91,16 @@ struct SwiftCOCO: ParsableCommand {
// TODO: better logic to assign json annotion to xfv/xpilot
let imageURL = jsonsURL.lastPathComponent.contains("WIDE") ? "FRONT_WIDE_rect" : "FRONT_rect"
print("Processing item \(jsonFileCount + 1) of total \(jsonsURLs.count) given json source")
let jsons: [URL]
var jsons: [URL]
do {
jsons = try FileManager.default.contentsOfDirectory(at: jsonsURL, includingPropertiesForKeys: nil, options: .skipsSubdirectoryDescendants)
} catch {
print("\(error) when loading \(jsonsURL)")
fatalError("Error loading data")
}

// // take only the first 100
// jsons = Array(jsons[0..<1000])
let total = jsons.count
for (index, cur_json) in jsons.enumerated() {
let progress = Float(index + 1) / Float(total)
Expand All @@ -103,6 +120,7 @@ struct SwiftCOCO: ParsableCommand {
// acutally all axera_img_ann.frames has only one inside
for frame in axera_img_anno.frames {
if scalingType == "3D" {
// read yaml file
for frameItem in frame.items! {
let labelsObj = frameItem.labelsObj
if labelsObj == nil {
Expand All @@ -111,23 +129,14 @@ struct SwiftCOCO: ParsableCommand {
if labelsObj!.visibility == "0% - 30%" {
continue
}
// read yaml file
var FrontCameraMatrix = PythonObject([])
var frontCamIntrinsics = PythonObject([])
let yaml = Python.import("yaml")
let cameraYaml = try! String(contentsOf: URL(fileURLWithPath: cameraYamlPath))
let cameraYamlDict = try! yaml.safe_load(cameraYaml)
for camera in cameraYamlDict["camera"] {
let camera_config = camera["camera_config"]
if camera_config["topic"] == "/camera/XFV/FRONT/compressed_image" {
FrontCameraMatrix = camera_config["tovcs"]
frontCamIntrinsics = camera_config["intrinsics"]
}
}
let bbox2d_8p = pytest(frameItem: frameItem,
FrontCameraMatrix: FrontCameraMatrix,
FrontCameraMatrix: &FrontCameraMatrix,
fov_w: 100,
frontCamIntrinsics: frontCamIntrinsics)
frontCamIntrinsics: &frontCamIntrinsics,
np: &np,
tan: &tan,
pi: &pi,
yaml: &yaml)

if bbox2d_8p.count == 0 {
continue
Expand Down Expand Up @@ -198,7 +207,7 @@ struct SwiftCOCO: ParsableCommand {
curSeg.append(cur_box_x_min)
curSeg.append(cur_box_y_min)
curSeg.append(cur_box_x_max)
curSeg.append(cur_box_y_max)
curSeg.append(cur_box_y_min)
curSeg.append(cur_box_x_max)
curSeg.append(cur_box_y_max)
curSeg.append(cur_box_x_min)
Expand Down

0 comments on commit 925a63c

Please sign in to comment.