The point cloud annotation tool mainly involves two coordinate systems:
3D Point Cloud Coordinate System
The 3D point cloud tool uses a right-handed coordinate system. The coordinate system of the point cloud data is the world coordinate system, and the coordinate system of the point cloud is up with the Z axis (as shown in the figure below).
2D Image Coordinate System
The origin of the image in the 2D & 3D fusion tool mapping of the point cloud tool is in the upper left corner of the image. The horizontal represents the X axis. The vertical represents the Y axis.
The X value becomes larger to the right, and the Y value becomes larger downward (as shown in the figure below).
Camera parameters (for 2D & 3D fusion datasets)
The 2D & 3D mapping conversion process in the tool is basically the same as that of the general dataset, and the conversion parameters include camera extrinsic parameters and camera intrinsic parameters.
When building the camera parameters into the tool, the 4x3 or 3x3 matrix needs to be filled into a 4x4 matrix, as follows:
A 3D point x in Velodyne LiDAR coordinates gets projected to a point y in the i’th camera image as:
y = P(i) @ R(0) @ Tvelo_cam @ x
Code with Three.js
// Example: Calculate external matrix of camera 1
// camera 0 projection matrix as internal matrix
// 7.215377e+02 0.000000e+00 6.095593e+02 0.000000e+00
// 0.000000e+00 7.215377e+02 1.728540e+02 0.000000e+00
// 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
let P_rect_matrix_0 = createMatrixFrom4x3(config.cameras[0].P_rect);
// camera 1 projection matrix
// 7.215377e+02 0.000000e+00 6.095593e+02 -3.875744e+02
// 0.000000e+00 7.215377e+02 1.728540e+02 0.000000e+00
// 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
let P_rect_matrix_1 = createMatrixFrom4x3(config.cameras[1].P_rect);
// camera 0 rotate matrix
let R_rect_matrix_0 = createMatrixFrom3x3(config.R_rect);
// velodyne-camera transform
let RT_velo_to_cam_matrix = createMatrixFrom4x3(config.RT_velo_to_cam);
// external parameters
let external_matrix = new THREE.Matrix4().copy(P_rect_matrix_0).invert()
.multiply(P_rect_matrix_1)
.multiply(R_rect_matrix_0)
.multiply(RT_velo_to_cam_matrix);
// elements of THREE.Matrix4 is column-major, need to convert to row-major
external_matrix.transpose()
console.log(external_matrix.elements)
// [0.0002347736981472108,-0.9999441545437641,-0.010563477811052198,-0.5399474051919163,
// 0.010449407416592824,0.010565353641379319,-0.9998895741176488,-0.07510879138296463,
// 0.9999453885620024,0.00012436537838650657,0.010451302995668946,-0.2721327964058732,
// 0,0,0,1]
// internal parameters from camera 0 projection matrix
// fx: 7.215377e+02 fy: 7.215377e+02
// cx: 6.095593e+02 cy: 1.728540e+02
// tool function
// 3x3 convert to 4x4
function createMatrixFrom3x3(a: Array<number>): THREE.Matrix4 {
return new THREE.Matrix4().set(a[0], a[1], a[2], 0, a[3], a[4], a[5], 0, a[6], a[7], a[8], 0, 0, 0, 0, 1);
}
// 4x3 convert to 4x4
function createMatrixFrom4x3(a: Array<number>): THREE.Matrix4 {
return new THREE.Matrix4().set(a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7], a[8], a[9], a[10], a[11], 0, 0, 0, 1);
}
Code with Three.js
// poses parameter from poses.json
function createExternalMatrix(poses) {
let quaternion = new THREE.Quaternion(pos.heading.x, pos.heading.y, pos.heading.z, pos.heading.w);
let position = new THREE.Vector3(pos.position.x, pos.position.y, pos.position.z);
let external_matrix = new THREE.Matrix4();
external_matrix.compose(position, quaternion, new THREE.Vector3(1, 1, 1));
// elements of THREE.Matrix4 is column-major, need to convert to row-major
external_matrix.transpose()
return external_matrix;
}
// internal parameters from intrinsics.json
// { fx: 933.4667, fy: 934.6754, cx: 896.4692, cy: 507.3557 }
