Topics created by afdrus

@afdrus

Sounds good, let me know if I can help in any way!

@afdrus

I believe that both logDepthBootstrap and logCameraHeightBootstrap are approaches to initialize the depth of new features, not only during initialization. However, the effect of these values is the greatest during initialization because the VIO state is not fully converged and it is difficult to triangulate the feature depths without a good VIO solution.

If VIO is fully initialized and tracking well, the effect of these values will be minimal because feature depth can be very quickly resolved even if there is a little motion (so in that case the initial depth of feature does not matter much).

However, if VIO is not working very well in a case of distant features, then initializing the feature depth to a larger value could help not only during initialization of VIO but also during normal tracking. The farther the features are, the more difficult it is to determine their depth without significant motion, so a good prior on the feature depth could help.

The logic behind using useLogCameraHeight is the following: if your vehicle has a camera pointing significantly down, lets say 45 degrees or even more, then a large part of majority of features that it sees will likely be on the floor during initialization. So if the camera is looking at a flat floor, the features on the floor will be arranged according to the flat plane geometry and if you know the camera height above the ground, which is easy to measure, that parameter is specified in logCameraHeightBootstrap (well log of that actual height). So, using this approach to initialize the feature depth, the depths will be initialized assuming the features in the image are on the ground plane, so the depth is calculated based on where the pixel appears in the image (it can be projected onto a ground plane).

If the camera is not looking sufficiently downward, an assumption that features are on the ground plane will not be very helpful, so it is better to just use an expected average feature depth during initialization.

In the past, we have pretty much always set useLogCameraHeight to true and logCameraHeightBootstrap according to the camera height above ground for best initialization results because majority of the vehicles have the tracking cameras pointing down at least 30 or 45 degrees. this helps during initialization because when the vehicle takes off, the feature motion is very significant during initial moments and knowing that the features are on the ground plane significantly helps with tracking during take-off. After take off, as I mentioned before, a good prior for feature depths is no longer needed.

Regarding using dual cameras, QVIO does not support using two cameras in one VIO instance. We are working on another solution that will support multiple cameras, but we are not ready to share the details yet, hopefully in about 2 months.

@Moderator Thank you for your reply. Yes, the hardware setup is fine (is a sentinel drone out of the box)

I did a very quick and dummy workaround to fix the problem, most of the changes are in geometry.c in the following snippet of code:

int geometry_calc_R_T_tag_in_local_frame(int64_t frame_timestamp_ns, rc_matrix_t R_tag_to_cam, rc_vector_t T_tag_wrt_cam, rc_matrix_t *R_tag_to_local, rc_vector_t *T_tag_wrt_local, char* input_pipe) { // these "correct" values are from the past aligning to the timestamp static rc_matrix_t correct_R_imu_to_vio = RC_MATRIX_INITIALIZER; static rc_vector_t correct_T_imu_wrt_vio = RC_VECTOR_INITIALIZER; int ret = rc_tf_ringbuf_get_tf_at_time(&vio_ringbuf, frame_timestamp_ns, &correct_R_imu_to_vio, &correct_T_imu_wrt_vio); // fail silently, on catastrophic failure the previous function would have // printed a message. If ret==-2 that's a soft failure meaning we just don't // have enough vio data yet, so also return silently. if (ret < 0) return -1; pthread_mutex_lock(&tf_mutex); // Read extrinsics parameters int n; vcc_extrinsic_t t[VCC_MAX_EXTRINSICS_IN_CONFIG]; vcc_extrinsic_t tmp; // now load in extrinsics if (vcc_read_extrinsic_conf_file(VCC_EXTRINSICS_PATH, t, &n, VCC_MAX_EXTRINSICS_IN_CONFIG)) { return -1; } // Initialize camera matrices rc_matrix_t R_cam_to_imu_tmp = RC_MATRIX_INITIALIZER; rc_matrix_t R_cam_to_body = RC_MATRIX_INITIALIZER; rc_vector_t T_cam_wrt_imu_tmp = RC_VECTOR_INITIALIZER; rc_vector_t T_cam_wrt_body = RC_VECTOR_INITIALIZER; rc_vector_zeros(&T_cam_wrt_body, 3); rc_vector_zeros(&T_cam_wrt_imu_tmp, 3); rc_matrix_identity(&R_cam_to_imu_tmp, 3); rc_matrix_identity(&R_cam_to_body, 3); printf("input_pipe: %s \n", input_pipe); if (strcmp(input_pipe, "stereo_front") == 0) { // Pick out IMU to Body. config.c already set this up for imu1 so just leave it as is if (vcc_find_extrinsic_in_array("body", "stereo_front_l", t, n, &tmp)) { fprintf(stderr, "ERROR: %s missing body to stereo_front_l, sticking with identity for now\n", VCC_EXTRINSICS_PATH); return -1; } rc_rotation_matrix_from_tait_bryan(tmp.RPY_parent_to_child[0] * DEG_TO_RAD, tmp.RPY_parent_to_child[1] * DEG_TO_RAD, tmp.RPY_parent_to_child[2] * DEG_TO_RAD, &R_cam_to_body); rc_vector_from_array(&T_cam_wrt_body, tmp.T_child_wrt_parent, 3); T_cam_wrt_imu_tmp.d[0] = T_cam_wrt_body.d[0] - T_imu_wrt_body.d[0]; T_cam_wrt_imu_tmp.d[1] = T_cam_wrt_body.d[1] - T_imu_wrt_body.d[1]; T_cam_wrt_imu_tmp.d[2] = T_cam_wrt_body.d[2] - T_imu_wrt_body.d[2]; rc_matrix_multiply(R_body_to_imu, R_cam_to_body, &R_cam_to_imu_tmp); printf("RPY: \n%5.2f %5.2f %5.2f\n\n", tmp.RPY_parent_to_child[0], tmp.RPY_parent_to_child[1],tmp.RPY_parent_to_child[2]); printf("R_cam_to_imu_tmp: \n%5.2f %5.2f %5.2f\n %5.2f %5.2f %5.2f\n %5.2f %5.2f %5.2f\n\n", R_cam_to_imu_tmp.d[0][0], R_cam_to_imu_tmp.d[0][1], R_cam_to_imu_tmp.d[0][2], R_cam_to_imu_tmp.d[1][0], R_cam_to_imu_tmp.d[1][1], R_cam_to_imu_tmp.d[1][2], R_cam_to_imu_tmp.d[2][0], R_cam_to_imu_tmp.d[2][1], R_cam_to_imu_tmp.d[2][2]); // calculate position of tag wrt local rc_matrix_times_col_vec(R_cam_to_imu_tmp, T_tag_wrt_cam, T_tag_wrt_local); rc_vector_sum_inplace(T_tag_wrt_local, T_cam_wrt_imu_tmp); rc_matrix_times_col_vec_inplace(correct_R_imu_to_vio, T_tag_wrt_local); rc_vector_sum_inplace(T_tag_wrt_local, correct_T_imu_wrt_vio); rc_matrix_times_col_vec_inplace(R_vio_to_local, T_tag_wrt_local); rc_vector_sum_inplace(T_tag_wrt_local, T_vio_ga_wrt_local); // calculate rotation tag to local rc_matrix_multiply(R_cam_to_imu_tmp, R_tag_to_cam, R_tag_to_local); rc_matrix_left_multiply_inplace(correct_R_imu_to_vio, R_tag_to_local); rc_matrix_left_multiply_inplace(R_vio_to_local, R_tag_to_local); } else if (strcmp(input_pipe, "stereo_rear") == 0) { // Pick out IMU to Body. config.c already set this up for imu1 so just leave it as is if (vcc_find_extrinsic_in_array("body", "stereo_rear_l", t, n, &tmp)) { fprintf(stderr, "ERROR: %s missing body to stereo_rear_l, sticking with identity for now\n", VCC_EXTRINSICS_PATH); return -1; } rc_rotation_matrix_from_tait_bryan(tmp.RPY_parent_to_child[0] * DEG_TO_RAD, tmp.RPY_parent_to_child[1] * DEG_TO_RAD, tmp.RPY_parent_to_child[2] * DEG_TO_RAD, &R_cam_to_body); rc_vector_from_array(&T_cam_wrt_body, tmp.T_child_wrt_parent, 3); T_cam_wrt_imu_tmp.d[0] = T_cam_wrt_body.d[0] - T_imu_wrt_body.d[0]; T_cam_wrt_imu_tmp.d[1] = T_cam_wrt_body.d[1] - T_imu_wrt_body.d[1]; T_cam_wrt_imu_tmp.d[2] = T_cam_wrt_body.d[2] - T_imu_wrt_body.d[2]; rc_matrix_multiply(R_body_to_imu, R_cam_to_body, &R_cam_to_imu_tmp); printf("R_cam_to_imu_tmp: \n%5.2f %5.2f %5.2f\n %5.2f %5.2f %5.2f\n %5.2f %5.2f %5.2f\n\n", R_cam_to_imu_tmp.d[0][0], R_cam_to_imu_tmp.d[0][1], R_cam_to_imu_tmp.d[0][2], R_cam_to_imu_tmp.d[1][0], R_cam_to_imu_tmp.d[1][1], R_cam_to_imu_tmp.d[1][2], R_cam_to_imu_tmp.d[2][0], R_cam_to_imu_tmp.d[2][1], R_cam_to_imu_tmp.d[2][2]); // calculate position of tag wrt local rc_matrix_times_col_vec(R_cam_to_imu_tmp, T_tag_wrt_cam, T_tag_wrt_local); rc_vector_sum_inplace(T_tag_wrt_local, T_cam_wrt_imu_tmp); rc_matrix_times_col_vec_inplace(correct_R_imu_to_vio, T_tag_wrt_local); rc_vector_sum_inplace(T_tag_wrt_local, correct_T_imu_wrt_vio); rc_matrix_times_col_vec_inplace(R_vio_to_local, T_tag_wrt_local); rc_vector_sum_inplace(T_tag_wrt_local, T_vio_ga_wrt_local); // calculate rotation tag to local rc_matrix_multiply(R_cam_to_imu_tmp, R_tag_to_cam, R_tag_to_local); rc_matrix_left_multiply_inplace(correct_R_imu_to_vio, R_tag_to_local); rc_matrix_left_multiply_inplace(R_vio_to_local, R_tag_to_local); } else if (strcmp(input_pipe, "tracking") == 0) { printf("R_cam_to_imu: \n%5.2f %5.2f %5.2f\n %5.2f %5.2f %5.2f\n %5.2f %5.2f %5.2f\n\n", R_cam_to_imu.d[0][0], R_cam_to_imu.d[0][1], R_cam_to_imu.d[0][2], R_cam_to_imu.d[1][0], R_cam_to_imu.d[1][1], R_cam_to_imu.d[1][2], R_cam_to_imu.d[2][0], R_cam_to_imu.d[2][1], R_cam_to_imu.d[2][2]); // calculate position of tag wrt local rc_matrix_times_col_vec(R_cam_to_imu, T_tag_wrt_cam, T_tag_wrt_local); rc_vector_sum_inplace(T_tag_wrt_local, T_cam_wrt_imu); rc_matrix_times_col_vec_inplace(correct_R_imu_to_vio, T_tag_wrt_local); rc_vector_sum_inplace(T_tag_wrt_local, correct_T_imu_wrt_vio); rc_matrix_times_col_vec_inplace(R_vio_to_local, T_tag_wrt_local); rc_vector_sum_inplace(T_tag_wrt_local, T_vio_ga_wrt_local); // calculate rotation tag to local rc_matrix_multiply(R_cam_to_imu, R_tag_to_cam, R_tag_to_local); rc_matrix_left_multiply_inplace(correct_R_imu_to_vio, R_tag_to_local); rc_matrix_left_multiply_inplace(R_vio_to_local, R_tag_to_local); } printf("\n__________________________________________________________________________\n"); pthread_mutex_unlock(&tf_mutex); return 0; }

@James-Strawson Thanks for the reply! Now I am facing a new issue though, after I swapped the camera ids the calibration of the extrinsics is asking me to move the checkboard in only three locations instead of four, as a result the reprojection error keeps being high, is this a bug?

Result of the new extrinsics:

Calibrating Extrinsics 4 frames will be processed R [-0.2178154138909582, 0.5612314553199155, 0.7984833742984259; 0.6516188344157804, -0.5254512170251917, 0.5470776116433248; 0.7266012249550162, 0.6394687420712871, -0.2512576124371861] T [-0.5933205685053882; -0.3699353785761611; 0.8991646361380521] Re-projection error reported by stereoCalibrate: 71.628572 Detected horizontal stereo pair Distance between cameras: 0.5933 Extrinsics Calibration Failed exceeded allowable max reprojection error: 0.5

voxl-camer-server with ids already swapped, the stereo in question is stereo_rear,

{ "version": 0.1, "cameras": [{ "name": "tracking", "enabled": true, "frame_rate": 30, "type": "ov7251", "camera_id": 2, "ae_desired_msv": 60, "ae_filter_alpha": 0.600000023841858, "ae_ignore_fraction": 0.20000000298023224, "ae_slope": 0.05000000074505806, "ae_exposure_period": 1, "ae_gain_period": 1 }, { "name": "hires", "enabled": true, "frame_rate": 30, "type": "imx214", "camera_id": 3, "preview_width": 640, "preview_height": 480, "snapshot_width": 3840, "snapshot_height": 2160 }, { "name": "stereo_front", "enabled": true, "frame_rate": 30, "type": "ov7251", "camera_id": 0, "camera_id_second": 1, "independent_exposure": false, "ae_desired_msv": 60, "ae_filter_alpha": 0.600000023841858, "ae_ignore_fraction": 0.20000000298023224, "ae_slope": 0.05000000074505806, "ae_exposure_period": 1, "ae_gain_period": 1 }, { "name": "stereo_rear", "enabled": true, "frame_rate": 30, "type": "ov7251", "camera_id": 4, "camera_id_second": 5, "independent_exposure": false, "ae_desired_msv": 60, "ae_filter_alpha": 0.600000023841858, "ae_ignore_fraction": 0.20000000298023224, "ae_slope": 0.05000000074505806, "ae_exposure_period": 1, "ae_gain_period": 1 }] }

Sorry for the inconvenience. We do screen for focus, this one may have slipped through. Please see attach focusing guidelines from our production team https://storage.googleapis.com/modalai_public/temp/MODALAI-Focusing-M0014-160823-043819.pdf

To conclude this post in the hope to help someone else who's having the same problem, I found this reply on another question in this forum which dates back to 2021. I guess the Relocalization still works only in Offboard and NOT in Mission. The same applies for an external flight controller e.g. in Ardupilot it works in Guided Mode and NOT in Auto.

@tom nevermind I found out the documentation here, and by reflashing the platform everything worked again! Thank you for the support!

@Moderator That makes way more sense, thank you for the clarification
As far as I could understand, there is no text saying that the .conf for the extrinsics is only used for the voxl-mapper on the modalai.docs, I think that is where my confusion came from

@afdrus My best guess is 2-4 weeks

Any ideas from the dev team?

@afdrus Those CP parameters right, I did it. I am using PX4 1.12

@afdrus the relocalization is external to the VIO EKF. You can trace this code path here: https://gitlab.com/voxl-public/voxl-sdk/services/voxl-vision-hub/-/blob/master/src/tag_manager.c#L110

In the end the issue was my laptop which was not powerful enough to support the graphics of the 3D visualization.

@afdrus instead of rebooting, you can restart the server with: systemctl restart voxl-camera-server