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;
}
int ltr_int_tracker_init(struct camera_control_block *ccb)
{
ltr_int_log_message("Initializing the tracker.\n");
assert(ccb != NULL);
assert((ccb->device.category == tir) || (ccb->device.category == tir_open));
last_threshold = -1;
char *libname = NULL;
dbg_flag_type fakeusb_dbg_flag = ltr_int_get_dbg_flag('f');
if(fakeusb_dbg_flag == DBG_ON) {
libname = "libfakeusb";
} else {
libname = "libltusb1";
}
if((libhandle = ltr_int_load_library(libname, functions)) == NULL) {
ltr_int_log_message("Problem loading library %s!\n", libname);
return -1;
}
if(!ltr_int_tir_init_prefs()) {
ltr_int_log_message("Problem initializing TrackIr prefs!\n");
return -1;
}
ltr_int_log_message("Lib loaded, prefs read...\n");
if(ltr_int_open_tir(false, !ltr_int_is_model_active())) {
tir_info info;
ltr_int_get_tir_info(&info);
ccb->pixel_width = info.width;
ccb->pixel_height = info.height;
ltr_int_prepare_for_processing(ccb->pixel_width, ccb->pixel_height);
return 0;
} else {
return -1;
}
}
bool ltr_int_init_usb()
{
ltr_int_log_message("Initializing libusb.\n");
if(comm_dbg_flag == DBG_CHECK){
comm_dbg_flag = ltr_int_get_dbg_flag('u');
}
int res = libusb_init(&usb_context);
if(res != 0){
ltr_int_log_message("Problem initializing libusb!\n");
return false;
}
ltr_int_log_message("Libusb initialized successfuly.\n");
libusb_set_debug(usb_context, 0);
ltr_int_log_message("Libusb debug level set.\n");
return true;
}
int ltr_int_tir_found(bool *have_firmware, bool *have_permissions)
{
char *libname = NULL;
dbg_flag_type fakeusb_dbg_flag = ltr_int_get_dbg_flag('f');
if(fakeusb_dbg_flag == DBG_ON) {
libname = "libfakeusb";
ltr_int_log_message("Loading fakeusb!\n");
} else {
libname = "libltusb1";
}
if((libhandle = ltr_int_load_library(libname, functions)) == NULL) {
ltr_int_log_message("Failed to load the library '%s'! \n", libname);
return 0;
}
if(!ltr_int_init_usb()) {
ltr_int_log_message("Failed to initialize usb!\n");
return 0;
}
dev_found device = ltr_int_find_tir();
printf("Found device %X\n", device);
if(device & NOT_PERMITTED) {
device ^= NOT_PERMITTED;
*have_permissions = false;
} else {
*have_permissions = true;
}
if((device < TIR4) || (device == SMARTNAV3)) {
*have_firmware = true;
} else {
char *fw = ltr_int_find_firmware(device);
if(fw != NULL) {
free(fw);
*have_firmware = true;
} else {
*have_firmware = false;
}
}
ltr_int_finish_usb(-1);
ltr_int_unload_library(libhandle, functions);
libhandle = NULL;
return device;
}
bool ltr_int_pose_init(struct reflector_model_type rm)
{
if(pts_dbg_flag == DBG_CHECK){
pts_dbg_flag = ltr_int_get_dbg_flag('3');
}
switch(rm.type){
case CAP:
type = M_CAP;
#ifdef PT_DBG
printf("MODEL:CAP\n");
#endif
break;
case CLIP:
type = M_CLIP;
#ifdef PT_DBG
printf("MODEL:CLIP\n");
#endif
break;
case SINGLE:
type = M_SINGLE;
#ifdef PT_DBG
printf("MODEL:SINGLE\n");
#endif
return true;
break;
case FACE:
type = M_FACE;
#ifdef PT_DBG
printf("MODEL:FACE\n");
#endif
return true;
break;
case ABSOLUTE:
type = M_ABSOLUTE;
#ifdef PT_DBG
printf("MODEL:ABSOLUTE\n");
#endif
return true;
break;
default:
ltr_int_log_message("Unknown model type specified %d!\n", rm.type);
assert(0);
return false;
break;
}
#ifdef PT_DBG
printf("RM0: %g %g %g\n", rm.p0[0], rm.p0[1], rm.p0[2]);
printf("RM1: %g %g %g\n", rm.p1[0], rm.p1[1], rm.p1[2]);
printf("RM2: %g %g %g\n", rm.p2[0], rm.p2[1], rm.p2[2]);
printf("RM_REF: %g %g %g\n", rm.hc[0], rm.hc[1], rm.hc[2]);
#endif
double ref[3];
ref[0] = rm.hc[0];
ref[1] = rm.hc[1];
ref[2] = rm.hc[2];
ltr_int_make_vec(rm.p0, ref, model_point0);
ltr_int_make_vec(rm.p1, ref, model_point1);
ltr_int_make_vec(rm.p2, ref, model_point2);
/* Out of model points create orthonormal base */
double vec1[3] = {0.0, 0.0, 0.0};
double vec2[3] = {0.0, 0.0, 0.0};
ltr_int_make_vec(model_point1, model_point0, vec1);
ltr_int_make_vec(model_point2, model_point0, vec2);
ltr_int_make_base(vec1, vec2, model_base);
if(!ltr_int_is_matrix_finite(model_base)){
ltr_int_log_message("Incorrect model dimmensions - can't create orthonormal base!\n");
return false;
}
//for testing purposes
ltr_int_make_base(vec1, vec2, center_base);
/* Convert reference point to model base coordinates */
// double ref_pt[3];
double origin[3] = {0,0,0};
double vec3[3];
ltr_int_make_vec(origin, model_point0, vec3);
ltr_int_matrix_times_vec(model_base, vec3, model_ref);
#ifdef MDL_DBG0
ltr_int_print_vec(model_point0, "model_point0");
ltr_int_print_vec(model_point1, "model_point1");
ltr_int_print_vec(model_point2, "model_point2");
ltr_int_print_matrix(model_base, "model_base");
ltr_int_print_vec(ref, "ref");
ltr_int_print_vec(vec3, "vec3");
ltr_int_print_vec(model_ref, "model_ref");
#endif
//use_alter = ltr_int_use_alter();
//use_old_pose = ltr_int_use_oldrot();
bool res = ltr_int_get_cbase(rm.p0, rm.p1, rm.p2, rm.hc, c_base) &&
ltr_int_center(rm.p0, rm.p1, rm.p2, c_base, tr_center, tr_rot);
ltr_int_assign_matrix(tr_rot, tr_rot_base);
return res;
}
