Line data Source code
1 : #ifndef BA54EBB4_2356_43F9_BB94_0BC11475E511
2 : #define BA54EBB4_2356_43F9_BB94_0BC11475E511
3 :
4 : #include "motion/state_cov_converter.h"
5 :
6 : namespace tracking
7 : {
8 : namespace motion
9 : {
10 :
11 : template <typename MM>
12 : inline void StateCovConverter<MM, MM>::convertFrom(typename MM::StateCov& dstCov, const typename MM::StateCov& srcCov)
13 : {
14 : if (&srcCov != &dstCov)
15 : {
16 : dstCov = srcCov;
17 : }
18 : }
19 :
20 : template <typename CovarianceMatrixPolicy_>
21 2 : inline void StateCovConverter<MotionModelCV<CovarianceMatrixPolicy_>, MotionModelCA<CovarianceMatrixPolicy_>>::convertFrom(
22 : typename MotionModelCV<CovarianceMatrixPolicy_>::StateCov& dstCov,
23 : const typename MotionModelCA<CovarianceMatrixPolicy_>::StateCov& srcCov)
24 : {
25 : using DstType = MotionModelCV<CovarianceMatrixPolicy_>;
26 : using SrcType = MotionModelCA<CovarianceMatrixPolicy_>;
27 :
28 : if constexpr (CovarianceMatrixPolicy_::is_factored)
29 : {
30 : using value_type = typename CovarianceMatrixPolicy_::value_type;
31 1 : constexpr auto one = static_cast<value_type>(1.0);
32 :
33 : // create a permutation matrix from SrcType to DstType
34 1 : math::SquareMatrix<value_type, SrcType::NUM_STATE_VARIABLES> A;
35 1 : A.setZeros();
36 1 : A.at_unsafe(DstType::X, SrcType::X) = one;
37 1 : A.at_unsafe(DstType::VX, SrcType::VX) = one;
38 1 : A.at_unsafe(DstType::Y, SrcType::Y) = one;
39 1 : A.at_unsafe(DstType::VY, SrcType::VY) = one;
40 :
41 : // fill dstCov with the resulting top left block
42 1 : dstCov.template fill<SrcType::NUM_STATE_VARIABLES, DstType::NUM_STATE_VARIABLES>(srcCov.apaT(A));
43 1 : }
44 : else
45 : {
46 : static_assert(DstType::VX == DstType::X + 1);
47 : static_assert(DstType::VY == DstType::Y + 1);
48 : static_assert(SrcType::VX == SrcType::X + 1);
49 : static_assert(SrcType::VY == SrcType::Y + 1);
50 : // copy x,vx and its correlations
51 : dstCov.template setBlock<SrcType::NUM_STATE_VARIABLES,
52 : SrcType::NUM_STATE_VARIABLES,
53 : 2,
54 : 2,
55 : SrcType::X,
56 : SrcType::X,
57 : SrcType::StateCov::IsRowMajor,
58 : DstType::X,
59 1 : DstType::X>(srcCov);
60 : // copy cross correlations between x,vx and y,vy
61 : dstCov.template setBlock<SrcType::NUM_STATE_VARIABLES,
62 : SrcType::NUM_STATE_VARIABLES,
63 : 2,
64 : 2,
65 : SrcType::X,
66 : SrcType::Y,
67 : SrcType::StateCov::IsRowMajor,
68 : DstType::X,
69 1 : DstType::Y>(srcCov);
70 : dstCov.template setBlock<SrcType::NUM_STATE_VARIABLES,
71 : SrcType::NUM_STATE_VARIABLES,
72 : 2,
73 : 2,
74 : SrcType::Y,
75 : SrcType::X,
76 : SrcType::StateCov::IsRowMajor,
77 : DstType::Y,
78 1 : DstType::X>(srcCov);
79 : // copy y,vy and its correlations
80 : dstCov.template setBlock<SrcType::NUM_STATE_VARIABLES,
81 : SrcType::NUM_STATE_VARIABLES,
82 : 2,
83 : 2,
84 : SrcType::Y,
85 : SrcType::Y,
86 : SrcType::StateCov::IsRowMajor,
87 : DstType::Y,
88 1 : DstType::Y>(srcCov);
89 : }
90 2 : }
91 :
92 : template <typename CovarianceMatrixPolicy_>
93 2 : inline void StateCovConverter<MotionModelCA<CovarianceMatrixPolicy_>, MotionModelCV<CovarianceMatrixPolicy_>>::convertFrom(
94 : typename MotionModelCA<CovarianceMatrixPolicy_>::StateCov& dstCov,
95 : const typename MotionModelCV<CovarianceMatrixPolicy_>::StateCov& srcCov)
96 : {
97 : using DstType = MotionModelCA<CovarianceMatrixPolicy_>;
98 : using SrcType = MotionModelCV<CovarianceMatrixPolicy_>;
99 :
100 : if constexpr (CovarianceMatrixPolicy_::is_factored)
101 : {
102 : using value_type = typename CovarianceMatrixPolicy_::value_type;
103 1 : constexpr auto one = static_cast<value_type>(1.0);
104 :
105 1 : math::SquareMatrix<value_type, DstType::NUM_STATE_VARIABLES> A;
106 1 : A.setZeros();
107 1 : A.at_unsafe(DstType::X, SrcType::X) = one;
108 1 : A.at_unsafe(DstType::VX, SrcType::VX) = one;
109 1 : A.at_unsafe(DstType::Y, SrcType::Y) = one;
110 1 : A.at_unsafe(DstType::VY, SrcType::VY) = one;
111 :
112 1 : dstCov.setIdentity();
113 : // copy CV into CA
114 1 : dstCov.template fill<SrcType::NUM_STATE_VARIABLES, SrcType::NUM_STATE_VARIABLES>(srcCov);
115 : // remap indeces by applying the permutation
116 1 : dstCov.apaT(A);
117 : // set ax,ay to variance 1.0
118 1 : dstCov.D(DstType::AX, one);
119 1 : dstCov.D(DstType::AY, one);
120 1 : }
121 : else
122 : {
123 : static_assert(DstType::VX == DstType::X + 1);
124 : static_assert(DstType::VY == DstType::Y + 1);
125 : static_assert(SrcType::VX == SrcType::X + 1);
126 : static_assert(SrcType::VY == SrcType::Y + 1);
127 : // set ax,ay variance to 1.0
128 1 : dstCov.setIdentity();
129 : // copy x,vx and its correlations
130 : dstCov.template setBlock<SrcType::NUM_STATE_VARIABLES,
131 : SrcType::NUM_STATE_VARIABLES,
132 : 2,
133 : 2,
134 : SrcType::X,
135 : SrcType::X,
136 : SrcType::StateCov::IsRowMajor,
137 : DstType::X,
138 1 : DstType::X>(srcCov);
139 : // copy cross correlations between x,vx and y,vy
140 : dstCov.template setBlock<SrcType::NUM_STATE_VARIABLES,
141 : SrcType::NUM_STATE_VARIABLES,
142 : 2,
143 : 2,
144 : SrcType::X,
145 : SrcType::Y,
146 : SrcType::StateCov::IsRowMajor,
147 : DstType::X,
148 1 : DstType::Y>(srcCov);
149 : dstCov.template setBlock<SrcType::NUM_STATE_VARIABLES,
150 : SrcType::NUM_STATE_VARIABLES,
151 : 2,
152 : 2,
153 : SrcType::Y,
154 : SrcType::X,
155 : SrcType::StateCov::IsRowMajor,
156 : DstType::Y,
157 1 : DstType::X>(srcCov);
158 : // copy y,vy and its correlations
159 : dstCov.template setBlock<SrcType::NUM_STATE_VARIABLES,
160 : SrcType::NUM_STATE_VARIABLES,
161 : 2,
162 : 2,
163 : SrcType::Y,
164 : SrcType::Y,
165 : SrcType::StateCov::IsRowMajor,
166 : DstType::Y,
167 1 : DstType::Y>(srcCov);
168 : }
169 2 : }
170 :
171 : } // namespace motion
172 : } // namespace tracking
173 :
174 : #endif // BA54EBB4_2356_43F9_BB94_0BC11475E511
|
