QString MOOptVector::toCSV(QString separator, QList<int> points)
{
QString csv;
int iVar;
double value;
QStringList scanTexts;
// if points is empty, print all of them
if(points.isEmpty())
{
for(int i=0;i<nbPoints();i++)
points.push_back(i);
}
if(this->size()>0)
{
// writing names
for(iVar=0;iVar<this->size();iVar++)
{
csv += this->at(iVar)->model()+"#"+this->at(iVar)->name(Variable::SHORT);
csv += separator;
}
csv += "\n";
// writing values
for(int iScan = 0;iScan<this->nbScans();iScan++)
{
scanTexts.push_back(QString());
for(int iPoint = 0; iPoint < points.size(); iPoint++)
{
for(iVar=0;iVar<this->size();iVar++)
{
if (this->at(iVar)->isComputedPoint(iScan,points.at(iPoint)))
{
value = this->at(iVar)->finalValue(iScan,points.at(iPoint));
scanTexts[iScan] += QString::number(value);
}
else
{
scanTexts[iScan] += "-";
}
scanTexts[iScan] += separator;
}
scanTexts[iScan] += "\n";
}
}
// writing grouped by scans
for(int iScan = 0;iScan<this->nbScans();iScan++)
{
csv+=scanTexts[iScan];
csv+="\n";
}
}
return csv;
}
std::vector<Vector3D> Polygone::getPoints3D() const
{
std::vector<Vector3D> res;
int taille = nbPoints();
res.reserve(taille);
for(int i = 0; i < taille; i++)
res.push_back(Vector3D(XY(get(i)),0));
return res;
}
double arlCore::ICP::computeCriterion( const arlCore::vnl_rigid_matrix &M, vnl_vector< double > &fx )
{
vnl_vector<double> RMS(1, 0.0), nbPoints(1, 0.0);
const arlCore::vnl_rigid_matrix InvM = M.computeInverse();
unsigned int i, j, noTriangle;
double n = 0.0, result = 0.0;
if(m_point2PointMode)
{ // Points to points
#ifdef ANN
const unsigned int Dimension = 3;
vnl_vector_fixed<double,3> traInit = InvM.getTranslation();
vnl_matrix_fixed<double,3,3> rotInit = InvM.getRotation();
const double Epsilon = 0.0;// Error bound
ANNpoint Pt = annAllocPt(Dimension); // Query point
for( i=0 ; i<m_cloudSize ; ++i )
{ // Search the matching point for every point of cloud
for( j=0 ; j<3; ++j )
Pt[j] = rotInit[j][0]*m_cloudPoints[i][0]+rotInit[j][1]*m_cloudPoints[i][1]+rotInit[j][2]*m_cloudPoints[i][2]+traInit[j];
m_ANNtree->annkSearch( Pt, m_nbNN, m_nn_idx, m_squaredDists, Epsilon );
if(fx.size()>i) fx[i] = m_squaredDists[0];
RMS[0] += m_squaredDists[0];
}
annDeallocPt(Pt);
n = (double)m_cloudSize;
#endif // ANN
}else
{ // Point to mesh
assert(m_cloud!=0 && m_modelMesh!=0);
RMS.set_size((unsigned int)m_modelMesh->getTriangles().size());
RMS.fill(0.0);
nbPoints.set_size(RMS.size());
nbPoints.fill(0.0);
unsigned int i;
arlCore::Point::sptr point = arlCore::Point::New(3);
for( i=0 ; i<m_cloud->size() ; ++i )
if(m_cloud->get(i))
if(!m_justVisible || m_cloud->get(i)->isVisible())
{
InvM.trf(m_cloud->get(i), point);
const double SquaredDist = m_modelMesh->computeDistance2(point, noTriangle);
if(SquaredDist>0.0)
{
RMS[noTriangle] += SquaredDist;
if(fx.size()>i) fx[i] = SquaredDist;
nbPoints[noTriangle] += 1;
}
}
}
assert(nbPoints.size()==RMS.size());
if(RMS.size()==1)
{
if(nbPoints[0]>0) return sqrt(RMS[i]/nbPoints[i]);
else return -1.0;
}
for( i=0 ; i<RMS.size() ; ++i )
if(nbPoints[i]>0)
{
result += sqrt(RMS[i]/nbPoints[i]);
n += 1.0;
}
if(n>0) return result/n;
else return -1;
}
