bool ltr_int_init_tracking()
{
if(tracking_dbg_flag == DBG_CHECK){
tracking_dbg_flag = ltr_int_get_dbg_flag('t');
raw_dbg_flag = ltr_int_get_dbg_flag('r');
}
orientation = ltr_int_get_orientation();
if(orientation & ORIENT_FROM_BEHIND){
behind = true;
}else{
behind = false;
}
if(ltr_int_check_pose() == false){
ltr_int_log_message("Can't get pose setup!\n");
return false;
}
// filtered_bloblist.num_blobs = 3;
// filtered_bloblist.blobs = filtered_blobs;
// first_frame = true;
ltr_int_log_message("Tracking initialized!\n");
tracking_initialized = true;
init_recenter = true;
recenter = false;
return true;
}
static int update_absolute_pose(struct frame_type *frame)
{
static float c_pitch = 0.0f;
static float c_yaw = 0.0f;
static float c_roll = 0.0f;
static float c_tx = 0.0f;
static float c_ty = 0.0f;
static float c_tz = 0.0f;
//printf("Updating pose 1pt...\n");
if(!ltr_int_check_pose()){
return -1;
}
//printf("Updating pose...\n");
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);
}
}
if(recenter){
c_pitch = frame->bloblist.blobs[0].y;
c_yaw = frame->bloblist.blobs[0].x;
c_roll = frame->bloblist.blobs[1].x;
c_tx = frame->bloblist.blobs[1].y;
c_ty = frame->bloblist.blobs[2].x;
c_tz = frame->bloblist.blobs[2].y;
recenter = false;
}
//double tmp_angles[3], tmp_translations[3];
pthread_mutex_lock(&pose_mutex);
current_pose.pose.raw_pitch = frame->bloblist.blobs[0].y - c_pitch;
current_pose.pose.raw_yaw = frame->bloblist.blobs[0].x - c_yaw;
current_pose.pose.raw_roll = frame->bloblist.blobs[1].x - c_roll;
current_pose.pose.raw_tx = frame->bloblist.blobs[1].y - c_tx;
current_pose.pose.raw_ty = frame->bloblist.blobs[2].x - c_ty;
current_pose.pose.raw_tz = frame->bloblist.blobs[2].y - c_tz;
current_pose.abs_pose.abs_pitch = frame->bloblist.blobs[0].y;
current_pose.abs_pose.abs_yaw = frame->bloblist.blobs[0].x;
current_pose.abs_pose.abs_roll = frame->bloblist.blobs[1].x;
current_pose.abs_pose.abs_tx = frame->bloblist.blobs[1].y;
current_pose.abs_pose.abs_ty = frame->bloblist.blobs[2].x;
current_pose.abs_pose.abs_tz = frame->bloblist.blobs[2].y;
pthread_mutex_unlock(&pose_mutex);
//printf("Pose updated => rp: %g, ry: %g...\n", current_pose.raw_pitch, current_pose.raw_yaw);
return 0;
}
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;
}
static int update_pose_1pt(struct frame_type *frame)
{
static float c_x = 0.0f;
static float c_y = 0.0f;
static float c_z = 0.0f;
//printf("Updating pose 1pt...\n");
if(!ltr_int_check_pose()){
return -1;
}
//printf("Updating pose...\n");
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);
}
}
if((frame->bloblist.num_blobs > 0) && ltr_int_is_finite(frame->bloblist.blobs[0].x)
&& ltr_int_is_finite(frame->bloblist.blobs[0].y)){
}else{
return -1;
}
if(recenter){
c_x = frame->bloblist.blobs[0].x;
c_y = frame->bloblist.blobs[0].y;
c_z = cam_distance * sqrtf((float)frame->bloblist.blobs[0].score);
recenter = false;
}
double tmp_angles[3], tmp_translations[3];
//printf("cz = %f, z = %f\n", c_z, sqrtf((float)frame->bloblist.blobs[0].score));
//angles will be approximately "normalized" to (-100, 100);
// the rest should be handled by sensitivities
tmp_angles[0] = (frame->bloblist.blobs[0].y - c_y) * 200.0 / frame->width;
tmp_angles[1] = (c_x - frame->bloblist.blobs[0].x) * 200.0 / frame->width;
tmp_angles[2] = 0.0f;
tmp_translations[0] = 0.0f;
tmp_translations[1] = 0.0f;
if(ltr_int_is_face() && (frame->bloblist.blobs[0].score > 0)){
tmp_translations[2] = c_z / sqrtf((float)frame->bloblist.blobs[0].score) - cam_distance;
}else{
tmp_translations[2] = 0.0f;
}
if(behind){
tmp_angles[0] *= -1;
//prudent, but not really needed as it is presend only in facetracking and
// face can't be tracked from behind ;) Well, normaly it can't ;)
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 = 0;
current_pose.abs_pose.abs_yaw = 0;
current_pose.abs_pose.abs_roll = 0;
current_pose.abs_pose.abs_tx = 0;
current_pose.abs_pose.abs_ty = 0;
current_pose.abs_pose.abs_tz = 0;
pthread_mutex_unlock(&pose_mutex);
//printf("Pose updated => rp: %g, ry: %g...\n", current_pose.raw_pitch, current_pose.raw_yaw);
return 0;
}
