[Fix] fix wrong coordinate transform in sweep loading

[windhw]

Motivation

Fix wrong coordinate transform in sweep loading， the wrong transform caused bad input for model, especially when rotation is large

Modification

the sensor to lidar transform is
Y = X @ Rt + T

so the reverse transform should be:

X= (Y - T) @ R

[ashutoshsingh0223]

Are you sure @windhw ?

The R.T is already included in the lidar2sensor[:, 3:4] as can be seen here -

https://github.com/open-mmlab/mmdetection3d/blob/fe25f7a51d36e3702f961e198894580d83c4387b/tools/dataset_converters/update_infos_to_v2.py#L307-L308C19

[windhw]

Are you sure @windhw ?

The R.T is already included in the lidar2sensor[:, 3:4] as can be seen here -

https://github.com/open-mmlab/mmdetection3d/blob/fe25f7a51d36e3702f961e198894580d83c4387b/tools/dataset_converters/update_infos_to_v2.py#L307-L308C19

Yes, I am sure. My modification is not related to the same topic as the code you quoted. The matrix saved in info of v1 is sensor2lidar. In the code you quoted, this matrix is ​​converted to lidar2sensor through inverse transformation. But in fact, when transforming the point cloud of sweep, the required matrix is ​​still sensor2lidar, so the author performed another inverse transformation in the loading function. The modification I submitted is that the author has an error in the processing order of R and T in the inverse transformation operation in the loading function, and I have verified this error through visualization. I don't know if my explanation is clear. If you still have questions, we can continue to discuss.

[ashutoshsingh0223]

So just to be clear -

This is code to obtain senor2lidar(sweep to lidar). From here

 # obtain the RT from sensor to Top LiDAR
  # sweep->ego->global->ego'->lidar
  l2e_r_s_mat = Quaternion(l2e_r_s).rotation_matrix
  e2g_r_s_mat = Quaternion(e2g_r_s).rotation_matrix
  R = (l2e_r_s_mat.T @ e2g_r_s_mat.T) @ (
      np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T)
  T = (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ (
      np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T)
  T -= e2g_t @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T
                ) + l2e_t @ np.linalg.inv(l2e_r_mat).T
  sweep['sensor2lidar_rotation'] = R.T  # points @ R.T + T                         -------(transposed 1)
  sweep['sensor2lidar_translation'] = T

Now going to v2 and lidar2sensor, i.e. what I marked earlier

  lidar2sensor = np.eye(4)
  rot = ori_sweep['sensor2lidar_rotation']
  trans = ori_sweep['sensor2lidar_translation']
  lidar2sensor[:3, :3] = rot.T                                                                         -------(transposed 1)
  lidar2sensor[:3, 3:4] = -1 * np.matmul(rot.T, trans.reshape(3, 1))

so now lidar2sensor essentially has R for rotation component(notice two transpose ops) and -RT as translation compoment. Now I agree you need sensor2lidar i.e. (R.T and T) to transform points from sweeps to main lidar so that all the sweeps are in the same frame of reference.

So then I agree again that the transform should be
lidar_points = sweep_points * R.T + T

but in lidar2sensor we have R and -RT.

Do we agree here?

If yes then we can discuss further. or if there is any correction so far let me know.

[windhw]

In your post:
lidar_points = sweep_points * R.T + T
I think this is wrong.

actually, from this code:
R = (l2e_r_s_mat.T @ e2g_r_s_mat.T) @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T)

we can get that (we assume lidar_points is [Nx3], R is [3x3] and T is [3])：
lidar_points.T = R.T @ sweep_points.T + T.T
or
lidar_points = sweep_points @ R + T

i.e. we can use a 3x4 or 4x4 tranform matrix : [R.T, T.T] to denote the sensor2lidar transform

and lidar2sensor is the inverse transform of sensor2lidar, and can be denoted as [R, [email protected]]

in loading function, I need transform sensor(sweep) points to lidar,
so as described above, what I need is:
lidar_points = sweep_points @ R + T

but in the original code in mmdet3d, I'll get:
lidar_points = sweep_points @ R - ([email protected]).T
which is wrong

correct operation is：

lidar_points = (sweep_points - ([email protected]).T ) @ R = sweep_points @ R + T
in the above code. [email protected] is the translation part of lidar2sensor and R is the rotation part. Here we perform the inverse operation again.

I think you may misunderstand the meaning of R rotation in sensor2lidar variabl, and the comment in this code is misleading：

sweep['sensor2lidar_rotation'] = R.T # points @ R.T + T

You can deduce R and T yourself~

So just to be clear -

This is code to obtain senor2lidar(sweep to lidar). From here

 # obtain the RT from sensor to Top LiDAR
  # sweep->ego->global->ego'->lidar
  l2e_r_s_mat = Quaternion(l2e_r_s).rotation_matrix
  e2g_r_s_mat = Quaternion(e2g_r_s).rotation_matrix
  R = (l2e_r_s_mat.T @ e2g_r_s_mat.T) @ (
      np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T)
  T = (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ (
      np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T)
  T -= e2g_t @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T
                ) + l2e_t @ np.linalg.inv(l2e_r_mat).T
  sweep['sensor2lidar_rotation'] = R.T  # points @ R.T + T                         -------(transposed 1)
  sweep['sensor2lidar_translation'] = T

Now going to v2 and lidar2sensor, i.e. what I marked earlier

  lidar2sensor = np.eye(4)
  rot = ori_sweep['sensor2lidar_rotation']
  trans = ori_sweep['sensor2lidar_translation']
  lidar2sensor[:3, :3] = rot.T                                                                         -------(transposed 1)
  lidar2sensor[:3, 3:4] = -1 * np.matmul(rot.T, trans.reshape(3, 1))

so now lidar2sensor essentially has R for rotation component(notice two transpose ops) and -RT as translation compoment. Now I agree you need sensor2lidar i.e. (R.T and T) to transform points from sweeps to main lidar so that all the sweeps are in the same frame of reference.

So then I agree again that the transform should be lidar_points = sweep_points * R.T + T

but in lidar2sensor we have R and -RT.

Do we agree here?

If yes then we can discuss further. or if there is any correction so far let me know.

So just to be clear -

This is code to obtain senor2lidar(sweep to lidar). From here

 # obtain the RT from sensor to Top LiDAR
  # sweep->ego->global->ego'->lidar
  l2e_r_s_mat = Quaternion(l2e_r_s).rotation_matrix
  e2g_r_s_mat = Quaternion(e2g_r_s).rotation_matrix
  R = (l2e_r_s_mat.T @ e2g_r_s_mat.T) @ (
      np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T)
  T = (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ (
      np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T)
  T -= e2g_t @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T
                ) + l2e_t @ np.linalg.inv(l2e_r_mat).T
  sweep['sensor2lidar_rotation'] = R.T  # points @ R.T + T                         -------(transposed 1)
  sweep['sensor2lidar_translation'] = T

Now going to v2 and lidar2sensor, i.e. what I marked earlier

  lidar2sensor = np.eye(4)
  rot = ori_sweep['sensor2lidar_rotation']
  trans = ori_sweep['sensor2lidar_translation']
  lidar2sensor[:3, :3] = rot.T                                                                         -------(transposed 1)
  lidar2sensor[:3, 3:4] = -1 * np.matmul(rot.T, trans.reshape(3, 1))

so now lidar2sensor essentially has R for rotation component(notice two transpose ops) and -RT as translation compoment. Now I agree you need sensor2lidar i.e. (R.T and T) to transform points from sweeps to main lidar so that all the sweeps are in the same frame of reference.

So then I agree again that the transform should be lidar_points = sweep_points * R.T + T

but in lidar2sensor we have R and -RT.

Do we agree here?

If yes then we can discuss further. or if there is any correction so far let me know.