void ransac3Dplane_distance(
const CMatrixTemplateNumeric<T> &allData,
const vector< CMatrixTemplateNumeric<T> > & testModels,
const T distanceThreshold,
unsigned int & out_bestModelIndex,
vector_size_t & out_inlierIndices )
{
ASSERT_( testModels.size()==1 )
out_bestModelIndex = 0;
const CMatrixTemplateNumeric<T> &M = testModels[0];
ASSERT_( size(M,1)==1 && size(M,2)==4 )
TPlane plane;
plane.coefs[0] = M(0,0);
plane.coefs[1] = M(0,1);
plane.coefs[2] = M(0,2);
plane.coefs[3] = M(0,3);
const size_t N = size(allData,2);
out_inlierIndices.clear();
out_inlierIndices.reserve(100);
for (size_t i=0;i<N;i++)
{
const double d = plane.distance( TPoint3D( allData.get_unsafe(0,i),allData.get_unsafe(1,i),allData.get_unsafe(2,i) ) );
if (d<distanceThreshold)
out_inlierIndices.push_back(i);
}
}
void ransac2Dline_distance(
const CMatrixTemplateNumeric<T> &allData,
const vector< CMatrixTemplateNumeric<T> > & testModels,
const T distanceThreshold,
unsigned int & out_bestModelIndex,
vector_size_t & out_inlierIndices )
{
out_inlierIndices.clear();
out_bestModelIndex = 0;
if (testModels.empty()) return; // No model, no inliers.
ASSERTMSG_( testModels.size()==1, format("Expected testModels.size()=1, but it's = %u",static_cast<unsigned int>(testModels.size()) ) )
const CMatrixTemplateNumeric<T> &M = testModels[0];
ASSERT_( size(M,1)==1 && size(M,2)==3 )
TLine2D line;
line.coefs[0] = M(0,0);
line.coefs[1] = M(0,1);
line.coefs[2] = M(0,2);
const size_t N = size(allData,2);
out_inlierIndices.reserve(100);
for (size_t i=0;i<N;i++)
{
const double d = line.distance( TPoint2D( allData.get_unsafe(0,i),allData.get_unsafe(1,i) ) );
if (d<distanceThreshold)
out_inlierIndices.push_back(i);
}
}
double matrix_test_chol_dyn(int a1, int a2)
{
CMatrixTemplateNumeric<T> A = randomGenerator.drawDefinitePositiveMatrix(DIM1, 0.2);
CMatrixTemplateNumeric<T> chol_U;
const long N = 100;
CTicTac tictac;
for (long i=0;i<N;i++)
{
A.chol(chol_U);
}
return tictac.Tac()/N;
}
double random_test_9(int a1, int a2)
{
CRandomGenerator rg;
CMatrixTemplateNumeric<double> R(a1, a1);
rg.drawGaussian1DMatrix(R, 0.0, 1.0);
CMatrixTemplateNumeric<double> COV;
COV.multiply_AAt(R);
const size_t NSAMPS = 1000;
// test 9:
// ----------------------------------------
const long N = 1000;
CTicTac tictac;
std::vector<CVectorDouble> res;
for (long i = 0; i < N; i++)
{
rg.drawGaussianMultivariateMany(res, NSAMPS, COV);
}
return tictac.Tac() / (N * NSAMPS);
}
inline T operator()(const pair<vector<T>,CMatrixTemplateNumeric<T> > &p) {
const vector<T> &vec=p.first;
p.second.chol(R);
T res=T(0);
for (size_t i=0;i<N-1;++i) {
T accum=T(0.0);
for (size_t j=i;j<N;++j) accum+=R(i,j)*vec[j];
//tmpV[i]=accum;
res+=square(accum);
}
//tmpV[N-1]=vec[N-1]*R.get_unsafe(0,0);
res+=square(vec[N-1]*R.get_unsafe(N-1,N-1));
return res;
}
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;
}
inline CMatrixTemplateNumeric<T> operator()() {
do gen.drawUniformMatrix(tmp,T(-3.0),T(3.0)); while (tmp.isSingular(eps));
res.multiply_AAt(tmp);
return res;
}
