# Stereo tag detection: WARNING, apriltag roll/pitch out of bounds Source: https://forum.modalai.com/topic/2653/stereo-tag-detection-warning-apriltag-roll-pitch-out-of-bounds Category: General Questions (https://forum.modalai.com/category/2/general-questions) Posted: 2023-09-14 08:57:58 UTC by afdrus Replies: 13 · Views: 3226 ## afdrus · 2023-09-14 08:57:58 UTC Hello everyone, I am having issues on reading the apriltags flat on the ground from the stereo using a sentinel drone. I keep receiving ``` WARNING, apriltag roll/pitch out of bounds ``` and I noticed that the warning vanished and the tag correction is correctly computed only when I roll the drone by +90/-90 degrees. However, the only thing that I changed is the T_tag_wrt_fixed. Below you can find the config files in question -tag_locations ``` "locations": [{ "id": 0, "name": "proof0", "loc_type": "fixed", "size_m": 0.40000, "T_tag_wrt_fixed": [1.34, -5.61, -1.38], "R_tag_to_fixed": [[0, -1, 0], [1, 0, 0], [0, 0, 1]] }, ``` - voxl.-tag-detector: ``` "detector_1": { "enable": true, "input_pipe": "stereo_front", "en_fast_mode": true, "n_threads": 1, "en_undistortion": true, "undistort_scale": 0.899999976158142, "overlay_name": "stereo_tag_overlay", "lens_cal_file": "/data/modalai/opencv_stereo_front_intrinsics.yml", "skip_n_frames": 5 }, ``` - Extrinsics ``` { "name": "Sentinel_V1", "extrinsics": [{ "parent": "imu_apps", "child": "tracking", "T_child_wrt_parent": [0.0273, 0.0067, 0.0182], "RPY_parent_to_child": [0, 45, 90] }, { "parent": "body", "child": "imu_apps", "T_child_wrt_parent": [0.0583, -0.0067, -0.0360], "RPY_parent_to_child": [0, 0, 0] }, { "parent": "body", "child": "stereo_front_l", "T_child_wrt_parent": [0.0793, -0.040, -0.0243], "RPY_parent_to_child": [0, 90, 90] }, { "parent": "body", "child": "stereo_rear_l", "T_child_wrt_parent": [-0.0297, 0.04, 0.0415], "RPY_parent_to_child": [0, -90, -90] }, { "parent": "body", "child": "tof", "T_child_wrt_parent": [0.078, 0, -0.025], "RPY_parent_to_child": [0, 90, 90] }, { "parent": "body", "child": "ground", "T_child_wrt_parent": [0, 0, 0.058], "RPY_parent_to_child": [0, 0, 0] }] } ``` Why does the frame appear to be wrong even though I used the standard rotation parameters that come with the drone? ## Reply by afdrus · 2023-09-18 07:47:38 UTC Is there someone that can unravel this issue? ## Reply by afdrus · 2023-09-18 10:17:54 UTC (in reply to afdrus) The command `voxl-vision-px4 -m` detects properly the distances between camera and tag, as well as their orientation. Also `voxl-vision-px4 -l` shows the correct pose of the tag in local frame, however the warning keeps being there. These results lead me to think that there seems to be an issue in the relation with the fixed frame, however if I use the same config files and I simply change the detector from any stereo pipe to tracking everything works fine and `WARNING, apriltag roll/pitch out of bounds` is gone. ## Reply by afdrus · 2023-09-20 07:24:33 UTC (in reply to afdrus) Sorry to insist, but I am not able to move forward with this issue, can anyone help? ## Reply by Moderator (ModalAI staff) · 2023-09-25 17:29:43 UTC Can you please help with which version of software you are using? voxl-vision-px4 has been deprecated. Have you tried with the latest? ## Reply by afdrus · 2023-09-26 08:12:20 UTC (in reply to Moderator) @Moderator Thank you for your reply, we are currently using voxl SDK 0.9.5. All the libraries that come with this SDK are up to date. I did not try with the latest version, but the bug should be easy to replicate with a stereo setup on a voxl2. ## Reply by afdrus · 2023-10-06 07:20:07 UTC (in reply to Moderator) @Moderator I just tried the same with SDK 1.0.0 and the result is the same, can't understand which transformation is wrong, could the parameters in the extrinscs.conf be involved? ## Reply by afdrus · 2023-10-30 15:32:18 UTC (in reply to afdrus) @Moderator Any updates on this issue? ## Reply by afdrus · 2023-11-27 09:08:55 UTC (in reply to afdrus) Nothing changed on v1.1.0 ## Reply by Moderator (ModalAI staff) · 2023-11-27 16:25:25 UTC (in reply to afdrus) @afdrus since the stereo sensors have different orientation than tracking, did you change R_tag_to_fixed ? You can find that error in the source code at this link below. That should help you debug https://gitlab.com/voxl-public/voxl-sdk/services/voxl-vision-hub/-/blob/master/src/geometry.c?ref_type=heads#L613 ## Reply by afdrus · 2023-11-28 08:44:53 UTC (in reply to Moderator) @Moderator Thank you for your response! I thought that, as the name of the transformation suggest, R_tag_to_fixed did not depend on the orientation of the camera but rather on the orientation between the tag and fixed frame. I would expect to set the camera orientation in the extrinsics.conf file. Hoever if I run `voxl-vision-hub --debug_tag_local` I get the following ``` ID 0 T_tag_wrt_local -4.74 2.63 0.89 Roll 1.45 Pitch 0.05 Yaw -2.14 WARNING, apriltag roll/pitch out of bounds ID 0 T_tag_wrt_local -4.74 2.63 0.89 Roll 1.39 Pitch 0.04 Yaw -2.16 WARNING, apriltag roll/pitch out of bounds ID 0 T_tag_wrt_local -4.73 2.63 0.89 Roll 1.32 Pitch 0.06 Yaw -2.08 ID 0 T_tag_wrt_local -4.74 2.63 0.89 Roll 1.40 Pitch 0.05 Yaw -2.13 ID 0 T_tag_wrt_local -4.74 2.64 0.89 Roll 1.40 Pitch 0.04 Yaw -2.14 ``` No matter what values I put in extrinsics.conf or in R_tag_to_fixed, the Roll keeps being around 1.4 radians ## Reply by Moderator (ModalAI staff) · 2023-11-28 18:02:44 UTC (in reply to afdrus) @afdrus OK, you could be right. Hopefully the source code helps? We don't use it in this way, so you may need to modify ## Reply by afdrus · 2023-12-05 11:15:54 UTC (in reply to Moderator) @Moderator Thanks to some prints I notice that the issue comes from `R_cam_to_imu` which always (and only) picks up the parameters from the tracking camera in extrinsics.conf. Regardless of the pipe specified in `voxl-tag-detector.conf`. Hope this insight might help you in solving the issue in future releases. ## Reply by afdrus · 2023-12-06 09:39:22 UTC (in reply to afdrus) 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; } ```