14 #include <Eigen/Dense>
51 using doubleDuration = std::chrono::duration<double>;
52 doubleDuration durationT(t.time_since_epoch());
53 double td = durationT.count();
56 std::chrono::duration_cast<doubleDuration>(p1.first.time_since_epoch())
59 std::chrono::duration_cast<doubleDuration>(p2.first.time_since_epoch())
62 std::chrono::duration_cast<doubleDuration>(p3.first.time_since_epoch())
65 std::chrono::duration_cast<doubleDuration>(p4.first.time_since_epoch())
71 yaw[0] = p1.second.phi;
74 yaw[1] = p2.second.phi;
77 yaw[2] = p3.second.phi;
80 yaw[3] = p4.second.phi;
105 td, ts[1], yaw[1], ts[2], yaw[2],
true);
112 math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, X);
114 math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
116 math::leastSquareLinearFit<double, decltype(ts), 4>(
126 math::leastSquareLinearFit<double, decltype(ts), 4>(
141 const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
143 out_interp.phi = yaw[1] + ratio * Aang;
154 const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
156 out_interp.phi = yaw[1] + ratio * Aang;