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