Line data    Source code

       1              : #ifndef AE0B3445_6354_4B13_BF8F_59880FFFC470
       2              : #define AE0B3445_6354_4B13_BF8F_59880FFFC470
       3              : 
       4              : #include "motion/motion_model_cv.h"
       5              : 
       6              : #include "motion/state_cov_converter.hpp" // IWYU pragma: keep
       7              : #include "motion/state_vec_converter.hpp" // IWYU pragma: keep
       8              : #include <iostream>
       9              : 
      10              : namespace tracking
      11              : {
      12              : namespace motion
      13              : {
      14              : 
      15              : template <typename CovarianceMatrixPolicy_>
      16           10 : MotionModelCV<CovarianceMatrixPolicy_>::MotionModelCV(const StateVec& vec, const StateCov& cov)
      17           10 :     : BaseExtendedMotionModel{vec, cov}
      18              : {
      19           10 : }
      20              : 
      21              : template <typename CovarianceMatrixPolicy_>
      22           20 : void MotionModelCV<CovarianceMatrixPolicy_>::computeEgoMotionCompensationMatrices(EgoMotionMappingMatrix& Ge,
      23              :                                                                                   StateMatrix&            Go,
      24              :                                                                                   const EgoMotionType&    egoMotion)
      25              : {
      26           20 :   const value_type& x  = this->operator[](StateDefCV::X);
      27           20 :   const value_type& y  = this->operator[](StateDefCV::Y);
      28           20 :   const value_type& vx = this->operator[](StateDefCV::VX);
      29           20 :   const value_type& vy = this->operator[](StateDefCV::VY);
      30              : 
      31           20 :   const value_type sinDeltaPsiEgo = egoMotion.getDisplacementCog().sinDeltaPsi;
      32           20 :   const value_type cosDeltaPsiEgo = egoMotion.getDisplacementCog().cosDeltaPsi;
      33           20 :   const value_type deltaXEgo      = egoMotion.getDisplacementCog().vec.at_unsafe(EgoMotionType::DS_X);
      34           20 :   const value_type deltaYEgo      = egoMotion.getDisplacementCog().vec.at_unsafe(EgoMotionType::DS_Y);
      35           20 :   const value_type distCog2Ego    = egoMotion.getGeometry().distCog2Ego;
      36              : 
      37           20 :   Go.setZeros();
      38           20 :   Go.at_unsafe(X, X)   = cosDeltaPsiEgo;
      39           20 :   Go.at_unsafe(X, Y)   = sinDeltaPsiEgo;
      40           20 :   Go.at_unsafe(Y, X)   = -sinDeltaPsiEgo;
      41           20 :   Go.at_unsafe(Y, Y)   = cosDeltaPsiEgo;
      42           20 :   Go.at_unsafe(VX, VX) = cosDeltaPsiEgo;
      43           20 :   Go.at_unsafe(VX, VY) = sinDeltaPsiEgo;
      44           20 :   Go.at_unsafe(VY, VX) = -sinDeltaPsiEgo;
      45           20 :   Go.at_unsafe(VY, VY) = cosDeltaPsiEgo;
      46              : 
      47           20 :   const value_type x0 = -deltaYEgo + y;
      48           20 :   const value_type x1 = deltaXEgo - distCog2Ego - x;
      49           20 :   Ge.setZeros();
      50           20 :   Ge.at_unsafe(X, EgoMotionType::DS_X)    = -cosDeltaPsiEgo;
      51           20 :   Ge.at_unsafe(X, EgoMotionType::DS_Y)    = -sinDeltaPsiEgo;
      52           20 :   Ge.at_unsafe(X, EgoMotionType::DS_PSI)  = (x0 * cosDeltaPsiEgo) + (x1 * sinDeltaPsiEgo);
      53           20 :   Ge.at_unsafe(Y, EgoMotionType::DS_X)    = sinDeltaPsiEgo;
      54           20 :   Ge.at_unsafe(Y, EgoMotionType::DS_Y)    = cosDeltaPsiEgo;
      55           20 :   Ge.at_unsafe(Y, EgoMotionType::DS_PSI)  = -(x0 * sinDeltaPsiEgo) + (x1 * cosDeltaPsiEgo);
      56           20 :   Ge.at_unsafe(VX, EgoMotionType::DS_PSI) = -(vx * sinDeltaPsiEgo) + (vy * cosDeltaPsiEgo);
      57           20 :   Ge.at_unsafe(VY, EgoMotionType::DS_PSI) = -(vx * cosDeltaPsiEgo) - (vy * sinDeltaPsiEgo);
      58           20 : }
      59              : 
      60              : template <typename CovarianceMatrixPolicy_>
      61           20 : void MotionModelCV<CovarianceMatrixPolicy_>::compensateState(const EgoMotionType& egoMotion)
      62              : {
      63           20 :   value_type& x  = this->operator[](StateDefCV::X);
      64           20 :   value_type& y  = this->operator[](StateDefCV::Y);
      65           20 :   value_type& vx = this->operator[](StateDefCV::VX);
      66           20 :   value_type& vy = this->operator[](StateDefCV::VY);
      67              : 
      68              :   // translate and rotate position
      69           20 :   egoMotion.compensatePosition(x, y, x, y);
      70              :   // rotate velocity and acceleration
      71           20 :   egoMotion.compensateDirection(vx, vy, vx, vy);
      72           20 : }
      73              : 
      74              : template <typename CovarianceMatrixPolicy_>
      75           40 : void MotionModelCV<CovarianceMatrixPolicy_>::applyProcessModel(const value_type dt)
      76              : {
      77           40 :   const StateVec& stateVec = this->getVec();
      78              : 
      79           40 :   this->operator[](StateDefCV::X) += dt * stateVec.at_unsafe(StateDefCV::VX);
      80           40 :   this->operator[](StateDefCV::Y) += dt * stateVec.at_unsafe(StateDefCV::VY);
      81           40 : }
      82              : 
      83              : template <typename CovarianceMatrixPolicy_>
      84           40 : void MotionModelCV<CovarianceMatrixPolicy_>::computeA(StateMatrix& A, const value_type dt) const
      85              : {
      86           40 :   A.setIdentity();
      87           40 :   A.at_unsafe(StateDefCV::X, StateDefCV::VX) = dt;
      88           40 :   A.at_unsafe(StateDefCV::Y, StateDefCV::VY) = dt;
      89           40 : }
      90              : 
      91              : template <typename CovarianceMatrixPolicy_>
      92           40 : void MotionModelCV<CovarianceMatrixPolicy_>::computeQ(ProcessNoiseDiagMatrix& Q, const value_type /* dt */)
      93              : {
      94              :   // DWNA process, elements in Q define an acceleration, i.e. unit is equal to m/s**2
      95           40 :   Q.at_unsafe(Q_VX) = static_cast<value_type>(10.0);
      96           40 :   Q.at_unsafe(Q_VY) = static_cast<value_type>(10.0);
      97           40 : }
      98              : 
      99              : template <typename CovarianceMatrixPolicy_>
     100           40 : void MotionModelCV<CovarianceMatrixPolicy_>::computeG(ProcessNoiseMappingMatrix& G, const value_type dt)
     101              : {
     102           40 :   const value_type halfDeltaTimePow2 = static_cast<value_type>(0.5) * dt * dt;
     103              : 
     104           40 :   G.setZeros();
     105           40 :   G.at_unsafe(X, Q_VX)  = halfDeltaTimePow2;
     106           40 :   G.at_unsafe(VX, Q_VX) = dt;
     107           40 :   G.at_unsafe(Y, Q_VY)  = halfDeltaTimePow2;
     108           40 :   G.at_unsafe(VY, Q_VY) = dt;
     109           40 : }
     110              : 
     111              : template <typename CovarianceMatrixPolicy_>
     112            2 : void MotionModelCV<CovarianceMatrixPolicy_>::convertFrom(const MotionModelCA<CovarianceMatrixPolicy_>& other)
     113              : {
     114              :   using other_type = MotionModelCA<CovarianceMatrixPolicy_>;
     115            2 :   StateVecConverter<instance_type, other_type>::convertFrom(this->getVec(), other.getVec());
     116            2 :   StateCovConverter<instance_type, other_type>::convertFrom(this->getCov(), other.getCov());
     117            2 : }
     118              : 
     119              : } // namespace motion
     120              : } // namespace tracking
     121              : 
     122              : #endif // AE0B3445_6354_4B13_BF8F_59880FFFC470