template<typename Scalar> void test_sparseqr_scalar()
{
typedef SparseMatrix<Scalar,ColMajor> MatrixType;
typedef Matrix<Scalar,Dynamic,Dynamic> DenseMat;
typedef Matrix<Scalar,Dynamic,1> DenseVector;
MatrixType A;
DenseMat dA;
DenseVector refX,x,b;
SparseQR<MatrixType, COLAMDOrdering<int> > solver;
generate_sparse_rectangular_problem(A,dA);
b = dA * DenseVector::Random(A.cols());
solver.compute(A);
if(internal::random<float>(0,1)>0.5f)
solver.factorize(A); // this checks that calling analyzePattern is not needed if the pattern do not change.
if (solver.info() != Success)
{
std::cerr << "sparse QR factorization failed\n";
exit(0);
return;
}
x = solver.solve(b);
if (solver.info() != Success)
{
std::cerr << "sparse QR factorization failed\n";
exit(0);
return;
}
VERIFY_IS_APPROX(A * x, b);
//Compare with a dense QR solver
ColPivHouseholderQR<DenseMat> dqr(dA);
refX = dqr.solve(b);
VERIFY_IS_EQUAL(dqr.rank(), solver.rank());
if(solver.rank()==A.cols()) // full rank
VERIFY_IS_APPROX(x, refX);
// else
// VERIFY((dA * refX - b).norm() * 2 > (A * x - b).norm() );
// Compute explicitly the matrix Q
MatrixType Q, QtQ, idM;
Q = solver.matrixQ();
//Check ||Q' * Q - I ||
QtQ = Q * Q.adjoint();
idM.resize(Q.rows(), Q.rows()); idM.setIdentity();
VERIFY(idM.isApprox(QtQ));
// Q to dense
DenseMat dQ;
dQ = solver.matrixQ();
VERIFY_IS_APPROX(Q, dQ);
}
bool solve(const int mode)
{
bool batch = (mode !=2 || first_ordered_node_ == 0);
bool order = (mode !=0 && n_nodes_ > 1);
// BATCH
if (batch)
{
// REORDER
if (order)
ordering(0);
//print_problem();
// SOLVE
t_solving_ = clock();
A_.makeCompressed();
solver_.compute(A_);
if (solver_.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
x_incr_ = solver_.solve(b_);
R_ = solver_.matrixR();
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
}
// INCREMENTAL
else
{
// REORDER SUBPROBLEM
ordering(first_ordered_node_);
//print_problem();
// SOLVE ORDERED SUBPROBLEM
t_solving_= clock();
A_nodes_.makeCompressed();
A_.makeCompressed();
// finding measurements block
SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_);
// std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
// std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
int first_ordered_measurement = measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location;
int unordered_variables = nodes_.at(first_ordered_node_).location;
SparseMatrix<double, ColMajor> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
solver_.compute(A_partial);
if (solver_.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
//std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements));
// store new part of R
eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
R_.makeCompressed();
// solving not ordered subproblem
if (unordered_variables > 0)
{
//std::cout << "--------------------- solving unordered part" << std::endl;
SparseMatrix<double, ColMajor> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
//std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
SparseMatrix<double, ColMajor> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
//std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
solver_.compute(R1);
if (solver_.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
x_incr_.head(unordered_variables) = solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables));
}
}
// UNDO ORDERING FOR RESULT
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
nodePermutation2VariablesPermutation(acc_node_permutation_, acc_permutation); // TODO via pointers
x_incr_ = acc_permutation.inverse() * x_incr_;
time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
return 1;
}
