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