search

ethz-asl / maplab

A Modular and Multi-Modal Mapping Framework
https://maplab.asl.ethz.ch
Apache License 2.0
2.64k stars 724 forks source link

Erorr Check failed: isValidRotationMatrix(matrix) #347

Closed Pan-long-ming closed 3 years ago

Pan-long-ming commented 3 years ago

I met this error,when i run "rosrun rovioli tutorial_thermal_live save_folder" using my own data,this is my calibration file

label: "tau2 - original_calibration" id: 412eab8e4058621f7036b5e765dfe812 cameras:

camera: label: /tau_nodelet id: 54812562fa109c40fe90b29a59dd7798 line-delay-nanoseconds: 0 image_height: 512 image_width: 640 type: pinhole intrinsics: cols: 1 rows: 4 data: [440.381678762, 440.34105427, 328.082910449, 257.335140534] distortion: type: radial-tangential parameters: cols: 1 rows: 4 data: [-0.125172452973,-0.0158508045529, 0.00552041769855, 1.00188160221577] T_B_C: cols: 4 rows: 4 data: [0.0207981429818, -0.9997828123456, 0.0013528123456, -0.019282391666587094, -0.3239881123456, -0.0080198123456, -0.9460272123465, -0.007020499072517088, 0.9458326123456, 0.0192373123456, -0.3240845123465, 0.13646235628270134, 0.0, 0.0, 0.0, 1.0] The error is "F1115 11:32:11.243661 18011 rotation-quaternion-inl.h:115] Check failed: isValidRotationMatrix(matrix) 000.0207981 00-0.999783 00.00135281 00-0.323988 -0.00801981 00-0.946027 0000.945833 000.0192373 00-0.324085"

Is there any problem in my calibration file or something wrong,I will very appreciate if you could give me some advise.

smauq commented 3 years ago

Rovioli is quite demanding on how precisely the rotation matrix has to be. This is because even small errors here would accumulate over time. An ideal rotation matrix has a determinant that is exactly 1.0. You have 1.00000017, but I think the threshold is somewhere around 1e-8. Anyways you can fix this with this little piece of python code:

import numpy as np
def gram_schmidt_columns(X):
    Q, R = np.linalg.qr(X)
    return Q

R = np.array([
0.0207981429818, -0.9997828123456, 0.0013528123456,
-0.3239881123456, -0.0080198123456, -0.9460272123465,
0.9458326123456, 0.0192373123456, -0.3240845123465], dtype=np.float64)
R = R.reshape((3, 3))

np.set_printoptions(precision=12)
print(gram_schmidt_columns(R))

This the output (it's just the rotation part, you need to add the translation back in)

[[-0.020798141003 -0.999782780121 -0.001352739376]
 [ 0.323988081523 -0.008019820071  0.946027169545]
 [-0.945832522364  0.01923733503   0.32408450222 ]]
Pan-long-ming commented 3 years ago

Thanks , I know that ,i