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

afdrus

Is there someone that can unravel this issue?

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.

afdrus

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

Moderator

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

afdrus

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

afdrus

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

afdrus

@Moderator Any updates on this issue?

afdrus

Nothing changed on v1.1.0

Moderator

@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

@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

Moderator

@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

afdrus

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

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;
}