static int update_pose_3pt(struct frame_type *frame)
{
if(frame->bloblist.num_blobs < 3){
return -1;
}
if(ltr_int_is_finite(frame->bloblist.blobs[0].x) && ltr_int_is_finite(frame->bloblist.blobs[0].y) &&
ltr_int_is_finite(frame->bloblist.blobs[1].x) && ltr_int_is_finite(frame->bloblist.blobs[1].y) &&
ltr_int_is_finite(frame->bloblist.blobs[2].x) && ltr_int_is_finite(frame->bloblist.blobs[2].y)){
}else{
return -1;
}
if(tracking_dbg_flag == DBG_ON){
unsigned int i;
for(i = 0; i < frame->bloblist.num_blobs; ++i){
ltr_int_log_message("*DBG_t* %d: %g %g %d\n", i, frame->bloblist.blobs[i].x, frame->bloblist.blobs[i].y,
frame->bloblist.blobs[i].score);
}
ltr_int_log_message("*DBG_t* d1 = %g d2 = %g d3 = %g\n",
two_d_size(frame->bloblist.blobs[0], frame->bloblist.blobs[1]),
two_d_size(frame->bloblist.blobs[0], frame->bloblist.blobs[2]),
two_d_size(frame->bloblist.blobs[1], frame->bloblist.blobs[2]));
}
linuxtrack_pose_t t;
linuxtrack_abs_pose_t abs_pose;
if(!ltr_int_pose_process_blobs(frame->bloblist, &t, &abs_pose, recenter)){
return -1;
}
recenter = false;
double tmp_angles[3], tmp_translations[3];
tmp_angles[0] = t.raw_pitch;
tmp_angles[1] = t.raw_yaw;
tmp_angles[2] = t.raw_roll;
tmp_translations[0] = t.raw_tx;
tmp_translations[1] = t.raw_ty;
tmp_translations[2] = t.raw_tz;
if(!ltr_int_is_vector_finite(tmp_angles) || !ltr_int_is_vector_finite(tmp_translations)){
return -1;
}
if(behind){
tmp_angles[0] *= -1;
tmp_angles[2] *= -1;
tmp_translations[0] *= -1;
tmp_translations[2] *= -1;
}
pthread_mutex_lock(&pose_mutex);
current_pose.pose.raw_pitch = tmp_angles[0];
current_pose.pose.raw_yaw = tmp_angles[1];
current_pose.pose.raw_roll = tmp_angles[2];
current_pose.pose.raw_tx = tmp_translations[0];
current_pose.pose.raw_ty = tmp_translations[1];
current_pose.pose.raw_tz = tmp_translations[2];
current_pose.abs_pose.abs_pitch = abs_pose.abs_pitch;
current_pose.abs_pose.abs_yaw = abs_pose.abs_yaw;
current_pose.abs_pose.abs_roll = abs_pose.abs_roll;
current_pose.abs_pose.abs_tx = abs_pose.abs_tx;
current_pose.abs_pose.abs_ty = abs_pose.abs_ty;
current_pose.abs_pose.abs_tz = abs_pose.abs_tz;
pthread_mutex_unlock(&pose_mutex);
if(raw_dbg_flag == DBG_ON){
printf("*DBG_r* yaw: %g pitch: %g roll: %g\n", tmp_angles[0], tmp_angles[1], tmp_angles[2]);
ltr_int_log_message("*DBG_r* x: %g y: %g z: %g\n",
tmp_translations[0], tmp_translations[1], tmp_translations[2]);
}
return 0;
}
bool ltr_int_postprocess_axes(ltr_axes_t axes, linuxtrack_pose_t *pose, linuxtrack_pose_t *unfiltered)
{
// printf(">>Pre: %f %f %f %f %f %f\n", pose->raw_pitch, pose->raw_yaw, pose->raw_roll,
// pose->raw_tx, pose->raw_ty, pose->raw_tz);
// static float filterfactor=1.0;
// ltr_int_get_filter_factor(&filterfactor);
static float filtered_angles[3] = {0.0f, 0.0f, 0.0f};
static float filtered_translations[3] = {0.0f, 0.0f, 0.0f};
//ltr_int_get_axes_ff(axes, filter_factors);
double raw_angles[3];
//Single point must be "denormalized"
raw_angles[0] = unfiltered->pitch = ltr_int_val_on_axis(axes, PITCH, pose->raw_pitch);
raw_angles[1] = unfiltered->yaw = ltr_int_val_on_axis(axes, YAW, pose->raw_yaw);
raw_angles[2] = unfiltered->roll = ltr_int_val_on_axis(axes, ROLL, pose->raw_roll);
//printf(">>Raw: %f %f %f\n", raw_angles[0], raw_angles[1], raw_angles[2]);
if(!ltr_int_is_vector_finite(raw_angles)){
return false;
}
pose->pitch = clamp_angle(ltr_int_filter_axis(axes, PITCH, raw_angles[0], &(filtered_angles[0])));
pose->yaw = clamp_angle(ltr_int_filter_axis(axes, YAW, raw_angles[1], &(filtered_angles[1])));
pose->roll = clamp_angle(ltr_int_filter_axis(axes, ROLL, raw_angles[2], &(filtered_angles[2])));
double rotated[3];
double transform[3][3];
double displacement[3];
displacement[0] = ltr_int_val_on_axis(axes, TX, pose->raw_tx);
displacement[1] = ltr_int_val_on_axis(axes, TY, pose->raw_ty);
displacement[2] = ltr_int_val_on_axis(axes, TZ, pose->raw_tz);
if(ltr_int_do_tr_align()){
//printf("Translations: Aligned\n");
ltr_int_euler_to_matrix(pose->pitch / 180.0 * M_PI, pose->yaw / 180.0 * M_PI,
pose->roll / 180.0 * M_PI, transform);
ltr_int_matrix_times_vec(transform, displacement, rotated);
if(!ltr_int_is_vector_finite(rotated)){
return false;
}
// ltr_int_print_matrix(transform, "trf");
// ltr_int_print_vec(displacement, "mv");
// ltr_int_print_vec(rotated, "rotated");
unfiltered->tx = rotated[0];
unfiltered->ty = rotated[1];
unfiltered->tz = rotated[2];
}else{
//printf("Translations: Unaligned\n");
unfiltered->tx = displacement[0];
unfiltered->ty = displacement[1];
unfiltered->tz = displacement[2];
}
pose->tx =
ltr_int_filter_axis(axes, TX, unfiltered->tx, &(filtered_translations[0]));
pose->ty =
ltr_int_filter_axis(axes, TY, unfiltered->ty, &(filtered_translations[1]));
pose->tz =
ltr_int_filter_axis(axes, TZ, unfiltered->tz, &(filtered_translations[2]));
//printf(">>Post: %f %f %f %f %f %f\n", pose->pitch, pose->yaw, pose->roll, pose->tx, pose->ty, pose->tz);
return true;
}
bool ltr_int_pose_process_blobs(struct bloblist_type blobs,
linuxtrack_pose_t *pose, linuxtrack_abs_pose_t *abs_pose, bool centering)
{
// double points[3][3];
double points[3][3] = {{28.35380, -1.24458, -0.11606},
{103.19049, -44.13490, -76.36657},
{131.32094, 10.75412, -50.19649}
};
/*
double points[3][3] = {{22.923, 110.165, 28.070},
{91.241, 44.781, 91.768},
{184.262, 18.075, 47.793}
};
*/
if(ltr_int_use_alter()){
alter_pose(blobs, points, centering);
}else{
iter_pose(blobs, points, centering);
}
double angles[3];
double displacement[3];
double abs_center[3] = {0.0, 0.0, 0.0};
double abs_angles[3] = {0.0, 0.0, 0.0};
if(ltr_int_use_oldrot()){
//printf("Rotations: Old algo\n");
//ltr_int_print_matrix(points, "points");
double new_base[3][3];
double vec1[3];
double vec2[3];
ltr_int_make_vec(points[1], points[0], vec1);
ltr_int_make_vec(points[2], points[0], vec2);
ltr_int_make_base(vec1, vec2, new_base);
// ltr_int_print_matrix(new_base, "new_base");
if(!ltr_int_is_matrix_finite(new_base)){
return false;
}
if(centering == true){
ltr_int_assign_matrix(new_base, center_base);
}
//all applications contain transposed base
ltr_int_transpose_in_place(new_base);
double new_center[3];
ltr_int_matrix_times_vec(new_base, model_ref, vec1);
ltr_int_add_vecs(points[0], vec1, new_center);
// ltr_int_print_matrix(new_base, "new_base'");
// ltr_int_print_vec(model_ref, "model_ref");
// ltr_int_print_vec(points[0], "pt0");
// ltr_int_print_vec(new_center, "new_center");
if(centering == true){
int i;
for(i = 0; i < 3; ++i){
center_ref[i] = new_center[i];
}
}
abs_center[0] = new_center[0];
abs_center[1] = new_center[1];
abs_center[2] = new_center[2];
ltr_int_make_vec(new_center, center_ref, displacement);
// ltr_int_print_vec(center_ref, "ref_pt");
// ltr_int_print_vec(displacement, "mv");
// ltr_int_print_matrix(center_base, "center_base");
double transform[3][3];
ltr_int_mul_matrix(new_base, center_base, transform);
// ltr_int_print_matrix(new_base, "new_base'");
// ltr_int_print_matrix(center_base, "center_base");
// ltr_int_print_matrix(transform, "transform");
//double pitch, yaw, roll;
ltr_int_matrix_to_euler(transform, &(angles[0]), &(angles[1]), &(angles[2]));
}else{
//printf("Rotations: New algo\n");
//ltr_int_print_matrix(points, "points");
if(centering){
if(!ltr_int_center(points[0], points[1], points[2], c_base, tr_center, tr_rot)){
ltr_int_log_message("Couldn't center in new pose!\n");
return false;
}
}
if(!ltr_int_get_pose(points[0], points[1], points[2], c_base, tr_center,
tr_rot, angles, displacement, abs_center, abs_angles)){
ltr_int_log_message("Couldn't determine the pose in new pose!\n");
return false;
}
}
ltr_int_mul_vec(angles, 180.0 /M_PI, angles);
ltr_int_mul_vec(abs_angles, 180.0 /M_PI, abs_angles);
if(ltr_int_is_vector_finite(angles) && ltr_int_is_vector_finite(displacement)){
pose->raw_pitch = angles[0];
pose->raw_yaw = angles[1];
pose->raw_roll = angles[2];
pose->raw_tx = displacement[0];
pose->raw_ty = displacement[1];
pose->raw_tz = displacement[2];
abs_pose->abs_pitch = abs_angles[0];
abs_pose->abs_yaw = abs_angles[1];
abs_pose->abs_roll = abs_angles[2];
abs_pose->abs_tx = abs_center[0];
abs_pose->abs_ty = abs_center[1];
abs_pose->abs_tz = abs_center[2];
return true;
}
return false;
}
