// Remove a single constraint
ConstraintSystem& ConstraintSystem::removeConstraint(
const Constraint& constraint )
{
ConstraintList backupList;
for(ConstraintList::iterator it= constraintList.begin();
it!=constraintList.end();
++it)
{
bool isEqual(true);
if(it->header.prefit!=constraint.header.prefit) isEqual=false;
if(it->header.variance!=constraint.header.variance) isEqual=false;
if(it->body != constraint.body) isEqual = false;
if( !isEqual ) backupList.push_back(*it);
}
clearConstraint();
for(ConstraintList::iterator it= backupList.begin();
it!=backupList.end();
++it)
{
addConstraint(*it);
}
return (*this);
} // End of method 'ConstraintSystem::removeConstraint()'
set<Constraint *> subConstraint(){
set <Constraint *> subcstr;
set<int> scope;
for(int k=0; k < arity(); k++) {
scope.insert(getVar(k)->wcspIndex);
}
for(set<int>::iterator itx = scope.begin(); itx != scope.end(); ++itx){
ConstraintList* xctrs = (wcsp->getVar(*itx))->getConstrs();
for (ConstraintList::iterator it=xctrs->begin(); it != xctrs->end(); ++it){
Constraint * ctr = (*it).constr;
if(ctr->arity() < arity() && scopeIncluded(ctr)) subcstr.insert(ctr);
}
}
return subcstr;
}
// Impose the constraints system to the equation system
// the prefit residuals vector, hMatrix and rMatrix will be appended.
void EquationSystem::imposeConstraints()
{
if(!equationConstraints.hasConstraints()) return;
ConstraintList destList;
ConstraintList tempList = equationConstraints.getConstraintList();
for(ConstraintList::iterator it = tempList.begin();
it != tempList.end();
++it )
{
VariableDataMap dataMapOk;
bool validConstraint(true);
try
{
VariableDataMap dataMap = it->body;
for(VariableDataMap::iterator itv = dataMap.begin();
itv != dataMap.end();
++itv )
{
bool isFound(false);
VariableSet::iterator itv2 = varUnknowns.find(itv->first);
if(itv2!=varUnknowns.end())
{
isFound = true;
dataMapOk[*itv2] = dataMap[itv->first];
}
else
{
for(itv2 = varUnknowns.begin();
itv2 != varUnknowns.end();
++itv2 )
{
if( (itv->first.getType() == itv2->getType()) &&
(itv->first.getSource() == itv2->getSource()) &&
(itv->first.getSatellite() == itv2->getSatellite()) )
{
isFound = true;
dataMapOk[*itv2] = dataMap[itv->first];
break;
}
}
}
if( !isFound ) validConstraint = false;
}
}
catch(...)
{
validConstraint = false;
}
if(validConstraint)
{
destList.push_back(Constraint(it->header,dataMapOk));
}
else
{
// we discard all constraints
return;
}
}
// Update the equation system
equationConstraints.setConstraintList(destList);
// Now, we can append the matrix(prefit design and weight)
try
{
Vector<double> meas;
Matrix<double> design;
Matrix<double> cov;
equationConstraints.constraintMatrix(varUnknowns,
meas, design, cov);
const int oldSize = measVector.size();
const int newSize = oldSize + meas.size();
const int colSize = hMatrix.cols();
Vector<double> tempPrefit(newSize,0.0);
Matrix<double> tempGeometry(newSize,colSize,0.0);
Matrix<double> tempWeight(newSize,newSize,0.0);
for(int i=0; i< newSize; i++)
{
// prefit
if(i<oldSize) tempPrefit(i) = measVector(i);
else tempPrefit(i) = meas(i-oldSize);
// geometry
for(int j=0;j<colSize;j++)
{
if(i<oldSize) tempGeometry(i,j) = hMatrix(i,j);
else tempGeometry(i,j) = design(i-oldSize,j);
}
// weight
if(i<oldSize) tempWeight(i,i) = rMatrix(i,i);
else tempWeight(i,i) = 1.0/cov(i-oldSize,i-oldSize);
}
// Update these matrix
measVector = tempPrefit;
hMatrix = tempGeometry;
rMatrix = tempWeight;
}
catch(...)
{
return;
}
} // End of method 'EquationSystem::imposeConstraints()'
