14 #include <Eigen/Dense>
33 std::map<mrpt::Clock::time_point, mrpt::poses::CPose3D> old_path;
36 for (
const auto& p : old_path)
38 m_path[p.first] = p.second.asTPose();
64 using doubleDuration = std::chrono::duration<double>;
65 doubleDuration durationT = t.time_since_epoch();
66 double td = durationT.count();
68 std::chrono::duration_cast<doubleDuration>(p1.first.time_since_epoch())
71 std::chrono::duration_cast<doubleDuration>(p2.first.time_since_epoch())
74 std::chrono::duration_cast<doubleDuration>(p3.first.time_since_epoch())
77 std::chrono::duration_cast<doubleDuration>(p4.first.time_since_epoch())
93 yaw[0] = p1.second.yaw;
94 pitch[0] = p1.second.pitch;
95 roll[0] = p1.second.roll;
96 yaw[1] = p2.second.yaw;
97 pitch[1] = p2.second.pitch;
98 roll[1] = p2.second.roll;
99 yaw[2] = p3.second.yaw;
100 pitch[2] = p3.second.pitch;
101 roll[2] = p3.second.roll;
102 yaw[3] = p4.second.yaw;
103 pitch[3] = p4.second.pitch;
104 roll[3] = p4.second.roll;
136 td, ts[1], yaw[1], ts[2], yaw[2],
true);
140 td, ts[1],
roll[1], ts[2],
roll[2],
true);
147 math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, X);
149 math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
151 math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
153 math::leastSquareLinearFit<double, decltype(ts), 4>(
156 math::leastSquareLinearFit<double, decltype(ts), 4>(
157 td, ts,
pitch,
true);
159 math::leastSquareLinearFit<double, decltype(ts), 4>(
169 math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
171 math::leastSquareLinearFit<double, decltype(ts), 4>(
174 math::leastSquareLinearFit<double, decltype(ts), 4>(
175 td, ts,
pitch,
true);
177 math::leastSquareLinearFit<double, decltype(ts), 4>(
187 math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
190 math::leastSquareLinearFit<double, decltype(ts), 4>(
191 td, ts,
pitch,
true);
193 math::leastSquareLinearFit<double, decltype(ts), 4>(
200 const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
202 TPose3D(0, 0, 0, yaw[1],
pitch[1],
roll[1]),
203 TPose3D(0, 0, 0, yaw[2],
pitch[2],
roll[2]), ratio, out_interp);
216 const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
218 TPose3D(0, 0, 0, yaw[1],
pitch[1],
roll[1]),
219 TPose3D(0, 0, 0, yaw[2],
pitch[2],
roll[2]), ratio, out_interp);