void Check() { ASSERT_(lvl == w.GetCloffLevel(), "Draw::Begin/End mismatch for " + UPP::Name(q)); }
static void sCheckGuiLock() { ASSERT_(ThreadHasGuiLock(), "Using GUI in non-main thread without GuiLock"); }
double joint_pdf_metric ( const CMatrixTemplateNumeric<T> &Z_observations_mean, const CMatrixTemplateNumeric<T> &Y_predictions_mean, const CMatrixTemplateNumeric<T> &Y_predictions_cov, const TAuxDataRecursiveJCBB &info, const TDataAssociationResults &aux_data) { MRPT_UNUSED_PARAM(aux_data); // Make a list of the indices of the predictions that appear in "currentAssociation": const size_t N = info.currentAssociation.size(); ASSERT_(N>0) vector_size_t indices_pred(N); // Appearance order indices in the std::maps vector_size_t indices_obs(N); { size_t i=0; for (map<size_t,size_t>::const_iterator it=info.currentAssociation.begin();it!=info.currentAssociation.end();++it) { indices_obs[i] = it->first; indices_pred[i] = it->second; i++; } } // ---------------------------------------------------------------------- // Extract submatrix of the covariances involved here: // COV = PREDICTIONS_COV(INDX,INDX) + OBSERVATIONS_COV(INDX2,INDX2) // ---------------------------------------------------------------------- Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> COV; Y_predictions_cov.extractSubmatrixSymmetricalBlocks( info.length_O, // dims of cov. submatrices indices_pred, COV ); // ---------------------------------------------------------------------- // Mean: // The same for the vector of "errors" or "innovation" between predictions and observations: // ---------------------------------------------------------------------- Eigen::Matrix<T,Eigen::Dynamic,1> innovations(N * info.length_O); T *dst_ptr= &innovations[0]; for (map<size_t,size_t>::const_iterator it=info.currentAssociation.begin();it!=info.currentAssociation.end();++it) { const T *pred_i_mean = Y_predictions_mean.get_unsafe_row( it->second ); const T *obs_i_mean = Z_observations_mean.get_unsafe_row( it->first ); for (unsigned int k=0;k<info.length_O;k++) *dst_ptr++ = pred_i_mean[k]-obs_i_mean[k]; } // Compute mahalanobis distance squared: CMatrixTemplateNumeric<T> COV_inv; COV.inv_fast(COV_inv); const double d2 = mrpt::math::multiply_HCHt_scalar(innovations, COV_inv); if (METRIC==metricMaha) return d2; ASSERT_(METRIC==metricML); // Matching likelihood: The evaluation at 0 of the PDF of the difference between the two Gaussians: const T cov_det = COV.det(); const double ml = exp(-0.5*d2) / ( std::pow(M_2PI, info.length_O * 0.5) * std::sqrt(cov_det) ); return ml; }
bool Ctrl::ProcessEvents(bool *quit) { ASSERT_(IsMainThread(), "ProcessEvents can only run in the main thread"); return ProcessEvents0(quit, true); }
bool Ctrl::IsWaitingEvent() { ASSERT_(IsMainThread(), "IsWaitingEvent can only run in the main thread"); return IsWaitingEvent0(true); }
String GetIniKey(const char *id, const String& def) { ASSERT_(IsMainRunning(), "GetIniKey is allowed only after APP_MAIN has started"); Mutex::Lock __(sMtx); return sIniKeys().Get(id, def); }