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