bool ltr_int_get_pose(double rp0[3], double rp1[3], double rp2[3], double cntr_base[3],
double center[3], double tr[3][3], double angles[3], double trans[3], double abs_pose[3], double abs_angles[3])
{
double rb0[3], rb1[3], rb[3][3], rc[3], RR[3][3];
//find model's current local base
ltr_int_make_vec(rp1, rp0, rb0);
ltr_int_make_vec(rp2, rp0, rb1);
ltr_int_make_base(rb0, rb1, rb);
ltr_int_transpose_in_place(rb);
if(!ltr_int_is_matrix_finite(rb)){
return false;
}
//raw model's base in camera coordiates?
//ltr_int_print_matrix(rb, "rb");
//transform model's center from local to global coordinates
// (using p0 as an anchor)
ltr_int_matrix_times_vec(rb, cntr_base, rc);
double current_center[3];
ltr_int_add_vecs(rc, rp0, current_center);
//Current center in cam. coords
abs_pose[0] = current_center[0];
abs_pose[1] = current_center[1];
abs_pose[2] = current_center[2];
ltr_int_make_vec(current_center, center, trans);
double rot[3][3];
ltr_int_make_vec(rp0, current_center, RR[0]);
ltr_int_make_vec(rp1, current_center, RR[1]);
ltr_int_make_vec(rp2, current_center, RR[2]);
ltr_int_transpose_in_place(RR);
ltr_int_mul_matrix(RR, tr, rot);
if(!ltr_int_is_matrix_finite(rot)){
return false;
}
ltr_int_matrix_to_euler(rot, &(angles[0]), &(angles[1]), &(angles[2]));
ltr_int_mul_matrix(RR, tr_rot_base, rot);
ltr_int_matrix_to_euler(rot, &(abs_angles[0]), &(abs_angles[1]), &(abs_angles[2]));
/*
printf("Center: %f %f %f\n", current_center[0], current_center[1], current_center[2]);
printf("Angles: %f %f %f\n", abs_angles[0], abs_angles[1], abs_angles[2]);
double test1[3];
ltr_int_make_vec(rp0, current_center, RR[0]);
ltr_int_make_vec(rp1, current_center, RR[1]);
ltr_int_make_vec(rp2, current_center, RR[2]);
ltr_int_matrix_times_vec(rot, model_point0, test1);
ltr_int_print_vec(RR[0], "MP0");
ltr_int_print_vec(test1, "Pok0");
ltr_int_print_vec(model_point1, "MP10");
ltr_int_print_vec(RR[1], "MP1");
ltr_int_print_vec(test1, "Pok1");
ltr_int_print_vec(model_point2, "MP20");
ltr_int_print_vec(RR[2], "MP2");
ltr_int_print_vec(test1, "Pok2");
*/
return true;
}
//Determine coordinates of the model's center of rotation in its local coordinates
// allowing to find it given only its three points in space.
bool ltr_int_get_cbase(double p0[3], double p1[3], double p2[3], double c[3],
double cbase[3])
{
double R[3][3];
double base[3][3];
//move model's center of rotation to [0,0,0]
ltr_int_make_vec(p0, c, R[0]);
ltr_int_make_vec(p1, c, R[1]);
ltr_int_make_vec(p2, c, R[2]);
//create a local model's orthonormal base (center is p0)
double b1[3], b2[3];
ltr_int_make_vec(R[1], R[0], b1);
ltr_int_make_vec(R[2], R[0], b2);
ltr_int_make_base(b1, b2, base);
if(!ltr_int_is_matrix_finite(base)){
return false;
}
//devise center of rotation in model's local base coordinates
// this allows to determine model's center rotation given only rp0-rp2
double tmp[3] = {0,0,0};
//reverse the vector to first point - later when added to first point,
// it gives the center's location
ltr_int_make_vec(tmp, R[0], tmp);
ltr_int_matrix_times_vec(base, tmp, cbase);
return true;
}
//Employed when centering to determine center position and transformation
// cancelling the rotation
bool ltr_int_center(double rp0[3], double rp1[3], double rp2[3], double cntr_base[3],
double center[3], double tr[3][3])
{
double rb0[3], rb1[3], rb[3][3], rc[3], RR[3][3];
//find model's current local base
ltr_int_make_vec(rp1, rp0, rb0);
ltr_int_make_vec(rp2, rp0, rb1);
ltr_int_make_base(rb0, rb1, rb);
ltr_int_transpose_in_place(rb);
if(!ltr_int_is_matrix_finite(rb)){
return false;
}
//ltr_int_print_matrix(rb, "rb");
//transform model's center from local to global coordinates
// (using p0 as an anchor)
ltr_int_matrix_times_vec(rb, cntr_base, rc);
ltr_int_add_vecs(rc, rp0, center);
//Devise the reverse transformation
// transform zero to the model's center of rotation
//ltr_int_print_vec(center, "center");
ltr_int_make_vec(rp0, center, RR[0]);
ltr_int_make_vec(rp1, center, RR[1]);
ltr_int_make_vec(rp2, center, RR[2]);
ltr_int_transpose_in_place(RR);
//ltr_int_print_matrix(RR, "RR");
//get inverse transform (not orthonormal!!!)
double tmp_tr[3][3];
ltr_int_invert_matrix(RR, tmp_tr);
if(!ltr_int_is_matrix_finite(tmp_tr)){
return false;
}
ltr_int_assign_matrix(tmp_tr, tr);
return true;
}
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_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;
}
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;
}
