summaryrefslogtreecommitdiffhomepage
path: root/opentrack
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-10-24 04:55:35 +0200
committerStanislaw Halik <sthalik@misaki.pl>2014-10-24 07:04:17 +0200
commit85aa1942a789c3df99a3a5b1a87485cc050be28e (patch)
tree1dfdecb7e7a10694557687854606baa583a4bfb2 /opentrack
parent69eb5f7b8c712482bb960f49e5c9ec717a902858 (diff)
Rotation basis algebra for centering
Reported-by: @doveman months ago, many times Issue: #63 @dbaarda please confirm that- or -whether- it makes any sense. Issue: #86 It could be either worse or better than before. Please specify. Sadly, no time to plug the videos as camera input to PT as of yet. Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>
Diffstat (limited to 'opentrack')
-rw-r--r--opentrack/pose.hpp11
-rw-r--r--opentrack/quat.hpp48
-rw-r--r--opentrack/tracker.cpp36
-rw-r--r--opentrack/tracker.h12
4 files changed, 61 insertions, 46 deletions
diff --git a/opentrack/pose.hpp b/opentrack/pose.hpp
index 41e984f5..d0aadcd9 100644
--- a/opentrack/pose.hpp
+++ b/opentrack/pose.hpp
@@ -13,7 +13,7 @@ private:
double axes[6];
public:
- Pose() : axes {0,0,0, 0,0,0 } {}
+ Pose() : axes {0,0,0, 0,0,0} {}
inline operator double*() { return axes; }
inline operator const double*() const { return axes; }
@@ -32,13 +32,4 @@ public:
q.to_euler_degrees(ret(Yaw), ret(Pitch), ret(Roll));
return ret;
}
-
- Pose operator&(const Pose& B) const
- {
- const Quat q = quat() * B.quat().inv();
- Pose ret = fromQuat(q);
- for (int i = TX; i < TX + 3; i++)
- ret(i) = axes[i] - B.axes[i];
- return ret;
- }
};
diff --git a/opentrack/quat.hpp b/opentrack/quat.hpp
index 6d777b28..aec87f4c 100644
--- a/opentrack/quat.hpp
+++ b/opentrack/quat.hpp
@@ -12,19 +12,19 @@ class Quat {
private:
static constexpr double pi = 3.141592653;
static constexpr double r2d = 180./pi;
- double a,b,c,d; // quaternion coefficients
+ double _0,_1,_2,_3; // quaternion coefficients
public:
- Quat() : a(1.),b(0.),c(0.),d(0.) {}
- Quat(double yaw, double pitch, double roll) { from_euler_rads(yaw, pitch, roll); }
- Quat(double a, double b, double c, double d) : a(a),b(b),c(c),d(d) {}
+ Quat() : _0(1.),_1(0.),_2(0.),_3(0.) {}
+ Quat(double yaw, double pitch, double roll) { *this = from_euler_rads(yaw, pitch, roll); }
+ Quat(double a, double b, double c, double d) : _0(a),_1(b),_2(c),_3(d) {}
- Quat inv(){
- return Quat(a,-b,-c, -d);
+ Quat inv() const {
+ return Quat(_0,-_1,-_2, -_3);
}
// conversions
// see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
- void from_euler_rads(double yaw, double pitch, double roll)
+ static Quat from_euler_rads(double yaw, double pitch, double roll)
{
const double sin_phi = sin(roll/2.);
@@ -34,17 +34,21 @@ public:
const double sin_psi = sin(yaw/2.);
const double cos_psi = cos(yaw/2.);
- a = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi;
- b = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi;
- c = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi;
- d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi;
+ Quat q;
+
+ q._0 = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi;
+ q._1 = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi;
+ q._2 = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi;
+ q._3 = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi;
+
+ return q;
}
void to_euler_rads(double& yaw, double& pitch, double& roll) const
{
- roll = atan2(2.*(a*b + c*d), 1. - 2.*(b*b + c*c));
- pitch = asin(2.*(a*c - b*d));
- yaw = atan2(2.*(a*d + b*c), 1. - 2.*(c*c + d*d));
+ roll = atan2(2.*(_0*_1 + _2*_3), 1. - 2.*(_1*_1 + _2*_2));
+ pitch = asin(2.*(_0*_2 - _1*_3));
+ yaw = atan2(2.*(_0*_3 + _1*_2), 1. - 2.*(_2*_2 + _3*_3));
}
void to_euler_degrees(double& yaw, double& pitch, double& roll) const
@@ -58,9 +62,15 @@ public:
const Quat operator*(const Quat& B) const
{
const Quat& A = *this;
- return Quat(A.a*B.a - A.b*B.b - A.c*B.c - A.d*B.d, // quaternion multiplication
- A.a*B.b + A.b*B.a + A.c*B.d - A.d*B.c,
- A.a*B.c - A.b*B.d + A.c*B.a + A.d*B.b,
- A.a*B.d + A.b*B.c - A.c*B.b + A.d*B.a);
- }
+ const Quat ret(A._0*B._0 - A._1*B._1 - A._2*B._2 - A._3*B._3,
+ A._0*B._1 + A._1*B._0 + A._2*B._3 - A._3*B._2,
+ A._0*B._2 - A._1*B._3 + A._2*B._0 + A._3*B._1,
+ A._0*B._3 + A._1*B._2 - A._2*B._1 + A._3*B._0);
+ const double mag_2 = ret._0 * ret._0 +
+ ret._1 * ret._1 +
+ ret._2 * ret._2 +
+ ret._3 * ret._3;
+ const double inv_mag = 1./sqrt(mag_2);
+ return Quat(1., ret._1 * inv_mag, ret._2 * inv_mag, ret._3 * inv_mag);
+ }
};
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index b40399e3..95e81c1a 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -28,7 +28,8 @@ Tracker::Tracker(main_settings& s, Mappings &m, SelectedLibraries &libs) :
centerp(false),
enabledp(true),
should_quit(false),
- libs(libs)
+ libs(libs),
+ t_b {0,0,0}
{
}
@@ -73,7 +74,7 @@ static cv::Matx33d euler_to_rmat(const double* input)
cosH * cosP,
};
- return cv::Matx33d(foo);
+ return cv::Matx33d(foo);
}
void Tracker::t_compensate(const double* input, double* output, bool rz)
@@ -91,7 +92,7 @@ void Tracker::t_compensate(const double* input, double* output, bool rz)
void Tracker::logic()
{
libs.pTracker->data(newpose);
-
+
Pose final_raw_;
if (enabledp)
@@ -110,29 +111,38 @@ void Tracker::logic()
}
final_raw = final_raw_;
}
-
+
Pose filtered_pose;
-
+
if (libs.pFilter)
libs.pFilter->filter(final_raw, filtered_pose);
else
filtered_pose = final_raw;
-
+
if (centerp)
{
centerp = false;
- raw_center = filtered_pose;
+ r_b = filtered_pose.quat();
+ for (int i = 0; i < 3; i++)
+ t_b[i] = filtered_pose(i);
+ }
+
+ Pose raw_centered;
+
+ {
+ const Quat q = filtered_pose.quat();
+ raw_centered = Pose::fromQuat(r_b.inv() * q);
+ for (int i = 0; i < 3; i++)
+ raw_centered(i) = filtered_pose(i) - t_b[i];
}
-
- Pose raw_centered = filtered_pose & raw_center;
-
+
Pose mapped_pose_precomp;
-
+
for (int i = 0; i < 6; i++)
mapped_pose_precomp(i) = map(raw_centered(i), m(i));
-
+
Pose mapped_pose;
-
+
mapped_pose = mapped_pose_precomp;
if (s.tcomp_p)
t_compensate(mapped_pose_precomp, mapped_pose, s.tcomp_tz);
diff --git a/opentrack/tracker.h b/opentrack/tracker.h
index 462f4e50..4282bc4e 100644
--- a/opentrack/tracker.h
+++ b/opentrack/tracker.h
@@ -21,18 +21,22 @@ private:
QMutex mtx;
main_settings& s;
Mappings& m;
-
+
Timer t;
- Pose output_pose, raw_6dof, raw_center, final_raw;
+ Pose output_pose, raw_6dof, final_raw;
+
double newpose[6];
std::atomic<bool> centerp;
std::atomic<bool> enabledp;
std::atomic<bool> should_quit;
SelectedLibraries const& libs;
-
+
+ Quat r_b;
+ double t_b[3];
+
double map(double pos, Mapping& axis);
void logic();
-
+
static void t_compensate(const double* input, double* output, bool rz);
void run() override;
public: