IGL_INLINE void igl::sparse(
const IndexVector & I,
const IndexVector & J,
const ValueVector & V,
const size_t m,
const size_t n,
Eigen::SparseMatrix<T>& X)
{
using namespace std;
using namespace Eigen;
assert((int)I.maxCoeff() < (int)m);
assert((int)I.minCoeff() >= 0);
assert((int)J.maxCoeff() < (int)n);
assert((int)J.minCoeff() >= 0);
assert(I.size() == J.size());
assert(J.size() == V.size());
// Really we just need .size() to be the same, but this is safer
assert(I.rows() == J.rows());
assert(J.rows() == V.rows());
assert(I.cols() == J.cols());
assert(J.cols() == V.cols());
vector<Triplet<T> > IJV;
IJV.reserve(I.size());
for(int x = 0;x<I.size();x++)
{
IJV.push_back(Triplet<T >(I(x),J(x),V(x)));
}
X.resize(m,n);
X.setFromTriplets(IJV.begin(),IJV.end());
}
IGL_INLINE void igl::sparse(
const IndexVector & I,
const IndexVector & J,
const ValueVector & V,
const size_t m,
const size_t n,
Eigen::SparseMatrix<T>& X)
{
using namespace std;
using namespace Eigen;
assert((int)I.maxCoeff() < (int)m);
assert((int)I.minCoeff() >= 0);
assert((int)J.maxCoeff() < (int)n);
assert((int)J.minCoeff() >= 0);
assert(I.size() == J.size());
assert(J.size() == V.size());
// Really we just need .size() to be the same, but this is safer
assert(I.rows() == J.rows());
assert(J.rows() == V.rows());
assert(I.cols() == J.cols());
assert(J.cols() == V.cols());
//// number of values
//int nv = V.size();
//Eigen::DynamicSparseMatrix<T, Eigen::RowMajor> dyn_X(m,n);
//// over estimate the number of entries
//dyn_X.reserve(I.size());
//for(int i = 0;i < nv;i++)
//{
// dyn_X.coeffRef((int)I(i),(int)J(i)) += (T)V(i);
//}
//X = Eigen::SparseMatrix<T>(dyn_X);
vector<Triplet<T> > IJV;
IJV.reserve(I.size());
for(int x = 0;x<I.size();x++)
{
IJV.push_back(Triplet<T >(I(x),J(x),V(x)));
}
X.resize(m,n);
X.setFromTriplets(IJV.begin(),IJV.end());
}
bool AlignmentAlgorithmSE2::operator()(AlignmentAlgorithmSE2::TransformType& transform, const CorrespondenceVector& correspondences, const IndexVector& indices){
if ((int)indices.size()<minimalSetSize())
return false;
SE2 x0;
SE2 ix0;
double deltaRSum=0.;
Vector2d mean1(0.,0.);
Vector2d mean2(0.,0.);
for (size_t i=0; i<indices.size(); i++){
const Correspondence& c = correspondences[indices[i]];
const EdgeSE2* edge = static_cast<const g2o::EdgeSE2*>(c.edge());
const VertexSE2* v1 = static_cast<const g2o::VertexSE2*>(edge->vertex(0));
const VertexSE2* v2 = static_cast<const g2o::VertexSE2*>(edge->vertex(1));
SE2 xi = v2->estimate()*v1->estimate().inverse();
mean1 += v1->estimate().translation();
mean2 += v2->estimate().translation();
if (i==0){
x0 = xi;
ix0 = x0.inverse();
} else {
SE2 delta=ix0*xi;
deltaRSum += delta.rotation().angle();
}
}
int count = indices.size();
double icount = 1./double(count);
deltaRSum*=icount;
mean1*=icount;
mean2*=icount;
Rotation2Dd R = x0.rotation()*Rotation2Dd(deltaRSum);
transform.setRotation(R);
transform.setTranslation(mean2 - R*mean1);
transform = transform.inverse();
return true;
}
void _computeReverseMap() {
_genesToTranscripts.resize( _geneNames.size(), {});
Index geneID;
Index transcriptID = 0;
size_t maxNumTrans = 0;
Index maxGene;
for ( size_t transcriptID = 0; transcriptID < _transcriptsToGenes.size(); ++transcriptID ) {
_genesToTranscripts[ _transcriptsToGenes[transcriptID] ].push_back( transcriptID );
if ( maxNumTrans < _genesToTranscripts[ _transcriptsToGenes[transcriptID] ].size() ) {
maxNumTrans = _genesToTranscripts[ _transcriptsToGenes[transcriptID] ].size();
maxGene = _transcriptsToGenes[transcriptID];
}
}
std::cerr << "max # of transcripts in a gene was " << maxNumTrans << " in gene " << _geneNames[maxGene] << "\n";
}
bool IdCorrespondenceValidator::operator()(const CorrespondenceVector& correspondences, const IndexVector& indices, int k){
if (k>minimalSetSize())
return true;
assert(indices.size()>=k && "VALIDATION_INDEX_OUT_OF_BOUND");
assert(correspondences.size()>=indices[k] && "VALIDATION_CORRESPONDENCE_INDEX_OUT_OF_BOUND");
const g2o::OptimizableGraph::Edge* edgek = correspondences[indices[k]].edge();
int idk1=edgek->vertex(0)->id();
int idk2=edgek->vertex(1)->id();
for (int i=0; i<k-1; i++){
const g2o::OptimizableGraph::Edge* edge = correspondences[indices[i]].edge();
int id1=edge->vertex(0)->id();
int id2=edge->vertex(1)->id();
if (idk1==id1)
return false;
if (idk2==id2)
return false;
}
return true;
}
// returns the number of race conditions detected (0 or 1 as of now)
int Specialization::verifyUpdateSequenceRaceConditions(LoopInfoSet& loopInfoSet, ArrayUpdatesSequence& arrayUpdates, VariableIdMapping* variableIdMapping) {
int cnt=0;
stringstream ss;
cout<<"STATUS: checking race conditions."<<endl;
cout<<"INFO: number of parallel loops: "<<numParLoops(loopInfoSet,variableIdMapping)<<endl;
VariableIdSet allIterVars;
for(LoopInfoSet::iterator lis=loopInfoSet.begin();lis!=loopInfoSet.end();++lis) {
allIterVars.insert((*lis).iterationVarId);
}
for(LoopInfoSet::iterator lis=loopInfoSet.begin();lis!=loopInfoSet.end();++lis) {
if((*lis).iterationVarType==ITERVAR_PAR) {
VariableId parVariable;
parVariable=(*lis).iterationVarId;
cout<<"INFO: checking parallel loop: "<<variableIdMapping->variableName(parVariable)<<endl;
// race check
// intersect w-set_i = empty
// w-set_i intersect r-set_j = empty, i!=j.
IndexToReadWriteDataMap indexToReadWriteDataMap;
for(ArrayUpdatesSequence::iterator i=arrayUpdates.begin();i!=arrayUpdates.end();++i) {
const EState* estate=(*i).first;
const PState* pstate=estate->pstate();
SgExpression* exp=(*i).second;
IndexVector index;
// use all vars for indexing or only outer+par loop variables
#ifdef USE_ALL_ITER_VARS
for(VariableIdSet::iterator ol=allIterVars.begin();ol!=allIterVars.end();++ol) {
VariableId otherVarId=*ol;
ROSE_ASSERT(otherVarId.isValid());
if(!pstate->varValue(otherVarId).isTop()) {
int otherIntVal=pstate->varValue(otherVarId).getIntValue();
index.push_back(otherIntVal);
}
}
#else
for(VariableIdSet::iterator ol=(*lis).outerLoopsVarIds.begin();ol!=(*lis).outerLoopsVarIds.end();++ol) {
VariableId otherVarId=*ol;
ROSE_ASSERT(otherVarId.isValid());
if(!pstate->varValue(otherVarId).isTop()&&pstate->varValue(otherVarId).isConstInt()) {
int otherIntVal=pstate->varValue(otherVarId).getIntValue();
index.push_back(otherIntVal);
}
}
if(!pstate->varValue(parVariable).isTop()&&pstate->varValue(parVariable).isConstInt()) {
int parIntVal=pstate->varValue(parVariable).getIntValue();
index.push_back(parIntVal);
}
#endif
if((*lis).isInAssociatedLoop(estate)) {
SgExpression* lhs=isSgExpression(SgNodeHelper::getLhs(exp));
SgExpression* rhs=isSgExpression(SgNodeHelper::getRhs(exp));
ROSE_ASSERT(isSgPntrArrRefExp(lhs)||SgNodeHelper::isFloatingPointAssignment(exp));
//cout<<"EXP: "<<exp->unparseToString()<<", lhs:"<<lhs->unparseToString()<<" :: "<<endl;
// read-set
RoseAst rhsast(rhs);
for(RoseAst::iterator j=rhsast.begin();j!=rhsast.end();++j) {
if(SgPntrArrRefExp* useRef=isSgPntrArrRefExp(*j)) {
j.skipChildrenOnForward();
ArrayElementAccessData access(useRef,variableIdMapping);
indexToReadWriteDataMap[index].readArrayAccessSet.insert(access);
} else if(SgVarRefExp* useRef=isSgVarRefExp(*j)) {
ROSE_ASSERT(useRef);
j.skipChildrenOnForward();
VariableId varId=variableIdMapping->variableId(useRef);
indexToReadWriteDataMap[index].readVarIdSet.insert(varId);
} else {
//cout<<"INFO: UpdateExtraction: ignored expression on rhs:"<<(*j)->unparseToString()<<endl;
}
}
if(SgPntrArrRefExp* arr=isSgPntrArrRefExp(lhs)) {
ArrayElementAccessData access(arr,variableIdMapping);
indexToReadWriteDataMap[index].writeArrayAccessSet.insert(access);
} else if(SgVarRefExp* var=isSgVarRefExp(lhs)) {
VariableId varId=variableIdMapping->variableId(var);
indexToReadWriteDataMap[index].writeVarIdSet.insert(varId);
} else {
cerr<<"Error: SSA Numbering: unknown LHS."<<endl;
exit(1);
}
ss<<"UPD"<<cnt<<":"<<pstate->toString(variableIdMapping)<<" : "<<exp->unparseToString()<<endl;
++cnt;
}
} // array sequence iter
// to be utilized later for more detailed output
#if 0
for(IndexToReadWriteDataMap::iterator imap=indexToReadWriteDataMap.begin();
imap!=indexToReadWriteDataMap.end();
++imap) {
// cout<<"DEBUG: INDEX: "<<(*imap).first<<" R-SET: ";
IndexVector index=(*imap).first;
cout<<"DEBUG: INDEX: ";
for(IndexVector::iterator iv=index.begin();iv!=index.end();++iv) {
if(iv!=index.begin())
cout<<",";
cout<<*iv;
}
cout<<" R-SET: ";
for(ArrayElementAccessDataSet::const_iterator i=indexToReadWriteDataMap[index].readArrayAccessSet.begin();i!=indexToReadWriteDataMap[index].readArrayAccessSet.end();++i) {
cout<<(*i).toString(variableIdMapping)<<" ";
}
cout<<endl;
cout<<"DEBUG: INDEX: ";
for(IndexVector::iterator iv=index.begin();iv!=index.end();++iv) {
if(iv!=index.begin())
cout<<",";
cout<<*iv;
}
cout<<" W-SET: ";
for(ArrayElementAccessDataSet::const_iterator i=indexToReadWriteDataMap[index].writeArrayAccessSet.begin();i!=indexToReadWriteDataMap[index].writeArrayAccessSet.end();++i) {
cout<<(*i).toString(variableIdMapping)<<" ";
}
cout<<endl;
cout<<"DEBUG: read-array-access:"<<indexToReadWriteDataMap[index].readArrayAccessSet.size()<<" read-var-access:"<<indexToReadWriteDataMap[index].readVarIdSet.size()<<endl;
cout<<"DEBUG: write-array-access:"<<indexToReadWriteDataMap[index].writeArrayAccessSet.size()<<" write-var-access:"<<indexToReadWriteDataMap[index].writeVarIdSet.size()<<endl;
} // imap
#endif
// perform the check now
// 1) compute vector if index-vectors for each outer-var-vector
// 2) check each index-vector. For each iteration of each par-loop iteration then.
//typedef set<int> ParVariableValueSet;
//ParVariableValueSet parVariableValueSet;
// MAP: par-variable-val -> vector of IndexVectors with this par-variable-val
typedef vector<IndexVector> ThreadVector;
typedef map<IndexVector,ThreadVector > CheckMapType;
CheckMapType checkMap;
for(IndexToReadWriteDataMap::iterator imap=indexToReadWriteDataMap.begin();
imap!=indexToReadWriteDataMap.end();
++imap) {
IndexVector index=(*imap).first;
IndexVector outVarIndex;
// if index.size()==0, it will analyze the loop independet of outer loops
if(index.size()>0) {
ROSE_ASSERT(index.size()>0);
for(size_t iv1=0;iv1<index.size()-1;iv1++) {
outVarIndex.push_back(index[iv1]);
}
ROSE_ASSERT(outVarIndex.size()<index.size());
} else {
// nothing to check
continue;
}
// last index of index of par-variable
//int parVariableValue=index[index.size()-1];
checkMap[outVarIndex].push_back(index);
}
//cout<<"INFO: race condition check-map size: "<<checkMap.size()<<endl;
// perform the check now
for(CheckMapType::iterator miter=checkMap.begin();miter!=checkMap.end();++miter) {
IndexVector outerVarIndexVector=(*miter).first;
ThreadVector threadVectorToCheck=(*miter).second;
//cout<<"DEBUG: to check: "<<threadVectorToCheck.size()<<endl;
for(ThreadVector::iterator tv1=threadVectorToCheck.begin();tv1!=threadVectorToCheck.end();++tv1) {
ArrayElementAccessDataSet wset=indexToReadWriteDataMap[*tv1].writeArrayAccessSet;
for(ThreadVector::iterator tv2=tv1;tv2!=threadVectorToCheck.end();++tv2) {
ThreadVector::iterator tv2b=tv2;
++tv2b;
if(tv2b!=threadVectorToCheck.end()) {
ArrayElementAccessDataSet rset2=indexToReadWriteDataMap[*tv2b].readArrayAccessSet;
ArrayElementAccessDataSet wset2=indexToReadWriteDataMap[*tv2b].writeArrayAccessSet;
// check intersect(rset,wset)
if(accessSetIntersect(wset,rset2)) {
// verification failed
cout<<"INFO: race condition detected (wset1,rset2)."<<endl;
return 1;
}
if(accessSetIntersect(wset,wset2)) {
// verification failed
cout<<"INFO: race condition detected (wset1,wset2)."<<endl;
return 1;
}
}
}
}
}
} // if parallel loop
} // foreach loop
return 0;
}
bool AlignmentAlgorithmPlaneLinear::operator()(AlignmentAlgorithmPlaneLinear::TransformType& transform, const CorrespondenceVector& correspondences, const IndexVector& indices)
{
double err=0;
//If my correspondaces indices are less then a minimum threshold, stop it please
if ((int)indices.size()<minimalSetSize()) return false;
//My initial guess for the transformation it's the identity matrix
//maybe in the future i could have a less rough guess
transform = Isometry3d::Identity();
//Unroll the matrix to a vector
Vector12d x=homogeneous2vector(transform.matrix());
Matrix9x1d rx=x.block<9,1>(0,0);
//Initialization of variables, melting fat, i've so much space
Matrix9d H;
H.setZero();
Vector9d b;
b.setZero();
Matrix3x9d A;
//iteration for each correspondace
for (size_t i=0; i<indices.size(); i++)
{
A.setZero();
const Correspondence& c = correspondences[indices[i]];
const EdgePlane* edge = static_cast<const EdgePlane*>(c.edge());
//estraggo i vertici dagli edge
const VertexPlane* v1 = static_cast<const VertexPlane*>(edge->vertex(0));
const VertexPlane* v2 = static_cast<const VertexPlane*>(edge->vertex(1));
//recupero i dati dei piani
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pi= v1->estimate();
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pj= v2->estimate();
//recupeo le normali, mi servono per condizionare la parte rotazionale
Vector3d ni;
Vector3d nj;
Vector4d coeffs_i=pi.toVector();
Vector4d coeffs_j=pj.toVector();
ni=coeffs_i.head(3);
nj=coeffs_j.head(3);
//inizializzo lo jacobiano, solo la parte rotazionale (per ora)
A.block<1,3>(0,0)=nj.transpose();
A.block<1,3>(1,3)=nj.transpose();
A.block<1,3>(2,6)=nj.transpose();
if(DEBUG) {
cerr << "[DEBUG] PI"<<endl;
cerr << coeffs_i<<endl;
cerr << "[DEBUG] PJ "<<endl;
cerr << coeffs_j<<endl;
cerr << "[ROTATION JACOBIAN][PLANE "<<i<<"]"<<endl;
cerr << A<<endl;
}
//errore
//inizializzo errore
Vector3d ek;
ek.setZero();
ek=A*x.block<9,1>(0,0)-coeffs_i.head(3);
H+=A.transpose()*A;
b+=A.transpose()*ek;
err+=abs(ek.dot(ek));
if(DEBUG)
cerr << "[ROTATIONAL Hessian for plane "<<i<<"]"<<endl<<H<<endl;
if(DEBUG)
cerr << "[ROTATIONAL B for plane "<<i<<"]"<<endl<<b<<endl;
}
LDLT<Matrix9d> ldlt(H);
if (ldlt.isNegative()) return false;
rx=ldlt.solve(-b); // using a LDLT factorizationldlt;
x.block<3,1>(0,0)+=rx.block<3,1>(0,0);
x.block<3,1>(3,0)+=rx.block<3,1>(3,0);
x.block<3,1>(6,0)+=rx.block<3,1>(6,0);
if(DEBUG) {
cerr << "Solving the linear system"<<endl;
cerr << "------------>H"<<endl;
cerr << H<<endl;
cerr << "------------>b"<<endl;
cerr << b<<endl;
cerr << "------------>rx"<<endl;
cerr << rx<<endl;
cerr << "------------>xTEMP"<<endl;
cerr << vector2homogeneous(x)<<endl<<endl;
cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
cerr << "łłłłłłłłłłł Ringraziamo Cthulhu la parte rotazionale è finitałłłłłłłłłłł"<<endl;
cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
}
Matrix4d Xtemp=vector2homogeneous(x);
//now the problem si solved but i could have (and probably i have!)
//a not orthogonal rotation matrix, so i've to recondition it
Matrix3x3d R=Xtemp.block<3,3>(0,0);
// recondition the rotation
JacobiSVD<Matrix3x3d> svd(R, Eigen::ComputeThinU | Eigen::ComputeThinV);
if (svd.singularValues()(0)<.5) return false;
R=svd.matrixU()*svd.matrixV().transpose();
Isometry3d X = Isometry3d::Identity();
X.linear()=R;
X.translation()= x.block<3,1>(0,3);
if(DEBUG)
cerr << "X dopo il ricondizionamento appare come:"<<endl;
if(DEBUG)
cerr << X.matrix()<<endl;
Matrix3d H2=Matrix3d::Zero();
Vector3d b2=Vector3d::Zero();
Vector3d A2=Vector3d::Zero();
//at this point rotation is cured, now it's time to work on the translation
for (size_t i=0; i<indices.size(); i++)
{
if(DEBUG)
cerr << "[TRANSLATION JACOBIAN START][PLANE "<<i<<"]"<<endl;
const Correspondence& c = correspondences[indices[i]];
const EdgePlane* edge = static_cast<const EdgePlane*>(c.edge());
//estraggo i vertici dagli edge
const VertexPlane* v1 = static_cast<const VertexPlane*>(edge->vertex(0));
const VertexPlane* v2 = static_cast<const VertexPlane*>(edge->vertex(1));
//recupero i dati dei piani
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pi= v1->estimate();
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pj= v2->estimate();
//recupeo le normali, mi servono per condizionare la parte rotazionale
Vector3d ni;
Vector3d nj;
Vector4d coeffs_i=pi.toVector();
Vector4d coeffs_j=pj.toVector();
double di;
double dj;
ni=coeffs_i.head(3);
nj=coeffs_j.head(3);
di=coeffs_i(3);
dj=coeffs_j(3);
if(DEBUG)
cerr << "[TRANSLATION JACOBIAN][ JACOBIAN IS: ]"<<endl;
A2=(-nj.transpose()*R.transpose());
if(DEBUG)
cerr << A2<<endl;
double ek;
ek=dj+A2.transpose()*X.translation()-di;
H2+=A2*A2.transpose();
b2+= (A2.transpose()*ek);
err += abs(ek*ek);
if(DEBUG)
cerr << "[TRANSLATIONAL Hessian for plane "<<i<<"]"<<endl<<H2<<endl;
if(DEBUG)
cerr << "[TRANSLATIONAL B for plane "<<i<<"]"<<endl<<b2<<endl;
}
if(DEBUG)
cerr << "[FINAL][TRANSLATIONAL Hessian]"<<endl<<H2<<endl;
if(DEBUG)
cerr << "[FINAL][TRANSLATIONAL B]"<<endl<<b2<<endl;
//solving the system
Vector3d dt;
LDLT<Matrix3d> ldlt2(H2);
dt=ldlt2.solve(-b2); // using a LDLT factorizationldlt;
if(DEBUG)
cerr << "[FINAL][TRANSLATIONAL DT]"<<endl<<dt<<endl;
X.translation()+=dt;
transform = X;
if(DEBUG)
{
cerr << "TRANSFORM found: " << endl<< endl<< endl;
cerr << g2o::internal::toVectorMQT(X) << endl;;
cerr << transform.matrix()<< endl;;
}
return true;
}