bool ltr_int_check_pose()
{
struct reflector_model_type rm;
if(ltr_int_model_changed(true)){
if(!ltr_int_get_model_setup(&rm)){
ltr_int_log_message("Can't get pose setup!\n");
return false;
}
ltr_int_log_message("Initializing model!\n");
if(!ltr_int_pose_init(rm)){
ltr_int_log_message("Can't initialize pose!\n");
return false;
}
}
return true;
}
int ltr_int_update_pose(struct frame_type *frame)
{
//printf("Updating pose...\n");
if(ltr_int_model_changed(false)){
ltr_int_check_pose();
recenter = true;
}
unsigned int i;
ltr_int_remove_camera_rotation(frame->bloblist);
ltr_int_pose_sort_blobs(frame->bloblist);
pthread_mutex_lock(&pose_mutex);
current_pose.pose.resolution_x = frame->width;
current_pose.pose.resolution_y = frame->height;
for(i = 0; i < MAX_BLOBS * BLOB_ELEMENTS; ++i){
current_pose.blob_list[i] = 0.0;
}
for(i = 0; i < frame->bloblist.num_blobs; ++i){
current_pose.blob_list[i * BLOB_ELEMENTS] = frame->bloblist.blobs[i].x;
current_pose.blob_list[i * BLOB_ELEMENTS + 1] = frame->bloblist.blobs[i].y;
current_pose.blob_list[i * BLOB_ELEMENTS + 2] = frame->bloblist.blobs[i].score;
}
current_pose.blobs = frame->bloblist.num_blobs;
pthread_mutex_unlock(&pose_mutex);
bool res = -1;
if(ltr_int_is_single_point()){
res = update_pose_1pt(frame);
}else if(ltr_int_is_absolute()){
res = update_absolute_pose(frame);
}else{
res = update_pose_3pt(frame);
}
if(res == 0){
++counter_d;
if(init_recenter){
//discard the first valid frame to allow good recenter on the next one
init_recenter = false;
recenter = true;
--counter_d;
res = -1;
}
}
return res;
}
