diff options
| -rw-r--r-- | logic/pipeline.cpp | 22 | ||||
| -rw-r--r-- | logic/pipeline.hpp | 5 |
2 files changed, 22 insertions, 5 deletions
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp index 2b3d9313..c540aa4e 100644 --- a/logic/pipeline.cpp +++ b/logic/pipeline.cpp @@ -281,16 +281,30 @@ void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic) libs.pFilter->center(); if (own_center_logic) - center = Pose(); + { + center.inv_R = rmat::eye(); + center.T = {}; + } else - center = value; + { + center.inv_R = euler_to_rmat(euler_t(&value[Yaw]) * (M_PI / 180)).t(); + center.T = euler_t(&value[TX]); + } } } Pose pipeline::apply_center(Pose value) const { - // this is incorrect but people like it - value = value - center; + { + for (unsigned k = 0; k < 3; k++) + value(k) -= center.T(k); + + euler_t rot = rmat_to_euler( + euler_to_rmat(euler_t(&value[Yaw]) * (M_PI / 180)) * center.inv_R + ); + for (unsigned k = 0; k < 3; k++) + value(k+3) = rot(k) * 180 / M_PI; + } for (int i = 0; i < 6; i++) // don't invert after reltrans diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp index 1471b310..cf87dfca 100644 --- a/logic/pipeline.hpp +++ b/logic/pipeline.hpp @@ -97,7 +97,10 @@ class OTR_LOGIC_EXPORT pipeline : private QThread reltrans rel; - Pose center; + struct { + rmat inv_R = rmat::eye(); + euler_t T; + } center; time_units::ns backlog_time {}; |