9 #ifndef CKalmanFilterCapable_IMPL_H
10 #define CKalmanFilterCapable_IMPL_H
12 #ifndef CKalmanFilterCapable_H
13 #error Include this file only from 'CKalmanFilterCapable.h'
24 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
26 void CKalmanFilterCapable<
27 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::runOneKalmanIteration()
32 m_timLogger.enable(KF_options.enable_profiler);
33 m_timLogger.enter(
"KF:complete_step");
35 ASSERT_(
int(m_xkk.size()) == m_pkk.cols());
36 ASSERT_(
size_t(m_xkk.size()) >= VEH_SIZE);
42 m_timLogger.enter(
"KF:1.OnGetAction");
44 m_timLogger.leave(
"KF:1.OnGetAction");
50 (((m_xkk.size() - VEH_SIZE) / FEAT_SIZE) * FEAT_SIZE) ==
51 (m_xkk.size() - VEH_SIZE));
57 m_timLogger.enter(
"KF:2.prediction stage");
59 const size_t N_map = getNumberOfLandmarksInTheMap();
63 bool skipPrediction =
false;
71 OnTransitionModel(u, xv, skipPrediction);
83 m_user_didnt_implement_jacobian =
false;
86 if (KF_options.use_analytic_transition_jacobian)
87 OnTransitionJacobian(dfv_dxv);
89 if (m_user_didnt_implement_jacobian ||
90 !KF_options.use_analytic_transition_jacobian ||
91 KF_options.debug_verify_analytic_jacobians)
96 OnTransitionJacobianNumericGetIncrements(xkk_veh_increments);
102 const std::pair<KFCLASS*, KFArray_ACT>& dat,
103 KFArray_VEH& out_x)>(&KF_aux_estimate_trans_jacobian),
104 xkk_veh_increments, std::pair<KFCLASS*, KFArray_ACT>(
this, u),
107 if (KF_options.debug_verify_analytic_jacobians)
110 OnTransitionJacobian(dfv_dxv_gt);
111 if ((dfv_dxv - dfv_dxv_gt).array().abs().
sum() >
112 KF_options.debug_verify_analytic_jacobians_threshold)
114 std::cerr <<
"[KalmanFilter] ERROR: User analytical "
115 "transition Jacobians are wrong: \n"
116 <<
" Real dfv_dxv: \n"
117 << dfv_dxv <<
"\n Analytical dfv_dxv:\n"
118 << dfv_dxv_gt <<
"Diff:\n"
119 << (dfv_dxv - dfv_dxv_gt) <<
"\n";
121 "ERROR: User analytical transition Jacobians are wrong "
122 "(More details dumped to cerr)")
130 OnTransitionNoise(Q);
136 Eigen::Block<typename KFMatrix::Base, VEH_SIZE, VEH_SIZE>(m_pkk, 0, 0) =
138 Eigen::Block<typename KFMatrix::Base, VEH_SIZE, VEH_SIZE>(
147 for (
size_t i = 0; i < N_map; i++)
150 Eigen::Block<typename KFMatrix::Base, VEH_SIZE, FEAT_SIZE>(
151 m_pkk, 0, VEH_SIZE + i * FEAT_SIZE);
153 Eigen::Block<typename KFMatrix::Base, VEH_SIZE, FEAT_SIZE>(
154 m_pkk, 0, VEH_SIZE + i * FEAT_SIZE) = aux;
155 Eigen::Block<typename KFMatrix::Base, FEAT_SIZE, VEH_SIZE>(
156 m_pkk, VEH_SIZE + i * FEAT_SIZE, 0) = aux.transpose();
162 for (
size_t i = 0; i < VEH_SIZE; i++) m_xkk[i] = xv[i];
165 OnNormalizeStateVector();
169 const double tim_pred = m_timLogger.leave(
"KF:2.prediction stage");
174 m_timLogger.enter(
"KF:3.predict all obs");
177 OnGetObservationNoise(
R);
181 all_predictions.resize(N_map);
183 mrpt::math::sequenceStdVec<size_t, 1>(0, N_map), all_predictions);
185 const double tim_pred_obs = m_timLogger.leave(
"KF:3.predict all obs");
187 m_timLogger.enter(
"KF:4.decide pred obs");
190 predictLMidxs.clear();
194 predictLMidxs.assign(1, 0);
197 OnPreComputingPredictions(all_predictions, predictLMidxs);
199 m_timLogger.leave(
"KF:4.decide pred obs");
222 m_timLogger.enter(
"KF:5.build Jacobians");
227 : predictLMidxs.size();
237 std::vector<size_t> missing_predictions_to_add;
242 size_t first_new_pred = 0;
248 if (!missing_predictions_to_add.empty())
250 const size_t nNew = missing_predictions_to_add.size();
252 "[KF] *Performance Warning*: "
254 <<
" LMs were not correctly predicted by "
255 "OnPreComputingPredictions().");
258 for (
size_t j = 0; j < nNew; j++)
259 predictLMidxs.push_back(missing_predictions_to_add[j]);
261 N_pred = predictLMidxs.size();
262 missing_predictions_to_add.clear();
268 for (
size_t i = first_new_pred; i < N_pred; ++i)
270 const size_t lm_idx = FEAT_SIZE == 0 ? 0 : predictLMidxs[i];
275 m_user_didnt_implement_jacobian =
278 if (KF_options.use_analytic_observation_jacobian)
279 OnObservationJacobians(lm_idx, Hx, Hy);
281 if (m_user_didnt_implement_jacobian ||
282 !KF_options.use_analytic_observation_jacobian ||
283 KF_options.debug_verify_analytic_jacobians)
285 const size_t lm_idx_in_statevector =
286 VEH_SIZE + lm_idx * FEAT_SIZE;
289 const KFArray_FEAT x_feat(&m_xkk[lm_idx_in_statevector]);
293 OnObservationJacobiansNumericGetIncrements(
294 xkk_veh_increments, feat_increments);
300 const std::pair<KFCLASS*, size_t>& dat,
301 KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hx_jacobian),
303 std::pair<KFCLASS*, size_t>(
this, lm_idx), Hx);
306 ::memcpy(&m_xkk[0], &x_vehicle[0],
sizeof(m_xkk[0]) * VEH_SIZE);
312 const std::pair<KFCLASS*, size_t>& dat,
313 KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hy_jacobian),
314 feat_increments, std::pair<KFCLASS*, size_t>(
this, lm_idx),
319 &m_xkk[lm_idx_in_statevector], &x_feat[0],
320 sizeof(m_xkk[0]) * FEAT_SIZE);
322 if (KF_options.debug_verify_analytic_jacobians)
326 OnObservationJacobians(lm_idx, Hx_gt, Hy_gt);
327 if ((Hx - Hx_gt).array().abs().
sum() >
328 KF_options.debug_verify_analytic_jacobians_threshold)
330 std::cerr <<
"[KalmanFilter] ERROR: User analytical "
331 "observation Hx Jacobians are wrong: \n"
333 << Hx <<
"\n Analytical Hx:\n"
334 << Hx_gt <<
"Diff:\n"
335 << Hx - Hx_gt <<
"\n";
337 "ERROR: User analytical observation Hx Jacobians "
338 "are wrong (More details dumped to cerr)")
340 if ((Hy - Hy_gt).array().abs().
sum() >
341 KF_options.debug_verify_analytic_jacobians_threshold)
343 std::cerr <<
"[KalmanFilter] ERROR: User analytical "
344 "observation Hy Jacobians are wrong: \n"
346 << Hy <<
"\n Analytical Hx:\n"
347 << Hy_gt <<
"Diff:\n"
348 << Hy - Hy_gt <<
"\n";
350 "ERROR: User analytical observation Hy Jacobians "
351 "are wrong (More details dumped to cerr)")
356 m_timLogger.leave(
"KF:5.build Jacobians");
358 m_timLogger.enter(
"KF:6.build S");
365 S.setSize(N_pred * OBS_SIZE, N_pred * OBS_SIZE);
373 for (
size_t i = 0; i < N_pred; ++i)
375 const size_t lm_idx_i = predictLMidxs[i];
379 m_pkk, VEH_SIZE + lm_idx_i * FEAT_SIZE, 0);
382 for (
size_t j = i; j < N_pred; ++j)
384 const size_t lm_idx_j = predictLMidxs[j];
386 Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>
387 Sij(S, OBS_SIZE * i, OBS_SIZE * j);
391 Pxyj(m_pkk, 0, VEH_SIZE + lm_idx_j * FEAT_SIZE);
395 m_pkk, VEH_SIZE + lm_idx_i * FEAT_SIZE,
396 VEH_SIZE + lm_idx_j * FEAT_SIZE);
398 Sij = Hxs[i] * Px * Hxs[j].transpose() +
399 Hys[i] * Pxyi_t * Hxs[j].transpose() +
400 Hxs[i] * Pxyj * Hys[j].transpose() +
401 Hys[i] * Pyiyj * Hys[j].transpose();
407 S, OBS_SIZE * j, OBS_SIZE * i) = Sij.transpose();
411 const size_t obs_idx_off = i * OBS_SIZE;
412 Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>(
413 S, obs_idx_off, obs_idx_off) +=
R;
421 S = Hxs[0] * m_pkk * Hxs[0].transpose() +
R;
424 m_timLogger.leave(
"KF:6.build S");
428 m_timLogger.enter(
"KF:7.get obs & DA");
431 OnGetObservationsAndDataAssociation(
433 all_predictions, S, predictLMidxs,
R
436 data_association.size() == Z.size() ||
437 (data_association.empty() && FEAT_SIZE == 0));
444 missing_predictions_to_add.clear();
447 for (
size_t i = 0; i < data_association.size(); ++i)
449 if (data_association[i] < 0)
continue;
450 const size_t assoc_idx_in_map =
451 static_cast<size_t>(data_association[i]);
452 const size_t assoc_idx_in_pred =
454 assoc_idx_in_map, predictLMidxs);
455 if (assoc_idx_in_pred == std::string::npos)
456 missing_predictions_to_add.push_back(assoc_idx_in_map);
460 first_new_pred = N_pred;
463 }
while (!missing_predictions_to_add.empty());
465 const double tim_obs_DA = m_timLogger.leave(
"KF:7.get obs & DA");
473 m_timLogger.enter(
"KF:8.update stage");
475 switch (KF_options.method)
486 std::vector<int> mapIndicesForKFUpdate(data_association.size());
488 mapIndicesForKFUpdate.begin(),
490 data_association.begin(), data_association.end(),
491 mapIndicesForKFUpdate.begin(),
492 [](
int i) { return i == -1; })));
495 (FEAT_SIZE == 0) ? 1 :
498 mapIndicesForKFUpdate
502 const size_t nKF_iterations = (KF_options.method ==
kfEKFNaive)
504 : KF_options.IKF_iterations;
511 for (
size_t IKF_iteration = 0;
512 IKF_iteration < nKF_iterations; IKF_iteration++)
516 size_t ytilde_idx = 0;
520 dh_dx_full_obs.zeros(
522 VEH_SIZE + FEAT_SIZE * N_map);
529 std::vector<size_t> S_idxs;
530 S_idxs.reserve(OBS_SIZE * N_upd);
535 for (
size_t i = 0; i < data_association.size(); ++i)
537 if (data_association[i] < 0)
continue;
539 const size_t assoc_idx_in_map =
540 static_cast<size_t>(data_association[i]);
541 const size_t assoc_idx_in_pred =
543 assoc_idx_in_map, predictLMidxs);
545 assoc_idx_in_pred != string::npos,
546 "OnPreComputingPredictions() didn't "
547 "recommend the prediction of a landmark "
548 "which has been actually observed!");
564 dh_dx_full_obs, S_idxs.size(), 0) =
565 Hxs[assoc_idx_in_pred];
569 dh_dx_full_obs, S_idxs.size(),
570 VEH_SIZE + assoc_idx_in_map * FEAT_SIZE) =
571 Hys[assoc_idx_in_pred];
575 for (
size_t k = 0; k < OBS_SIZE; k++)
577 assoc_idx_in_pred * OBS_SIZE + k);
581 OnSubstractObservationVectors(
584 [predictLMidxs[assoc_idx_in_pred]]);
585 for (
size_t k = 0; k < OBS_SIZE; k++)
586 ytilde[ytilde_idx++] = ytilde_i[k];
590 S.extractSubmatrixSymmetrical(S_idxs, S_observed);
595 Z.size() == 1 && all_predictions.size() == 1);
597 dh_dx_full_obs.rows() == OBS_SIZE &&
598 dh_dx_full_obs.cols() == VEH_SIZE);
600 dh_dx_full_obs = Hxs[0];
602 OnSubstractObservationVectors(
603 ytilde_i, all_predictions[0]);
604 for (
size_t k = 0; k < OBS_SIZE; k++)
605 ytilde[ytilde_idx++] = ytilde_i[k];
613 m_timLogger.enter(
"KF:8.update stage:1.FULLKF:build K");
615 K.setSize(m_pkk.rows(), S_observed.cols());
618 K.multiply_ABt(m_pkk, dh_dx_full_obs);
624 m_timLogger.leave(
"KF:8.update stage:1.FULLKF:build K");
627 if (nKF_iterations == 1)
630 "KF:8.update stage:2.FULLKF:update xkk");
633 "KF:8.update stage:2.FULLKF:update xkk");
638 "KF:8.update stage:2.FULLKF:iter.update xkk");
641 dh_dx_full_obs.multiply_Ab(
642 m_xkk - xkk_0, HAx_column);
646 (ytilde - HAx_column), m_xkk,
651 "KF:8.update stage:2.FULLKF:iter.update xkk");
657 if (IKF_iteration == (nKF_iterations - 1))
660 "KF:8.update stage:3.FULLKF:update Pkk");
667 aux_K_dh_dx.multiply(K, dh_dx_full_obs);
670 const size_t stat_len = aux_K_dh_dx.cols();
671 for (
size_t r = 0;
r < stat_len;
r++)
673 for (
size_t c = 0;
c < stat_len;
c++)
676 aux_K_dh_dx.get_unsafe(
r,
c) =
677 -aux_K_dh_dx.get_unsafe(
r,
c) +
680 aux_K_dh_dx.get_unsafe(
r,
c) =
681 -aux_K_dh_dx.get_unsafe(
r,
c);
685 m_pkk.multiply_result_is_symmetric(
689 "KF:8.update stage:3.FULLKF:update Pkk");
702 for (
size_t obsIdx = 0; obsIdx < Z.size(); obsIdx++)
706 size_t idxInTheFilter = 0;
708 if (data_association.empty())
714 doit = data_association[obsIdx] >= 0;
715 if (doit) idxInTheFilter = data_association[obsIdx];
721 "KF:8.update stage:1.ScalarAtOnce.prepare");
724 const size_t idx_off =
733 std::vector<size_t>(1, idxInTheFilter), pred_obs);
738 OnSubstractObservationVectors(ytilde, pred_obs[0]);
745 const size_t i_idx_in_preds =
747 idxInTheFilter, predictLMidxs);
749 i_idx_in_preds != string::npos,
750 "OnPreComputingPredictions() didn't recommend the "
751 "prediction of a landmark which has been actually "
758 "KF:8.update stage:1.ScalarAtOnce.prepare");
761 for (
size_t j = 0; j < OBS_SIZE; j++)
764 "KF:8.update stage:2.ScalarAtOnce.update");
788 for (
size_t a = 0;
a < OBS_SIZE;
a++)
789 for (
size_t b = 0;
b < OBS_SIZE;
b++)
793 "This KF algorithm assumes "
796 "observation (matrix R). "
802 KFTYPE Sij =
R.get_unsafe(j, j);
805 for (
size_t k = 0; k < VEH_SIZE; k++)
808 for (
size_t q = 0;
q < VEH_SIZE;
q++)
809 accum += Hx.get_unsafe(j,
q) *
810 m_pkk.get_unsafe(
q, k);
811 Sij += Hx.get_unsafe(j, k) * accum;
816 for (
size_t k = 0; k < VEH_SIZE; k++)
819 for (
size_t q = 0;
q < FEAT_SIZE;
q++)
820 accum += Hy.get_unsafe(j,
q) *
821 m_pkk.get_unsafe(idx_off +
q, k);
822 term2 += Hx.get_unsafe(j, k) * accum;
827 for (
size_t k = 0; k < FEAT_SIZE; k++)
830 for (
size_t q = 0;
q < FEAT_SIZE;
q++)
831 accum += Hy.get_unsafe(j,
q) *
833 idx_off +
q, idx_off + k);
834 Sij += Hy.get_unsafe(j, k) * accum;
840 size_t N = m_pkk.cols();
841 vector<KFTYPE> Kij(N);
843 for (
size_t k = 0; k < N; k++)
849 for (
q = 0;
q < VEH_SIZE;
q++)
850 K_tmp += m_pkk.get_unsafe(k,
q) *
854 for (
q = 0;
q < FEAT_SIZE;
q++)
855 K_tmp += m_pkk.get_unsafe(k, idx_off +
q) *
858 Kij[k] = K_tmp / Sij;
863 for (
size_t k = 0; k < N; k++)
864 m_xkk[k] += Kij[k] * ytilde[j];
869 for (
size_t k = 0; k < N; k++)
871 for (
size_t q = k;
q < N;
874 m_pkk(k,
q) -= Sij * Kij[k] * Kij[
q];
876 m_pkk(
q, k) = m_pkk(k,
q);
879 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
882 m_pkk.saveToTextFile(
"Pkk_err.txt");
892 "KF:8.update stage:2.ScalarAtOnce.update");
909 size_t nKF_iterations = KF_options.IKF_iterations;
913 if (nKF_iterations>1)
923 for (
size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
931 xkk_next_iter = xkk_0;
935 for (
size_t obsIdx=0;obsIdx<Z.rows();obsIdx++)
939 size_t idxInTheFilter=0;
941 if (data_association.empty())
947 doit = data_association[obsIdx] >= 0;
949 idxInTheFilter = data_association[obsIdx];
955 const size_t idx_off = VEH_SIZE + idxInTheFilter *FEAT_SIZE;
956 const size_t R_row_offset = obsIdx*OBS_SIZE;
960 OnObservationModelAndJacobians(
1000 R.extractMatrix(R_row_offset,0, Si);
1016 KFMatrix Pyix( FEAT_SIZE, VEH_SIZE );
1017 m_pkk.extractMatrix(idx_off,0, Pyix);
1019 term.multiply_ABCt( Hy, Pyix, Hx );
1036 size_t N = m_pkk.cols();
1044 for (
size_t c=0;
c<OBS_SIZE;
c++)
1049 for (
q=0;
q<VEH_SIZE;
q++)
1050 K_tmp+= m_pkk.get_unsafe(k,
q) * Hx.get_unsafe(
c,
q);
1053 for (
q=0;
q<FEAT_SIZE;
q++)
1054 K_tmp+= m_pkk.get_unsafe(k,idx_off+
q) * Hy.get_unsafe(
c,
q);
1056 Ki.set_unsafe(k,
c, K_tmp);
1060 Ki.multiply(Ki, Si.inv() );
1064 if (nKF_iterations==1)
1069 for (
size_t q=0;
q<OBS_SIZE;
q++)
1070 m_xkk[k] += Ki.get_unsafe(k,
q) * ytilde[
q];
1075 Eigen::Matrix<KFTYPE,Eigen::Dynamic,1> HAx(OBS_SIZE);
1078 for (o=0;o<OBS_SIZE;o++)
1081 for (
q=0;
q<VEH_SIZE;
q++)
1082 tmp += Hx.get_unsafe(o,
q) * (xkk_0[
q] - m_xkk[
q]);
1084 for (
q=0;
q<FEAT_SIZE;
q++)
1085 tmp += Hy.get_unsafe(o,
q) * (xkk_0[idx_off+
q] - m_xkk[idx_off+
q]);
1091 for (o=0;o<OBS_SIZE;o++)
1092 HAx[o] = ytilde[o] - HAx[o];
1098 KFTYPE tmp = xkk_next_iter[k];
1100 for (o=0;o<OBS_SIZE;o++)
1101 tmp += Ki.get_unsafe(k,o) * HAx[o];
1103 xkk_next_iter[k] = tmp;
1112 Ki.multiplyByMatrixAndByTransposeNonSymmetric(
1118 m_pkk.force_symmetry();
1146 if (nKF_iterations>1)
1149 cout <<
"IKF iter: " << IKF_iteration <<
" -> " << xkk_next_iter << endl;
1151 m_xkk = xkk_next_iter;
1157 if (saved_Pkk)
delete saved_Pkk;
1168 const double tim_update = m_timLogger.leave(
"KF:8.update stage");
1170 m_timLogger.enter(
"KF:9.OnNormalizeStateVector");
1171 OnNormalizeStateVector();
1172 m_timLogger.leave(
"KF:9.OnNormalizeStateVector");
1177 if (!data_association.empty())
1179 m_timLogger.enter(
"KF:A.add new landmarks");
1181 m_timLogger.leave(
"KF:A.add new landmarks");
1185 m_timLogger.enter(
"KF:B.OnPostIteration");
1187 m_timLogger.leave(
"KF:B.OnPostIteration");
1189 m_timLogger.leave(
"KF:complete_step");
1192 "[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: "
1194 static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
1195 1e3 * tim_pred, 1e3 * tim_pred_obs, 1e3 * tim_obs_DA,
1201 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1205 const KFArray_VEH&
x,
const std::pair<KFCLASS*, KFArray_ACT>& dat,
1210 dat.first->OnTransitionModel(dat.second, out_x, dumm);
1214 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1218 const KFArray_VEH&
x,
const std::pair<KFCLASS*, size_t>& dat,
1221 std::vector<size_t> idxs_to_predict(1, dat.second);
1224 ::memcpy(&dat.first->m_xkk[0], &
x[0],
sizeof(
x[0]) * VEH_SIZE);
1225 dat.first->OnObservationModel(idxs_to_predict, prediction);
1227 out_x = prediction[0];
1231 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1235 const KFArray_FEAT&
x,
const std::pair<KFCLASS*, size_t>& dat,
1238 std::vector<size_t> idxs_to_predict(1, dat.second);
1240 const size_t lm_idx_in_statevector = VEH_SIZE + FEAT_SIZE * dat.second;
1243 &dat.first->m_xkk[lm_idx_in_statevector], &
x[0],
1244 sizeof(
x[0]) * FEAT_SIZE);
1245 dat.first->OnObservationModel(idxs_to_predict, prediction);
1247 out_x = prediction[0];
1254 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1259 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::vector_KFArray_OBS& Z,
1260 const std::vector<int>& data_association,
1262 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::KFMatrix_OxO&
R)
1267 for (
size_t idxObs = 0; idxObs < Z.size(); idxObs++)
1270 if (data_association[idxObs] < 0)
1278 0 == ((
obj.internal_getXkk().size() - VEH_SIZE) %
1281 const size_t newIndexInMap =
1282 (
obj.internal_getXkk().size() - VEH_SIZE) / FEAT_SIZE;
1285 typename KF::KFArray_FEAT yn;
1286 typename KF::KFMatrix_FxV dyn_dxv;
1287 typename KF::KFMatrix_FxO dyn_dhn;
1288 typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
1289 bool use_dyn_dhn_jacobian =
true;
1292 obj.OnInverseObservationModel(
1293 Z[idxObs], yn, dyn_dxv, dyn_dhn, dyn_dhn_R_dyn_dhnT,
1294 use_dyn_dhn_jacobian);
1298 obj.OnNewLandmarkAddedToMap(idxObs, newIndexInMap);
1304 size_t idx =
obj.internal_getXkk().size();
1305 obj.internal_getXkk().conservativeResize(
1306 obj.internal_getXkk().size() + FEAT_SIZE);
1308 for (
q = 0;
q < FEAT_SIZE;
q++)
1309 obj.internal_getXkk()[idx +
q] = yn[
q];
1315 obj.internal_getPkk().cols() == (
int)idx &&
1316 obj.internal_getPkk().rows() == (
int)idx);
1318 obj.internal_getPkk().setSize(idx + FEAT_SIZE, idx + FEAT_SIZE);
1322 typename KF::KFMatrix_VxV Pxx;
1323 obj.internal_getPkk().extractMatrix(0, 0, Pxx);
1324 typename KF::KFMatrix_FxV Pxyn;
1325 Pxyn.multiply(dyn_dxv, Pxx);
1327 obj.internal_getPkk().insertMatrix(idx, 0, Pxyn);
1328 obj.internal_getPkk().insertMatrixTranspose(0, idx, Pxyn);
1333 (idx - VEH_SIZE) / FEAT_SIZE;
1334 for (
q = 0;
q < nLMs;
q++)
1336 typename KF::KFMatrix_VxF P_x_yq(
1338 obj.internal_getPkk().extractMatrix(
1339 0, VEH_SIZE +
q * FEAT_SIZE, P_x_yq);
1341 typename KF::KFMatrix_FxF P_cross(
1343 P_cross.multiply(dyn_dxv, P_x_yq);
1345 obj.internal_getPkk().insertMatrix(
1346 idx, VEH_SIZE +
q * FEAT_SIZE, P_cross);
1347 obj.internal_getPkk().insertMatrixTranspose(
1348 VEH_SIZE +
q * FEAT_SIZE, idx, P_cross);
1356 dyn_dxv.multiply_HCHt(Pxx, P_yn_yn);
1357 if (use_dyn_dhn_jacobian)
1358 dyn_dhn.multiply_HCHt(
1361 P_yn_yn += dyn_dhn_R_dyn_dhnT;
1363 obj.internal_getPkk().insertMatrix(idx, idx, P_yn_yn);
1365 obj.getProfiler().leave(
"KF:9.create new LMs");
1370 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1373 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>&
obj,
1375 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE,
1376 KFTYPE>::vector_KFArray_OBS& Z,
1377 const std::vector<int>& data_association,
1379 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE,
1380 KFTYPE>::KFMatrix_OxO&
R)
1386 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1392 return (
obj.getStateVectorLength() - VEH_SIZE) / FEAT_SIZE;
1395 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1398 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>&
obj)
1404 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1410 return (
obj.getStateVectorLength() == VEH_SIZE);
1413 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1416 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>&
obj)