[ VOXL ] Visual Odometry Function

Local Frame : 부팅시 드론의 포지션, 자세

Body Frame : 드론의 무게 중심 포지션, 자세

fixed Frame : Apriltag와 같이 고정된 사물 기준의 포지션, 자세

 

// copy out position & rotation, camera and imu relation, send into geometry module

geometry_update_imu_to_vio(d.timestamp_ns, d.T_imu_wrt_vio, d.R_imu_to_vio);   RPY, XYZ 를 IMU에서 VIO로 전달
geometry_update_cam_to_imu(d.T_cam_wrt_imu, d.R_cam_to_imu);  카메라의 RPY, XYZ를 IMU로 전달

 

 

 

geometry_get_R_body_to_local(&R_body_to_local);
geometry_get_T_body_wrt_local(&T_body_wrt_local);

 

rc_rotation_to_quaternion(R_body_to_local, &q_body_to_local);

 

geometry_rotate_vec_from_imu_to_body_frame(w_imu_wrt_imu, &w_body_wrt_body);

 

geometry_calc_velocity_in_body_frame(v_imu_wrt_vio, w_imu_wrt_imu, &v_body_wrt_body);

 

 

 

[ VOXL의 IMU ]

On VOXL, there is one ICM-20948 designated IMU1 (primary) and one MPU-9250 designated IMU0 (secondary).

The primary IMU is connected to BLSP1 and is accessible through the SDSP at /dev/spi-1.

The secondary IMU is connected to BLSP10 and is accessible through the SDSP at /dev/spi-10.

 

 

 

[ 카메라 좌표 기준 ]

SNAV and the ModalAI Vision Lib use the convention of X to the right, Y down, and Z out the front of the lens.

 

 

[ 카메라와 IMU의 상관관계 ]

 

TBC 값은 IMU1로 부터 카메라 프레임으로 전환되는 Transition 오프셋 XYZ값(m단위)

OMBC 값은 IMU1으로 부터 카메라 프레임으로 전환되는 Rotation 오프셋 RPY 값(m 단위)

Upside down 일 경우 Preset 값

IMU1 to tracking camera for VOXL-Cam with upside-down tracking <SnavMountDownward> <!-- Vector from the origin of the IMU frame to the origin of the camera frame represented in the IMU frame --> <param name="mv_vislam_tbc_1" value="0.02715"/>   0.013 <param name="mv_vislam_tbc_2" value="-0.0344"/>    0.020 <param name="mv_vislam_tbc_3" value="0.0088"/>     0.8 <!-- Axis-angle representation of the rotation of the camera frame relative to the IMU frame --> <param name="mv_vislam_ombc_1" value="-1.7599884"/>  x 축으로 -145도 이동 --> 2.53f <param name="mv_vislam_ombc_2" value="1.7599884f"/>  0 <param name="mv_vislam_ombc_3" value="-0.7290111f"/> 0

 

측정 방법

- Transition 오프셋은 IMU1 에서 X, Y, Z 축 방향으로 m단위의 오프셋을 측정해서 기록

- Rotation 오프셋은 원게 카메라의 전방이 하늘을 보고 있을 때가 IMU와 좌표가 일치되므로 Voxl Flightdeck의 경우는 X축으로 -145degree만큼 오프셋이 있음

 

이 측정방법이 맞다면 Red Tray의 Preset값이 맞다고 봄

<SnavMountDownward> <!-- Vector from the origin of the IMU1 frame to the origin of the camera frame represented in the IMU frame --> <param name="mv_vislam_tbc_1" value="0.0152"/> <param name="mv_vislam_tbc_2" value="0.0021"/> <param name="mv_vislam_tbc_3" value="0.0095"/> <!-- Axis-angle representation of the rotation of the camera frame relative to the IMU1 frame --> <param name="mv_vislam_ombc_1" value="-2.3561945f"/> <param name="mv_vislam_ombc_2" value="0.0f"/> <param name="mv_vislam_ombc_3" value="0.0f"/>