ModalAI Forum
    • Categories
    • Recent
    • Tags
    • Popular
    • Users
    • Groups
    • Register
    • Login

    Stereo tag detection: WARNING, apriltag roll/pitch out of bounds

    Ask your questions right here!
    2
    14
    784
    Loading More Posts
    • Oldest to Newest
    • Newest to Oldest
    • Most Votes
    Reply
    • Reply as topic
    Log in to reply
    This topic has been deleted. Only users with topic management privileges can see it.
    • A
      afdrus
      last edited by afdrus

      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?

      A 1 Reply Last reply Reply Quote 0
      • A
        afdrus @afdrus
        last edited by

        Is there someone that can unravel this issue?

        A 1 Reply Last reply Reply Quote 0
        • A
          afdrus @afdrus
          last edited by

          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.

          A 1 Reply Last reply Reply Quote 0
          • A
            afdrus @afdrus
            last edited by

            Sorry to insist, but I am not able to move forward with this issue, can anyone help?

            1 Reply Last reply Reply Quote 0
            • ModeratorM
              Moderator ModalAI Team
              last edited by

              Can you please help with which version of software you are using? voxl-vision-px4 has been deprecated. Have you tried with the latest?

              A 2 Replies Last reply Reply Quote 0
              • A
                afdrus @Moderator
                last edited by

                @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.

                1 Reply Last reply Reply Quote 0
                • A
                  afdrus @Moderator
                  last edited by

                  @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?

                  A 1 Reply Last reply Reply Quote 0
                  • A
                    afdrus @afdrus
                    last edited by

                    @Moderator Any updates on this issue?

                    A 1 Reply Last reply Reply Quote 0
                    • A
                      afdrus @afdrus
                      last edited by

                      Nothing changed on v1.1.0

                      ModeratorM 1 Reply Last reply Reply Quote 0
                      • ModeratorM
                        Moderator ModalAI Team @afdrus
                        last edited by

                        @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

                        A 1 Reply Last reply Reply Quote 0
                        • A
                          afdrus @Moderator
                          last edited by

                          @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

                          ModeratorM 1 Reply Last reply Reply Quote 0
                          • ModeratorM
                            Moderator ModalAI Team @afdrus
                            last edited by

                            @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

                            A 1 Reply Last reply Reply Quote 0
                            • A
                              afdrus @Moderator
                              last edited by

                              @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.

                              A 1 Reply Last reply Reply Quote 0
                              • A
                                afdrus @afdrus
                                last edited by

                                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;
}
                                1 Reply Last reply Reply Quote 0
                                • K Kessie referenced this topic on
                                • First post
                                  Last post
                                Powered by NodeBB | Contributors