summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2019-04-05 20:28:02 +0200
committerStanislaw Halik <sthalik@misaki.pl>2019-04-05 20:30:04 +0200
commit61592edd7b953c35167fb95f1d26aecc4e5c594a (patch)
treedd447e625d972ced090b19540fcc897e4d3f47d2
parent7289933633b0c231d45c799fcf6523775337a98e (diff)
logic/pipeline: kill improper center mode
Issue: #912, #925
-rw-r--r--logic/pipeline.cpp22
-rw-r--r--logic/pipeline.hpp5
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 {};