Stereo tag detection: WARNING, apriltag roll/pitch out of bounds
-
Hello everyone,
I am having issues on reading the apriltags flat on the ground from the stereo using a sentinel drone. I keep receivingWARNING, apriltag roll/pitch out of boundsand 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?
-
The command
voxl-vision-px4 -mdetects properly the distances between camera and tag, as well as their orientation.
Alsovoxl-vision-px4 -lshows 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 andWARNING, apriltag roll/pitch out of boundsis gone. -
The command
voxl-vision-px4 -mdetects properly the distances between camera and tag, as well as their orientation.
Alsovoxl-vision-px4 -lshows 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 andWARNING, apriltag roll/pitch out of boundsis gone. -
Can you please help with which version of software you are using? voxl-vision-px4 has been deprecated. Have you tried with the latest?
-
Can you please help with which version of software you are using? voxl-vision-px4 has been deprecated. Have you tried with the latest?
@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.
-
Can you please help with which version of software you are using? voxl-vision-px4 has been deprecated. Have you tried with the latest?
-
@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?
-
@Moderator Any updates on this issue?
-
@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 -
@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@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_localI get the followingID 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.14No matter what values I put in extrinsics.conf or in R_tag_to_fixed, the Roll keeps being around 1.4 radians
-
@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_localI get the followingID 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.14No matter what values I put in extrinsics.conf or in R_tag_to_fixed, the Roll keeps being around 1.4 radians
-
@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
@Moderator Thanks to some prints I notice that the issue comes from
R_cam_to_imuwhich always (and only) picks up the parameters from the tracking camera in extrinsics.conf. Regardless of the pipe specified invoxl-tag-detector.conf. Hope this insight might help you in solving the issue in future releases. -
@Moderator Thanks to some prints I notice that the issue comes from
R_cam_to_imuwhich always (and only) picks up the parameters from the tracking camera in extrinsics.conf. Regardless of the pipe specified invoxl-tag-detector.conf. Hope this insight might help you in solving the issue in future releases.I did a very quick and dummy workaround to fix the problem, most of the changes are in
geometry.cin 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; } -
K Kessie referenced this topic on
Hello! It looks like you're interested in this conversation, but you don't have an account yet.
Getting fed up of having to scroll through the same posts each visit? When you register for an account, you'll always come back to exactly where you were before, and choose to be notified of new replies (either via email, or push notification). You'll also be able to save bookmarks and upvote posts to show your appreciation to other community members.
With your input, this post could be even better 💗
Register Login