GCS: Refactor Non-Zero elimination over pivot

This commit is contained in:
Abdullah Tahiri
2020-12-13 16:24:29 +01:00
committed by abdullahtahiriyo
parent 28f6978a0d
commit 909adbd93d
2 changed files with 17 additions and 11 deletions

View File

@@ -4193,6 +4193,21 @@ void System::identifyDependentGeometryParametersInTransposedJacobianDenseQRDecom
}
void System::eliminateNonZerosOverPivotInUpperTriangularMatrix( Eigen::MatrixXd &R, int rank)
{
for (int i=1; i < rank; i++) {
// eliminate non zeros above pivot
assert(R(i,i) != 0);
for (int row=0; row < i; row++) {
if (R(row,i) != 0) {
double coef=R(row,i)/R(i,i);
R.block(row,i+1,1,R.cols()-i-1) -= coef * R.block(i,i+1,1,R.cols()-i-1);
R(row,i) = 0;
}
}
}
}
template <typename T>
void System::identifyConflictingRedundantConstraints( Algorithm alg,
const T & qrJT,
@@ -4204,17 +4219,7 @@ void System::identifyConflictingRedundantConstraints( Algorithm alg,
int &nonredundantconstrNum
)
{
for (int i=1; i < rank; i++) {
// eliminate non zeros above pivot
assert(R(i,i) != 0);
for (int row=0; row < i; row++) {
if (R(row,i) != 0) {
double coef=R(row,i)/R(i,i);
R.block(row,i+1,1,constrNum-i-1) -= coef * R.block(i,i+1,1,constrNum-i-1);
R(row,i) = 0;
}
}
}
eliminateNonZerosOverPivotInUpperTriangularMatrix(R, rank);
std::vector< std::vector<Constraint *> > conflictGroups(constrNum-rank);
for (int j=rank; j < constrNum; j++) {

View File

@@ -161,6 +161,7 @@ namespace GCS
int &nonredundantconstrNum
);
void eliminateNonZerosOverPivotInUpperTriangularMatrix(Eigen::MatrixXd &R, int rank);
#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_
void extractSubsystem(SubSystem *subsys, bool isRedundantsolving);