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