Skip to content

Fix VRPN, improve tracker layout #146

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 9 commits into
base: master
Choose a base branch
from
Open

Fix VRPN, improve tracker layout #146

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
7d6055a
Fix head rotation
Apr 3, 2019
a113e88
Merge remote-tracking branch 'upstream/master' into HEAD
Jul 17, 2019
f359fe6
Fix VRPN Tracker. Start to introduce augmented Tracker struct.
Jul 10, 2019
edff317
Use new tracker class
Jul 11, 2019
f8041bb
Fix update SSR with new values
Jul 12, 2019
3d2fd03
Further improvements on tracker handling, introduce data getter
Jul 15, 2019
2d78f52
Fix Warning
Jul 15, 2019
fde501c
Fix rebase onto upstream master
Jul 17, 2019
5aca323
Start to use quaternions, restructure
Jul 17, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1924,7 +1924,7 @@ Controller<Renderer>::_calibrate_client()
#if defined(ENABLE_INTERSENSE) || defined(ENABLE_POLHEMUS) || defined(ENABLE_VRPN) || defined(ENABLE_RAZOR)
if (_tracker)
{
_tracker->calibrate();
_tracker->reset();
}
else
{
Expand Down
8 changes: 8 additions & 0 deletions src/geometry.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,14 @@ struct quat : gml::quat
}
};

// Converting from yaw (around Z), pitch (around Y), roll (around X) to quaternions.
Copy link
Member

@mgeier mgeier Jul 18, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is ambiguous and also not true.

  • ambiguous: the rotations are around the local coordinate axes (after the axes themselves have been rotated). I'm not sure what's the best way to say this un-ambiguously.
    I personally find it better understandable to describe it as three global rotations in a given order (first "roll" then "pitch" then "yaw").

  • not true: "pitch" is a rotation around a local X axis, because the default orientation is "north".
    At least that's how I think it should be in the SSR, other systems will differ.

This stuff is complicated, so I might well be totally wrong about it.

I think it's best just not to create an "official" function for it, since it's a one-liner anyway.

Copy link
Contributor Author

@chris-hld chris-hld Jul 19, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oh, I see... I just ran into the same problem as in the GUI head-tracker PR earlier.
We have to document this somewhere. Really.

  • I refer to yaw pitch roll as Tait-Bryan angles (I thought this is the most "common" definition).
    More specifically, the aerospace norm/ nautical angles.
    At least for me, an airplane first yaws on ground, then pitches during take-off and then rolls around its forward (nose) axis.
  • I'm not sure if I would understand "north" orientation just from reading it. I think less ambiguous is Y-forward, Z-up. That's at least how coordinate systems got communicated to me, when in doubt (always assuming right handed coordinate systems). Did I understand correctly and do you agree?

We should also make sure to use this convention of YPR consistently across all trackers and maybe even the SSR. I'm not sure if this is the case atm. I thought that at least the Razor uses X-forward, Z-up and the YPR definition above.

Copy link
Member

@mgeier mgeier Jul 19, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We have to document this somewhere. Really.

I agree.
My first attempt is there: https://audioscenedescriptionformat.readthedocs.io/en/latest/position-orientation.html

But this can be for sure improved, and we should mention it in the SSR docs somewhere.

I refer to yaw pitch roll as Tait-Bryan angles (I thought this is the most "common" definition).

Yeah, I prefer those, too. But there are still 6 possible conventions (not considering left-handed systems and not even considering where x, y and z point to in the real world).

I don't really like the aerospace convention because the z axis points down. I don't think that's intuitive in our situation and for our community.

I'm not sure if I would understand "north" orientation just from reading it. I think less ambiguous is Y-forward, Z-up.

I would hope that neither is ambiguous. Do you think it is?
I agree that north/east/south/west is a secondary way of looking at it, but it might still be helpful for some people.
And "north" is definitely more concise than "positive y axis".
And there is definitely a tradition of naming those things after compass directions, e.g. "ENU" and "NED" (see https://en.wikipedia.org/wiki/Axes_conventions).

I think that both are equally valid ways of describing the same thing.

We should also make sure to use this convention of YPR consistently across all trackers

For those tracker APIs that provide quaternions, this whole problem doesn't matter (except for left/right-handedness I guess).

For those that provide angles, we just have to figure out the convention used in their API and correctly convert it to ssr::quat or ssr::Rot.

But each tracker might use their own convention, therefore I don't think it makes sense to provide a general "ypr2quat()" function.

I thought that at least the Razor uses X-forward, Z-up and the YPR definition above.

Maybe. But that doesn't mean we have to use that convention for anything else.

It should really be an implementation detail of each tracker, the important thing is that we get a quaternion out.

Copy link
Member

@mgeier mgeier Jul 20, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In the meantime I've created a new page in the ASDF docs: https://audioscenedescriptionformat.readthedocs.io/en/latest/rotation-matrices.html

Please tell me your suggestions for improvements. I hope there are no ambiguities left.

At some point I'll probably make a similar page using quaternions, but I think the rotation matrix stuff might be easier to understand anyway.

Copy link
Member

@mgeier mgeier Oct 1, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've added a page about quaternions: https://audioscenedescriptionformat.readthedocs.io/en/latest/quaternions.html

// This a lot worse than the other way around and should be avoided.
Copy link
Member

@mgeier mgeier Jul 18, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Depending on your definition of "worse", this might be exactly wrong.

This way is a unique transformation (except for the fact that each rotation can be represented by exactly two quaternions), the other way around is not unique, since there are rotations that can have infinitely many yaw/pitch/roll representations.

Really, the other way around should be avoided!

Copy link
Contributor Author

@chris-hld chris-hld Jul 19, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah, of course. This is just plain wrong.
I was trying to say stay in quaternions and don't convert back to YPR.

Copy link
Member

@mgeier mgeier Jul 19, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was trying to say stay in quaternions and don't convert back to YPR.

Yes, that should be the advice!

template <typename T>
ssr::quat ypr2quaternion(T yaw, T pitch, T roll)
{
return gml::qrotate(gml::vec3{roll, pitch, yaw});
Copy link
Member

@mgeier mgeier Jul 18, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Another way to write this is:

return gml::qrotate(gml::radians(azimuth),   {0.0f, 0.0f, 1.0f})
     * gml::qrotate(gml::radians(elevation), {1.0f, 0.0f, 0.0f})
     * gml::qrotate(gml::radians(roll),      {0.0f, 1.0f, 0.0f});

See e.g. https://github.com/AudioSceneDescriptionFormat/asdfpp-jack-ecasound/blob/f6d5c019fec5520e85256fa0f5379632a0f80aff/src/asdfpp.cpp#L183-L185

I have no clue whether those two are equivalent, because the order of rotations and the order of axes is not visible in the single call to gml::qrotate().

I like having both orders visible.

I don't know which one of the alternatives is faster, would be interesting to benchmark this ...

}

/// Build a unit quaternion representing the rotation
/// from u to v. The input vectors need not be normalised.
/// From http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
Expand Down
12 changes: 8 additions & 4 deletions src/gui/qopenglplotter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -382,8 +382,10 @@ void ssr::QOpenGLPlotter::_draw_reference()

glTranslatef(0.03f, -0.03f, 0.0f);

// rotate according to reference position
glRotatef(_scene.get_reference().orientation.azimuth, 0.0f, 0.0f, 1.0f);
// rotate according to reference offset position
glRotatef(_scene.get_reference().orientation.azimuth +
_scene.get_reference_offset().orientation.azimuth,
0.0f, 0.0f, 1.0f);
Copy link
Member

@mgeier mgeier Jul 18, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As long as the "rotation" and the "rotation offset" are indistinguishable in the interface for the binaural renderer, I agree that adding both makes sense.

But then you should also add "position" and "position offset".


glBindTexture(GL_TEXTURE_2D, _listener_shadow_texture);

Expand All @@ -396,8 +398,10 @@ void ssr::QOpenGLPlotter::_draw_reference()

glPopMatrix();

// rotate according to reference position
glRotatef(_scene.get_reference().orientation.azimuth, 0.0f, 0.0f, 1.0f);
// rotate according to reference offset position
glRotatef(_scene.get_reference().orientation.azimuth +
_scene.get_reference_offset().orientation.azimuth,
0.0f, 0.0f, 1.0f);

glBindTexture(GL_TEXTURE_2D, _listener_texture);

Expand Down
13 changes: 7 additions & 6 deletions src/gui/quserinterface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -980,8 +980,8 @@ void ssr::QUserInterface::mouseMoveEvent(QMouseEvent *event)
// absolut mouse position in OpenGL coordinates
Position mouse_pos(static_cast<float>(pos_x), static_cast<float>(pos_y));

// position relative to reference position
mouse_pos -= _scene.get_reference().position;
// position relative to reference position offset
mouse_pos -= _scene.get_reference_offset().position;

_get_openGL_pos(_previous_mouse_event.x(),
_previous_mouse_event.y(),
Expand All @@ -990,11 +990,12 @@ void ssr::QUserInterface::mouseMoveEvent(QMouseEvent *event)
// previous absolut position in OpenGL coordinates
Position prev_mouse_pos(static_cast<float>(pos_x), static_cast<float>(pos_y));

// previous position relative to relative position
prev_mouse_pos -= _scene.get_reference().position;
// previous position relative to relative position offset
prev_mouse_pos -= _scene.get_reference_offset().position;

_controller.take_control()->reference_rotation(_scene.get_reference().orientation +
(mouse_pos.orientation() - prev_mouse_pos.orientation()));
_controller.take_control()->reference_rotation_offset(_scene.get_reference().orientation +
_scene.get_reference_offset().orientation +
(mouse_pos.orientation() - prev_mouse_pos.orientation()));
Copy link
Member

@mgeier mgeier Jul 18, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think the mouse should affect the "rotation offset".

Using the right mouse button doesn't affect the "position offset" either.

Copy link
Member

@mgeier mgeier Jul 18, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

When I try rotating the head (using the binaural renderer), the orientation is jumping around eratically. Something is wrong there ...


} // else if

Expand Down
2 changes: 0 additions & 2 deletions src/razor-ahrs/RazorAHRS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -131,11 +131,9 @@ class RazorAHRS

// thread related stuff
std::thread _tracker_thread;

std::atomic<bool> _stop_thread; // thread stop flag
void _start(); ///< start the tracking thread
void _stop(); ///< stop the tracking thread

void _thread(); // thread main function
};

Expand Down
41 changes: 37 additions & 4 deletions src/tracker.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,13 +30,46 @@
#ifndef SSR_TRACKER_H
#define SSR_TRACKER_H

#include <atomic>
#include <thread>

#include "geometry.h"


namespace ssr
{

/// Class definition
struct Tracker
class Tracker
{
virtual ~Tracker() = default; ///< destructor
public:
Tracker(api::Publisher& controller) : _controller(controller){}; ///< constructor

/// calibrate tracker; set the instantaneous position to be the reference
virtual void calibrate() = 0;
virtual ~Tracker() = default; ///< destructor

/// reset tracker; set the instantaneous position to be the reference
virtual void reset()
{
SSR_VERBOSE2("Tracker reset.");
this->_correction_rot = this->_current_rot;
}

// Update SSR
virtual void update()
{
ssr::quat r = inverse(_correction_rot) * inverse(_current_rot);
Copy link
Member

@mgeier mgeier Jul 18, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why do you call inverse() twice?

Shouldn't it be enough to call inverse() once in reset() and here only left-multiply with the already inversed value?

_controller.take_control()->reference_rotation_offset(r);
}

protected:
// Current tracker data (should be atomic)
ssr::quat _current_rot;
ssr::quat _correction_rot;

private:
api::Publisher& _controller;
};

} // namespace ssr

#endif
33 changes: 20 additions & 13 deletions src/trackerpolhemus.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,16 +38,13 @@
#include "api.h" // for Publisher
#include "legacy_orientation.h" // for Orientation
#include "trackerpolhemus.h"
#include "ssr_global.h"
#include "apf/stringtools.h"

using apf::str::A2S;

ssr::TrackerPolhemus::TrackerPolhemus(api::Publisher& controller
, const std::string& type, const std::string& ports)
: Tracker()
, _controller(controller)
, _az_corr(0.0f)
: Tracker(controller)
, _stop_thread(false)
{
if (ports == "")
Expand Down Expand Up @@ -141,7 +138,7 @@ ssr::TrackerPolhemus::TrackerPolhemus(api::Publisher& controller
// wait until tracker has started
std::this_thread::sleep_for(std::chrono::milliseconds(50));

this->calibrate();
Tracker::reset();
}

ssr::TrackerPolhemus::~TrackerPolhemus()
Expand Down Expand Up @@ -189,12 +186,6 @@ ssr::TrackerPolhemus::_open_serial_port(const char *portname)
return -1;
}

void
ssr::TrackerPolhemus::calibrate()
{
_az_corr = _current_data.azimuth;
}

void
ssr::TrackerPolhemus::_start()
{
Expand Down Expand Up @@ -225,6 +216,17 @@ ssr::TrackerPolhemus::_thread()
fds.fd = _tracker_port;
fds.events = POLLRDNORM;

struct
{
float header{};
float x{};
float y{};
float z{};
float azimuth{};
float elevation{};
float roll{};
} _current_data{} ;

while (!_stop_thread)
{
c = 0;
Expand Down Expand Up @@ -291,7 +293,12 @@ ssr::TrackerPolhemus::_thread()
>> _current_data.elevation
>> _current_data.roll;

_controller.take_control()->reference_rotation_offset(
Orientation(-_current_data.azimuth + _az_corr));
// Write back to tracker_data
Tracker::_current_rot = ypr2quaternion(_current_data.azimuth,
_current_data.elevation,
_current_data.roll);

// Push updates to SSR
Tracker::update();
};
}
33 changes: 4 additions & 29 deletions src/trackerpolhemus.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,9 @@
#include <memory>
#include <stdexcept> // for std::runtime_error

#include "tracker.h"
#include "ssr_global.h" // for ERROR, VERBOSE
#include "tracker.h" // base class


namespace ssr
{
Expand All @@ -55,48 +57,21 @@ class TrackerPolhemus : public Tracker
static ptr_t create(api::Publisher& controller, const std::string& type
, const std::string& ports);

virtual void calibrate();

private:
/// constructor
TrackerPolhemus(api::Publisher& controller, const std::string& type
, const std::string& ports);

struct tracker_data_t
{
float header;
float x;
float y;
float z;
float azimuth;
float elevation;
float roll;

// contructor
tracker_data_t()
: header(0.0f), x(0.0f), y(0.0f), z(0.0f)
, azimuth(0.0f), elevation(0.0f), roll(0.0f)
{}
};

api::Publisher& _controller;

tracker_data_t _current_data;

int _tracker_port;
int _open_serial_port(const char *portname);

float _az_corr; ///< correction of the azimuth due to calibration

int _open_serial_port(const char *portname);
std::string::size_type _line_size;

// thread related stuff
std::thread _tracker_thread;

std::atomic<bool> _stop_thread; // thread stop flag
void _start(); ///< start the tracking thread
void _stop(); ///< stop the tracking thread

void _thread(); // thread main function
};

Expand Down
11 changes: 6 additions & 5 deletions src/trackerrazor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,11 +32,7 @@

ssr::TrackerRazor::TrackerRazor(api::Publisher& controller
, const std::string& ports)
: Tracker()
, _controller(controller)
, _current_azimuth(0.0f)
, _az_corr(90.0f)
, _init_az_corr(true)
: Tracker(controller)
, _tracker(nullptr)
{
if (ports == "")
Expand Down Expand Up @@ -72,6 +68,11 @@ ssr::TrackerRazor::TrackerRazor(api::Publisher& controller
{
throw std::runtime_error("Could not open serial port!");
}

// wait until tracker has started
std::this_thread::sleep_for(std::chrono::milliseconds(50));

Tracker::reset();
}

ssr::TrackerRazor::ptr_t
Expand Down
27 changes: 9 additions & 18 deletions src/trackerrazor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,9 @@
#define SSR_TRACKERRAZOR_H

#include "legacy_orientation.h" // for Orientation
#include "ssr_global.h" // for ERROR
#include "ssr_global.h" // for ERROR, VERBOSE
#include "tracker.h" // base class
#include "apf/math.h" // for deg2rad()

#include "razor-ahrs/RazorAHRS.h"

Expand All @@ -57,33 +58,23 @@ class TrackerRazor : public Tracker
if (_tracker != nullptr) delete _tracker;
}

virtual void calibrate() { _az_corr = _current_azimuth; }

private:
/// constructor
TrackerRazor(api::Publisher& controller, const std::string& ports);

RazorAHRS* _tracker;

/// Razor AHRS callback functions
void on_data(const float ypr[])
{
// TODO: get 3D rotation
_current_azimuth = ypr[0];
if (_init_az_corr)
{
calibrate();
_init_az_corr = false;
}
_controller.take_control()->reference_rotation_offset(
Orientation(-_current_azimuth + _az_corr));
Tracker::_current_rot = ypr2quaternion(apf::math::deg2rad(ypr[0]),
apf::math::deg2rad(ypr[1]),
apf::math::deg2rad(ypr[2]));
// Push updates to SSR
Tracker::update();
}
void on_error(const std::string &msg) { SSR_ERROR("Razor AHRS: " << msg); }

api::Publisher& _controller;
volatile float _current_azimuth;
volatile float _az_corr;
volatile bool _init_az_corr;

RazorAHRS* _tracker;
};

} // namespace ssr
Expand Down
Loading