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

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