error occurred when parse camera parameters

aau-cns / MSCEqF

A Multi State Constraint Equivariant Filter for Vision-aided Inertial Navigation

Other

84 stars 3 forks source link

error occurred when parse camera parameters #6

Closed yueyunqingwu closed 5 days ago

yueyunqingwu commented 1 month ago

Hi Alessandro, thank you for your great work! When parse camera parameters, an error occurs. The type of extrinsics_mat is Eigen::Matrix, while extrinsics is SE3, when extrinsics_mat assigns to extrinsics, raise an error, terminate called after throwing an instance of 'std::invalid_argument' what(): SO3: QuaternionType has to be normalized!, how can i fix it? My extrinsic of camera is T_imu_cam:

[-0.06171486, 0.0726234, 0.99541608, 0.96885831]
[-0.99809154, -0.00647916, -0.06140622, -0.16231786]
[0.00199083, -0.99733858, 0.07288502, -0.47493288]
[0.0, 0.0, 0.0, 1.0] Looking forward your replay.

AlessandroFornasier commented 1 month ago

Hi,

Apologies for the late reply.

this happens because the provide rotation does not have determinant equal to 1 so the converted quaternion is not normalized. Try out the following:

[-0.061715931798389, 0.072624699851919, 0.995448038188672, 0.96885831] [-0.998091770339611, -0.006479580322338, -0.061407108865235, -0.16231786] [0.001990412670045, -0.997338291659488, 0.072886008582841, -0.47493288] [0.0, 0.0, 0.0, 1.0]

Please, let me know if it works.

MarceloContreras commented 6 days ago

I think the program is to sensitive to determinant != 1. I am having the same issue even after normalizing the rotation part with its determinant and increasing the number of digits to have better precision.

AlessandroFornasier commented 5 days ago

Hi! The latest commit on Lie++ is meant to solve this issue and normalize a given rotation that is not normalized. Please re-configure the project to pull the latest Lie++ and re-build.