| 55 | |
| 56 | template <typename T> |
| 57 | VectorT<T, 3> RelativeMotion2D(const T* PointOld, const T* YawOld, const T* PointNew, const T* const YawNew) |
| 58 | { |
| 59 | const VectorT<T, 2> POld(PointOld[0], PointOld[1]); |
| 60 | const VectorT<T, 2> PNew(PointNew[0], PointNew[1]); |
| 61 | |
| 62 | VectorT<T, 3> RelativeMotion; |
| 63 | |
| 64 | RelativeMotion.head(2) = RotationMatrix2D(YawOld[0]).transpose() * (PNew - POld); |
| 65 | RelativeMotion(2) = NormalizeAngle(YawNew[0] - YawOld[0]); |
| 66 | |
| 67 | return RelativeMotion; |
| 68 | } |
| 69 | |
| 70 | /** 3D */ |
| 71 |
no test coverage detected