void VoEstimator::updatePosition(int64_t utime, int64_t utime_prev, Eigen::Isometry3d delta_camera){ // 1. Update the Position of the head frame: Eigen::Isometry3d delta_head = camera_to_head_.inverse()*delta_camera*camera_to_head_; local_to_head_ = local_to_head_*delta_head; // 2. Evaluate Rates: double delta_time = ( (double) utime - utime_prev)/1E6; if(utime_prev==0){ std::cout << "utime_prev is zero [at init]\n"; vo_initialized_ = false; // reconfirming what is set above }else{ vo_initialized_ = true; //Eigen::Isometry3d delta_head = local_to_head_prev_.inverse() * local_to_head_; head_lin_rate_ = delta_head.translation()/delta_time; Eigen::Vector3d delta_head_rpy; quat_to_euler( Eigen::Quaterniond(delta_head.rotation()) , delta_head_rpy(0), delta_head_rpy(1), delta_head_rpy(2)); head_rot_rate_ = delta_head_rpy/delta_time; // rotation rate } // 3. Maintain a smoothed version: double alpha = 0.8; head_lin_rate_alpha_ = alpha*head_lin_rate_alpha_ + (1-alpha)*head_lin_rate_; head_rot_rate_alpha_ = alpha*head_rot_rate_alpha_ + (1-alpha)*head_rot_rate_; // 4. Output the head position update, at the rates provided by VO publishUpdate(utime_prev, local_to_head_, "POSE_HEAD_ALT_FOVIS", true); local_to_head_prev_ = local_to_head_; delta_head_prev_ = delta_head; }
void print_Isometry3d(Eigen::Isometry3d pose, std::stringstream &ss){ Eigen::Vector3d t(pose.translation()); Eigen::Quaterniond r(pose.rotation()); double rpy[3]; quat_to_euler(r, rpy[0], rpy[1], rpy[2]); ss <<t[0]<<", "<<t[1]<<", "<<t[2]<<" | " <<r.w()<<", "<<r.x()<<", "<<r.y()<<", "<<r.z() << " | RPY " << rpy[0] <<", "<< rpy[1] <<", "<< rpy[2]; }
std::string print_Isometry3d(Eigen::Isometry3d pose){ Eigen::Vector3d t(pose.translation()); Eigen::Quaterniond r(pose.rotation()); double rpy[3]; quat_to_euler(r, rpy[0], rpy[1], rpy[2]); std::stringstream ss; ss <<t[0]<<", "<<t[1]<<", "<<t[2]<<", " <<r.w()<<", "<<r.x()<<", "<<r.y()<<", "<<r.z() << ", " << rpy[0] <<", "<< rpy[1] <<", "<< rpy[2]; return ss.str(); }
int kam_pol_cti_klic(AnimHandle handle, float time, BOD * p_t, float *p_r, float *p_fi, float *p_vzdal) { KAMERA_TRACK_INFO *p_track; int loop, dtime; QUAT q(1, 0, 0, 0); if (handle >= p_ber->kamnum) return (K_CHYBA); p_track = p_ber->kamery + handle; if (!p_track || p_track->flag & GAME_KAMERA_3DS) return (K_CHYBA); loop = p_track->flag & GK_LOOP; if (time > 1.0f) time = 1.0f; else if (time < 0.0f) time = 0.0f; dtime = ftoi(time * p_track->endtime); if (p_track->pos_keys) { key_track_interpolace_bod(p_t, p_track->p_pos, p_track->p_pkeys, dtime, p_track->endtime, p_track->pos_keys, loop); } else { p_t->set(0.0f); } if (p_track->quat_keys) { key_track_interpolace_quat(&q, p_track->p_quat, p_track->p_qkeys, dtime, p_track->endtime, p_track->quat_keys, loop); } if (p_track->roll_keys) { key_track_interpolace_float(p_vzdal, p_track->p_roll, p_track->p_rlkeys, dtime, p_track->endtime, p_track->roll_keys, loop); } else { *p_vzdal = 0; } quat_to_euler(&q, p_r, p_fi); return (TRUE); }
void quest_estimator_update(float q[4]) { // Make best guess for which sequential rotation to use if (fabsf(last_orientation[0]) > PI * 0.5f) { // Rotation around x axis sequential_rotations = 1; } else if (fabsf(last_orientation[2]) > PI * 0.5f) { // Rotation around z axis sequential_rotations = 3; } else { // No rotation sequential_rotations = 0; } sequential_rotations = 0; #ifdef PRINT_SEQUENTIAL_ROTATION PRINT("Rotation %i\n", sequential_rotations); #endif for (int attempt = 0; attempt < 4; attempt++) { rotate_measurement_data(); float B[3][3]; for (int row = 0; row < 3; row++) { for (int col = 0; col < 3; col++) { B[row][col] = AO_rotated[row] * AR[col] * AccelA + MO_rotated[row] * MR[col] * MagA; } } float minus_s[3][3]; for (int row = 0; row < 3; row++) { for (int col = 0; col < 3; col++) { minus_s[row][col] = -B[row][col] - B[col][row]; } } float sigma = MagA * (MR[0] * MO_rotated[0] + MR[1] * MO_rotated[1] + MR[2] * MO_rotated[2]) + AccelA * (AR[0] * AO_rotated[0] + AR[1] * AO_rotated[1] + AR[2] * AO_rotated[2]); float Z[3]; Z[0] = B[1][2] - B[2][1]; Z[1] = B[2][0] - B[0][2]; Z[2] = B[0][1] - B[1][0]; float deltaCos = vector_dot(MO_rotated, AO_rotated) * vector_dot(MR, AR) + vector_cross_mag(MO_rotated, AO_rotated) * vector_cross_mag(MR, AR); float lambda = sqrtf(MagA * MagA + 2 * MagA * AccelA * deltaCos + AccelA * AccelA); float lamda_plus_sig = lambda + sigma; minus_s[0][0] += lamda_plus_sig; minus_s[1][1] += lamda_plus_sig; minus_s[2][2] += lamda_plus_sig; float ymat[3][3]; float det = mat3x3_det(minus_s); if (det < 1e-2f) { // We made the wrong sequential rotation assumption - try a different one // This will only occur if spinning really fast or upside down sequential_rotations++; if (sequential_rotations > 3) sequential_rotations = 0; PRINT("Made incorrect first guess! Changing to %i\n",sequential_rotations); continue; } mat3x3_inv_transpose(minus_s, det, ymat); float a = B[0][1] - B[1][0]; float b = B[0][2] - B[2][0]; float c = B[1][2] - B[2][1]; for (int i = 0; i < 3; i++) { q[i] = ymat[2][i] * a - ymat[1][i] * b + ymat[0][i] * c; } float qNorm = sqrtf(1.0f + q[0] * q[0] + q[1] * q[1] + q[2] * q[2]); q[3] = 1.0f / qNorm; for (int i = 0; i < 3; i++) q[i] /= qNorm; rotate_quaternion(q); if (q[3] < 0.0f) { for (int i = 0; i < 4; i++) q[i] = -q[i]; } quat_to_euler(q, last_orientation); last_w = q[3]; //PRINT("%f %f %f\n", last_orientation[0], last_orientation[1], last_orientation[2]); break; } }
void kani_updatuj(G_KONFIG * p_ber) { KAMERA_TRACK_INFO *p_track; GAME_KAMERA *p_kam; int konec, loop, zrusit = FALSE; int next_time = p_ber->TimeEndLastFrame; QUAT q(1.0f, 0.0f, 0.0f, 0.0f); p_kam = &p_ber->kamera; p_track = p_ber->kamera.p_anim; loop = p_kam->flag & GK_LOOP; if ((konec = (next_time > p_kam->time_stop))) { if (loop) { calc_time_loop(next_time, p_kam->start, (dword *) & p_kam->time_start, (dword *) & p_kam->time_stop, (dword *) & p_kam->time_delka, (dword *) & p_kam->time); if (p_kam->p_flag) *(p_kam->p_flag) = 0; konec = 0; } else { p_kam->time = p_kam->time_delka; zrusit = (p_kam->flag & GK_REMOVE); if (p_kam->p_flag) *(p_kam->p_flag) = K_CHYBA; } } else { p_kam->time = calc_time_akt(next_time, p_kam->time_start); if (p_kam->p_flag) *(p_kam->p_flag) = ftoi((p_kam->time / (float) p_kam->time_delka) * 100.0f); } if (p_kam->aktivni & GAME_KAMERA_3DS) { if (p_track->pos_keys) key_track_interpolace_bod(&p_kam->p, p_track->p_pos, p_track->p_pkeys, p_kam->time, p_track->endtime, p_track->pos_keys, loop); if (p_track->trg_keys) key_track_interpolace_bod(&p_kam->t, p_track->p_trg, p_track->p_tkeys, p_kam->time, p_track->endtime, p_track->trg_keys, loop); if (p_track->roll_keys) key_track_interpolace_float(&p_kam->roll, p_track->p_roll, p_track->p_rlkeys, p_kam->time, p_track->endtime, p_track->roll_keys, loop); calc_camera_3ds(p_ber->p_camera, p_ber->p_invcam, &p_kam->p, &p_kam->t, p_kam->roll); set_matrix_camera(p_ber->p_camera); set_matrix_camera_project(p_ber->p_project); p_ber->kamera.zmena = TRUE; } else { if (p_track->pos_keys) key_track_interpolace_bod(&p_kam->t, p_track->p_pos, p_track->p_pkeys, p_kam->time, p_track->endtime, p_track->pos_keys, loop); if (p_track->quat_keys) key_track_interpolace_quat(&q, p_track->p_quat, p_track->p_qkeys, p_kam->time, p_track->endtime, p_track->quat_keys, loop); if (p_track->roll_keys) key_track_interpolace_float(&p_kam->vzdal, p_track->p_roll, p_track->p_rlkeys, p_kam->time, p_track->endtime, p_track->roll_keys, loop); calc_camera_polar(p_ber->p_camera, p_ber->p_invcam, &p_kam->t, &q, p_kam->vzdal); quat_to_euler(&q, &p_kam->r, &p_kam->fi); ber_kamera_korekce_vzdalenosti(p_ber, p_ber->kam_omezeni, FALSE); p_ber->kamera.zmena = TRUE; } // Zrus animaci if (zrusit) kam_stop(); }
void Node::update_euler_angles() { for (unsigned int i = 0; i < quat_arr.size(); i++) { quat_to_euler(quat_arr.at(i), freuler->at(i)); } }
void des_thrusters::step(double delta) { bool softkilled; shm_get(switches, soft_kill, softkilled); shm_getg(settings_control, shm_settings); if (softkilled || !shm_settings.enabled) { f -= f; t -= t; return; } // Read PID settings & desires. // (would switching to watchers improve performance?) SHM_GET_PID(settings_heading, shm_hp, hp) SHM_GET_PID(settings_pitch, shm_pp, pp) SHM_GET_PID(settings_roll, shm_rp, rp) SHM_GET_PID(settings_velx, shm_xp, xp) SHM_GET_PID(settings_vely, shm_yp, yp) SHM_GET_PID(settings_depth, shm_dp, dp) shm_getg(desires, shm_desires); // Read current orientation, velocity & position (in the model frame). // Orientation quaternion in the model frame. Eigen::Quaterniond qm = q * mtob_rq; Eigen::Vector3d rph = quat_to_euler(qm); // Component of the model quaternion about the z-axis (heading-only rotation). Eigen::Quaterniond qh = euler_to_quat(rph[2], 0, 0); // Velocity in heading modified world space. Eigen::Vector3d vp = qh.conjugate() * v; // Step the PID loops. Eigen::Vector3d desires = quat_to_euler(euler_to_quat(shm_desires.heading * DEG_TO_RAD, shm_desires.pitch * DEG_TO_RAD, shm_desires.roll * DEG_TO_RAD)) * RAD_TO_DEG; double ho = hp.step(delta, desires[2], rph[2] * RAD_TO_DEG); double po = pp.step(delta, desires[1], rph[1] * RAD_TO_DEG); double ro = rp.step(delta, desires[0], rph[0] * RAD_TO_DEG); double xo = xp.step(delta, shm_desires.speed, vp[0]); double yo = yp.step(delta, shm_desires.sway_speed, vp[1]); double zo = dp.step(delta, shm_desires.depth, x[2]); // f is in the heading modified world frame. // t is in the model frame. // We will work with f and b in the model frame until the end of this // function. f[0] = shm_settings.velx_active ? xo : 0; f[1] = shm_settings.vely_active ? yo : 0; f[2] = shm_settings.depth_active ? zo : 0; f *= m; Eigen::Vector3d w_in; w_in[0] = shm_settings.roll_active ? ro : 0; w_in[1] = shm_settings.pitch_active ? po : 0; w_in[2] = shm_settings.heading_active ? ho : 0; // TODO Avoid this roundabout conversion from hpr frame // to world frame and back to model frame. Eigen::Quaterniond qhp = euler_to_quat(rph[2], rph[1], 0); Eigen::Vector3d w = qm.conjugate() * (Eigen::Vector3d(0, 0, w_in[2]) + qh * Eigen::Vector3d(0, w_in[1], 0) + qhp * Eigen::Vector3d(w_in[0], 0, 0)); t = btom_rm * q.conjugate() * I * q * mtob_rm * w; // Output diagnostic information. Shown by auv-control-helm. SHM_PUT_PID(control_internal_heading, ho) SHM_PUT_PID(control_internal_pitch, po) SHM_PUT_PID(control_internal_roll, ro) SHM_PUT_PID(control_internal_velx, xo) SHM_PUT_PID(control_internal_vely, yo) SHM_PUT_PID(control_internal_depth, zo) // Subtract passive forces. // (this implementation does not support discrimination!) if (shm_settings.buoyancy_forces || shm_settings.drag_forces) { // pff is in the body frame. screw ps = pff(); f -= qh.conjugate() * q * ps.first; t -= btom_rm * ps.second; } // Hyper-ellipsoid clamping. Sort of limiting to the maximum amount of // energy the sub can output (e.g. can't move forward at full speed // and pitch at full speed at the same time). // Doesn't really account for real-world thruster configurations (e.g. // it might be possible to move forward at full speed and ascend at // full speed at the same time) but it's just an approximation. for (int i = 0; i < 3; i++) { f[i] /= axes[i]; } for (int i = 0; i < 3; i++) { t[i] /= axes[3 + i]; } double m = f.dot(f) + t.dot(t); if (m > 1) { double sm = sqrt(m); f /= sm; t /= sm; } for (int i = 0; i < 3; i++) { f[i] *= axes[i]; } for (int i = 0; i < 3; i++) { t[i] *= axes[3 + i]; } // Regular min/max clamping. clamp(f, fa, fb, 3); clamp(t, ta, tb, 3); f = q.conjugate() * qh * f; t = mtob_rm * t; }