void SingleParticle2dx::DataStructures::Particle::calculateConsistency()
{
size_type n = 6;
std::vector<Eigen::VectorXf> points;
std::vector<Particle*> neighbors = getNeighbors();
for (size_type i=0; i<static_cast<size_type>(neighbors.size()); i++ )
{
Eigen::VectorXf new_vec(n);
new_vec[0] = neighbors[i]->getNewOrientation().getTLTAXIS();
new_vec[1] = neighbors[i]->getNewOrientation().getTLTANG();
new_vec[2] = neighbors[i]->getNewOrientation().getTAXA();
new_vec[3] = neighbors[i]->getParticleShift().getShiftX();
new_vec[4] = neighbors[i]->getParticleShift().getShiftY();
new_vec[5] = neighbors[i]->getSimMeasure();
points.push_back(new_vec);
}
Eigen::VectorXf mean(n);
for(size_type i=0; i<static_cast<size_type>(points.size()); i++)
{
mean += points[i];
}
mean /= static_cast<value_type>(points.size());
// std::cout << ": mean = " << mean[0] << " " << mean[1] << " " << mean[2] << " " << mean[3] << " " << mean[4] << " " << mean[5] << std::endl;
Eigen::MatrixXf covMat = Eigen::MatrixXf::Zero(n,n);
// std::cout << mean << std::endl;
for(size_type i=0; i<static_cast<size_type>(points.size()); i++)
{
Eigen::VectorXf diff = (points[i]-mean).conjugate();
covMat += diff * diff.adjoint();
}
// std::cout << ":det = " << covMat.determinant() << std::endl;
value_type det = covMat.determinant();
if ( !std::isfinite(det) )
{
setConsistency(0);
return;
}
if ( det < 0.001 )
{
setConsistency(0);
return;
}
//SingleParticle2dx::Utilities::UtilityFunctions::reqularizeMatrix(covMat);
// std::cout << covMat << std::endl;
Eigen::VectorXf vec(n);
vec[0] = getNewOrientation().getTLTAXIS();
vec[1] = getNewOrientation().getTLTANG();
vec[2] = getNewOrientation().getTAXA();
vec[3] = getParticleShift().getShiftX();
vec[4] = getParticleShift().getShiftY();
vec[5] = getSimMeasure();
value_type result = -0.5 * log(covMat.determinant());
// #pragma omp critical (det_output)
//{
//std::cout << ":det = " << det << std::endl;
//}
if ( covMat.determinant() < 0.000001 )
{
setConsistency(0);
return;
}
// std::cout << ":first term: " << result << std::endl;
Eigen::VectorXf tmp1 = (covMat.inverse()) * (vec-mean);
value_type tmp2 = (vec-mean).dot(tmp1);
result -= 0.5 * tmp2;
// std::cout << ":second term: " << -0.5 * tmp2 << std::endl;
if ( boost::math::isnan(result) || boost::math::isnan(-result) )
{
std::cout << "::reset CONS realy done now" << std::endl;
result = 0;
}
setConsistency(result);
}
