diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f1eaf6f9dec6eb0b2d97dac26b994632907af21e..25d8c530677fc6b3052b77e5c5a0cedad44a0925 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -159,7 +159,7 @@ IF (laser_scan_utils_FOUND) ENDIF(laser_scan_utils_FOUND) IF (Suitesparse_FOUND) - ADD_SUBDIRECTORY(wolf_solver) + ADD_SUBDIRECTORY(solver) ENDIF(Suitesparse_FOUND) # create the shared library diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 3120701a6d9fa187b5bb151bb695fda79ecb605c..adb268674689fcc0dcab761da45bb81c588af65a 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -30,6 +30,11 @@ TARGET_LINK_LIBRARIES(test_ceres_odom_batch ${PROJECT_NAME}) ADD_EXECUTABLE(test_ceres_odom_iterative test_ceres_odom_iterative.cpp) TARGET_LINK_LIBRARIES(test_ceres_odom_iterative ${PROJECT_NAME}) +# Testing Eigen Permutations +ADD_EXECUTABLE(test_permutations test_permutations.cpp) +TARGET_LINK_LIBRARIES(test_permutations ${PROJECT_NAME}) + + IF(Suitesparse_FOUND) # Testing a ccolamd test ADD_EXECUTABLE(test_ccolamd test_ccolamd.cpp) @@ -38,6 +43,18 @@ IF(Suitesparse_FOUND) # Testing a blocks ccolamd test ADD_EXECUTABLE(test_ccolamd_blocks test_ccolamd_blocks.cpp) TARGET_LINK_LIBRARIES(test_ccolamd_blocks ${PROJECT_NAME}) + + # Testing an incremental blocks ccolamd test + ADD_EXECUTABLE(test_incremental_ccolamd_blocks test_incremental_ccolamd_blocks.cpp) + TARGET_LINK_LIBRARIES(test_incremental_ccolamd_blocks ${PROJECT_NAME}) + + # Testing an incremental QR with block ccolamd test + ADD_EXECUTABLE(test_iQR test_iQR.cpp) + TARGET_LINK_LIBRARIES(test_iQR ${PROJECT_NAME}) + + # Testing SPQR simple test + ADD_EXECUTABLE(test_SPQR test_SPQR.cpp) + TARGET_LINK_LIBRARIES(test_SPQR ${PROJECT_NAME}) ENDIF(Suitesparse_FOUND) # Building and populating the wolf tree diff --git a/src/examples/test_SPQR.cpp b/src/examples/test_SPQR.cpp new file mode 100644 index 0000000000000000000000000000000000000000..49f7215a19697f3c663f121c5f259af3005f6291 --- /dev/null +++ b/src/examples/test_SPQR.cpp @@ -0,0 +1,92 @@ +/* + * test_SPQR.cpp + * + * Created on: Jun 18, 2015 + * Author: jvallve + */ + +#include <iostream> +#include <Eigen/SPQRSupport> +#include "SuiteSparseQR.hpp" + +using namespace Eigen; + +int main (int argc, char **argv) +{ + /////////////////////////////////////////////////////////////////////// + // Eigen Support SPQR + SPQR < SparseMatrix<double> > solver; + //solver.setSPQROrdering(0); // no ordering -> segmentation fault + + SparseMatrix<double> matA(4,3); + matA.coeffRef(0,0) = 0.1; + matA.coeffRef(1,0) = 0.4; + matA.coeffRef(1,1) = 0.2; + matA.coeffRef(2,1) = 0.4; + matA.coeffRef(2,2) = 0.2; + matA.coeffRef(3,2) = 0.1; + + std::cout << "matA: " << std::endl << matA << std::endl; + + VectorXd b_ = VectorXd::Ones(4); + VectorXd x_(3); + + std::cout << "b_: " << std::endl << b_ << std::endl; + + solver.compute(matA); + if (solver.info() != Success) + { + std::cout << "decomposition failed" << std::endl; + return 0; + } + std::cout << "R: " << std::endl << solver.matrixR() << std::endl; + x_ = solver.solve(b_); + std::cout << "solved x_" << std::endl << x_ << std::endl; + std::cout << "ordering: " << solver.colsPermutation().indices().transpose() << std::endl; + + + /////////////////////////////////////////////////////////////////////// + // Directly in suitesparse + cholmod_common Common, *cc ; + cholmod_sparse A ; + cholmod_dense *X, *B, *Residual ; + double rnorm, one [2] = {1,0}, minusone [2] = {-1,0} ; + int mtype ; + + // start CHOLMOD + cc = &Common ; + cholmod_l_start (cc) ; + + // load A + A = viewAsCholmod(matA); + //A = (cholmod_sparse *) cholmod_l_read_matrix (stdin, 1, &mtype, cc) ; + std::cout << "A.xtype " << A.xtype << std::endl; + std::cout << "A.nrow " << A.nrow << std::endl; + std::cout << "A.ncol " << A.ncol << std::endl; + + // B = ones (size (A,1),1) + B = cholmod_l_ones (A.nrow, 1, A.xtype, cc) ; + + std::cout << "2" << std::endl; + // X = A\B + //X = SuiteSparseQR <double> (0, SPQR_DEFAULT_TOL, &A, B, cc) ; + X = SuiteSparseQR <double> (&A, B, cc); + + std::cout << "3" << std::endl; + // rnorm = norm (B-A*X) + Residual = cholmod_l_copy_dense (B, cc) ; + std::cout << "4" << std::endl; + cholmod_l_sdmult (&A, 0, minusone, one, X, Residual, cc) ; + std::cout << "5" << std::endl; + rnorm = cholmod_l_norm_dense (Residual, 2, cc) ; + printf ("2-norm of residual: %8.1e\n", rnorm) ; + printf ("rank %ld\n", cc->SPQR_istat [4]) ; + + // free everything and finish CHOLMOD + cholmod_l_free_dense (&Residual, cc) ; + //cholmod_l_free_sparse (A, cc) ; + cholmod_l_free_dense (&X, cc) ; + cholmod_l_free_dense (&B, cc) ; + cholmod_l_finish (cc) ; + return (0) ; +} diff --git a/src/examples/test_ccolamd.cpp b/src/examples/test_ccolamd.cpp index 4afe282409ec9ed76a7b8b21d5d577cd329995e8..f3d1e28aa3752abd6e01a1fbd9299510e45bfe24 100644 --- a/src/examples/test_ccolamd.cpp +++ b/src/examples/test_ccolamd.cpp @@ -24,7 +24,7 @@ typedef int IndexType; #include <eigen3/Eigen/SparseLU> // ccolamd -#include "../wolf_solver/ccolamd_ordering.h" +#include "solver/ccolamd_ordering.h" using namespace Eigen; diff --git a/src/examples/test_ccolamd_blocks.cpp b/src/examples/test_ccolamd_blocks.cpp index e585c7a3537da6b4888238f2b1335d84f4a3b196..50993d2acbc549a8eadd3c3b0f484f9c07f7424c 100644 --- a/src/examples/test_ccolamd_blocks.cpp +++ b/src/examples/test_ccolamd_blocks.cpp @@ -22,7 +22,7 @@ #include <eigen3/Eigen/SparseLU> // ccolamd -#include "../wolf_solver/ccolamd_ordering.h" +#include "solver/ccolamd_ordering.h" using namespace Eigen; diff --git a/src/examples/test_iQR.cpp b/src/examples/test_iQR.cpp new file mode 100644 index 0000000000000000000000000000000000000000..997b0a586fad3758a51a2844f374f5742bc8bb7b --- /dev/null +++ b/src/examples/test_iQR.cpp @@ -0,0 +1,289 @@ +/* + * test_iQR.cpp + * + * Created on: Jun 17, 2015 + * Author: jvallve + */ + +/* + * test_ccolamd_blocks.cpp + * + * Created on: Jun 12, 2015 + * Author: jvallve + */ + + +//std includes +#include <cstdlib> +#include <iostream> +#include <fstream> +#include <memory> +#include <random> +#include <typeinfo> +#include <ctime> +#include <queue> + +// eigen includes +#include <eigen3/Eigen/OrderingMethods> +#include <eigen3/Eigen/SparseQR> +#include <Eigen/SPQRSupport> + +// ccolamd +#include "solver/ccolamd_ordering.h" + +using namespace Eigen; + +void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols) +{ + for (uint i = row; i < row + Nrows; i++) + for (uint j = col; j < row + Ncols; j++) + original.coeffRef(i,j) = 0.0; + + original.makeCompressed(); +} + +void add_sparse_block(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col) +{ + for (uint r=0; r<ins.rows(); ++r) + for (uint c = 0; c < ins.cols(); c++) + if (ins(r,c) != 0) + original.coeffRef(r + row, c + col) += ins(r,c); +} + +//void add_sparse_row(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col) +//{ +// for (uint r=0; r<ins.rows(); ++r) +// for (uint c = 0; c < ins.cols(); c++) +// if (ins(r,c) != 0) +// original.coeffRef(r + row, c + col) += ins(r,c); +//} + +void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables, const int dim) +{ + ArrayXXi idx(dim, perm_nodes.indices().rows()); + idx.row(0) = dim * perm_nodes.indices().transpose(); + + for (unsigned int i = 1; i<dim; i++) + idx.row(i) = idx.row(i-1) + 1; + Map<ArrayXi> idx_blocks(idx.data(), dim*perm_nodes.indices().rows(), 1); + perm_variables.indices() = idx_blocks; +} + +void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size) +{ + int old_size = perm.indices().size(); + int dim = new_size - old_size; + VectorXi new_indices(new_size); + new_indices.head(old_size)= perm.indices(); + new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1); + perm.resize(new_size); + perm.indices() = new_indices; +} + + +//main +int main(int argc, char *argv[]) +{ + if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1) + { + std::cout << "Please call me with: [./test_iQR SIZE DIM], where:" << std::endl; + std::cout << " - SIZE: integer size of the problem" << std::endl; + std::cout << " - DIM: integer dimension of the nodes" << std::endl; + std::cout << "EXIT due to bad user input" << std::endl << std::endl; + return -1; + } + int size = atoi(argv[1]); + int dim = atoi(argv[2]); + + // Problem variables + SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver, solver2; + + MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim); + SparseMatrix<double> A(0,0), + A_ordered(0,0); + VectorXd b, + b_ordered, + x_ordered, + x; + int n_measurements = 0; + int n_nodes = 0; + + // ordering variables + SparseMatrix<int> A_nodes(0,0), A_nodes_ordered(0,0); + PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(0), acc_permutation_nodes_matrix(0); + + CCOLAMDOrdering<int> ordering; + VectorXi nodes_ordering_constraints; + + // results variables + clock_t t1, t2; + double time1=0, time2=0; + + std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl; + + // GENERATING MEASUREMENTS + std::vector<std::vector<int>> measurements; + for (unsigned int i = 0; i < size; i++) + { + std::vector<int> meas(0); + if (i == 0) //prior + { + meas.push_back(0); + measurements.push_back(meas); + meas.clear(); + } + else //odometry + { + meas.push_back(i-1); + meas.push_back(i); + measurements.push_back(meas); + meas.clear(); + } + if (i > size / 2) // loop closures + { + meas.push_back(0); + meas.push_back(i); + measurements.push_back(meas); + meas.clear(); + } + } + + // INCREMENTAL LOOP + for (unsigned int i = 0; i < measurements.size(); i++) + { + std::cout << "========================= MEASUREMENT " << i << ":" << std::endl; + std::vector<int> measurement = measurements.at(i); + + // AUGMENT THE PROBLEM ---------------------------- + n_measurements++; + while (n_nodes < measurement.back()+1) + { + n_nodes++; + // Resize accumulated permutations + augment_permutation(acc_permutation_matrix, n_nodes*dim); + augment_permutation(acc_permutation_nodes_matrix, n_nodes); + + // Resize state + x.conservativeResize(n_nodes*dim); + x_ordered.conservativeResize(n_nodes*dim); + } + A.conservativeResize(n_measurements*dim,n_nodes*dim); + A_ordered.conservativeResize(n_measurements*dim,n_nodes*dim); + b.conservativeResize(n_measurements*dim); + A_nodes.conservativeResize(n_measurements,n_nodes); + A_nodes_ordered.conservativeResize(n_measurements,n_nodes); + + // ADD MEASUREMENTS + int min_ordered_node = n_nodes; + for (unsigned int j = 0; j < measurement.size(); j++) + { + int ordered_node = acc_permutation_nodes_matrix.indices()(measurement.at(j)); + std::cout << "measurement.at(j) " << measurement.at(j) << std::endl; + std::cout << "ordered_block " << ordered_node << std::endl; + + add_sparse_block(2*omega, A, A.rows()-dim, measurement.at(j) * dim); + add_sparse_block(2*omega, A_ordered, A_ordered.rows()-dim, ordered_node * dim); + + A_nodes.coeffRef(A_nodes.rows()-1, measurement.at(j)) = 1; + A_nodes_ordered.coeffRef(A_nodes_ordered.rows()-1, ordered_node) = 1; + + b.segment(b.size() - dim, dim) = VectorXd::LinSpaced(dim, b.size()-dim, b.size()-1); + // store minimum ordered node + if (min_ordered_node > ordered_node) + min_ordered_node = ordered_node; + } + std::cout << "min_ordered_node " << min_ordered_node << std::endl; + + std::cout << "Solving Ax = b" << std::endl; + std::cout << "A_nodes: " << std::endl << MatrixXi::Identity(A_nodes.rows(), A_nodes.rows()) * A_nodes << std::endl << std::endl; + std::cout << "A_nodes_ordered: " << std::endl << MatrixXi::Identity(A_nodes.rows(), A_nodes.rows()) * A_nodes_ordered << std::endl << std::endl; + std::cout << "A: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A << std::endl << std::endl; + std::cout << "A_ordered: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A_ordered << std::endl << std::endl; + std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl; + + + // BLOCK REORDERING ------------------------------------ + t1 = clock(); + if (n_nodes > 1 && n_nodes - min_ordered_node > 2) + { + // ordering constraints + nodes_ordering_constraints.resize(A_nodes_ordered.cols()); + nodes_ordering_constraints = A_nodes_ordered.bottomRows(1).transpose(); + std::cout << "nodes_ordering_constraints: " << std::endl << nodes_ordering_constraints.transpose() << std::endl << std::endl; + + std::cout << "old acc_permutation_nodes: " << std::endl << acc_permutation_nodes_matrix.indices().transpose() << std::endl << std::endl; + std::cout << "old acc_permutation: " << std::endl << acc_permutation_matrix.indices().transpose() << std::endl << std::endl; + + A_nodes_ordered.makeCompressed(); + // computing nodes ordering + PermutationMatrix<Dynamic, Dynamic, int> permutation_nodes_matrix(A_nodes_ordered.cols()); + ordering(A_nodes_ordered, permutation_nodes_matrix, nodes_ordering_constraints.data()); + + // applying block ordering + PermutationMatrix<Dynamic, Dynamic, int> permutation_matrix(A_ordered.cols()); + permutation_2_block_permutation(permutation_nodes_matrix, permutation_matrix , dim); + + std::cout << "new permutation_nodes: " << std::endl << permutation_nodes_matrix.indices().transpose() << std::endl << std::endl; + std::cout << "new permutation: " << std::endl << permutation_matrix.indices().transpose() << std::endl << std::endl; + + A_ordered = (A_ordered * permutation_matrix.transpose()).sparseView(); + A_ordered.makeCompressed(); + A_nodes_ordered = (A_nodes_ordered * permutation_nodes_matrix.transpose()).sparseView(); + A_nodes_ordered.makeCompressed(); + A.makeCompressed(); + + // accumulating permutations + acc_permutation_nodes_matrix = permutation_nodes_matrix * acc_permutation_nodes_matrix; + acc_permutation_matrix = permutation_matrix * acc_permutation_matrix; + + std::cout << "new acc_permutation_nodes: " << std::endl << acc_permutation_nodes_matrix.indices().transpose() << std::endl << std::endl; + std::cout << "new acc_permutation: " << std::endl << acc_permutation_matrix.indices().transpose() << std::endl << std::endl; + } + //std::cout << "incrementally ordered A Block structure: " << std::endl << MatrixXi::Identity(A_nodes.rows(), A_nodes.rows()) * A_nodes_ordered << std::endl << std::endl; + //std::cout << "accumulated ordered A Block structure: " << std::endl << A_nodes * acc_permutation_nodes_matrix.transpose() << std::endl << std::endl; + + //std::cout << "A: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A << std::endl << std::endl; + //std::cout << "ordered A: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A_ordered << std::endl << std::endl; + //std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl; + + // solving + A_ordered.makeCompressed(); + solver.compute(A_ordered); + if (solver.info() != Success) + { + std::cout << "decomposition failed" << std::endl; + return 0; + } + //std::cout << "R: " << std::endl << solver.matrixR() << std::endl; + x_ordered = solver.solve(b); + x_ordered = acc_permutation_matrix.inverse() * x_ordered; + time1 += ((double) clock() - t1) / CLOCKS_PER_SEC; + + // WITHOUT ORDERING + t2 = clock(); + A.makeCompressed(); + solver2.compute(A); + if (solver2.info() != Success) + { + std::cout << "decomposition failed" << std::endl; + return 0; + } + //std::cout << "no ordering? " << solver2.colsPermutation().indices().transpose() << std::endl; + x = solver2.solve(b); + time2 += ((double) clock() - t2) / CLOCKS_PER_SEC; + + // RESULTS ------------------------------------ + std::cout << "========================= RESULTS " << i << ":" << std::endl; + std::cout << "NO REORDERING: solved in " << time2*1e3 << " ms | " << solver2.matrixR().nonZeros() << " nonzeros in R"<< std::endl; + std::cout << "BLOCK REORDERING: solved in " << time1*1e3 << " ms | " << solver.matrixR().nonZeros() << " nonzeros in R"<< std::endl; + //std::cout << "x1 = " << x.transpose() << std::endl; + std::cout << "x = " << x.transpose() << std::endl; + std::cout << "x = " << x_ordered.transpose() << std::endl; + } +} + + + + + + diff --git a/src/examples/test_incremental_ccolamd_blocks.cpp b/src/examples/test_incremental_ccolamd_blocks.cpp new file mode 100644 index 0000000000000000000000000000000000000000..51609f915c1007c4d6058130c66cb0adcfc5687e --- /dev/null +++ b/src/examples/test_incremental_ccolamd_blocks.cpp @@ -0,0 +1,271 @@ +/* + * test_ccolamd_blocks.cpp + * + * Created on: Jun 12, 2015 + * Author: jvallve + */ + + +//std includes +#include <cstdlib> +#include <iostream> +#include <fstream> +#include <memory> +#include <random> +#include <typeinfo> +#include <ctime> +#include <queue> + +// eigen includes +#include <eigen3/Eigen/OrderingMethods> +#include <eigen3/Eigen/CholmodSupport> +#include <eigen3/Eigen/SparseLU> + +// ccolamd +#include "solver/ccolamd_ordering.h" + +using namespace Eigen; + +void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols) +{ + for (uint i = row; i < row + Nrows; i++) + for (uint j = col; j < row + Ncols; j++) + original.coeffRef(i,j) = 0.0; + + original.makeCompressed(); +} + +void add_sparse_block(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col) +{ + for (uint r=0; r<ins.rows(); ++r) + for (uint c = 0; c < ins.cols(); c++) + if (ins(r,c) != 0) + original.coeffRef(r + row, c + col) += ins(r,c); +} + +void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm, PermutationMatrix<Dynamic, Dynamic, int> &perm_blocks, const int dim, const int size) +{ + ArrayXXi idx(dim, size); + idx.row(0) = dim * perm.indices().transpose(); + + for (unsigned int i = 1; i<dim; i++) + idx.row(i) = idx.row(i-1) + 1; + Map<ArrayXi> idx_blocks(idx.data(), dim*size, 1); + perm_blocks.indices() = idx_blocks; +} + + +//main +int main(int argc, char *argv[]) +{ + if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1) + { + std::cout << "Please call me with: [./test_ccolamd SIZE DIM], where:" << std::endl; + std::cout << " - SIZE: integer size of the problem" << std::endl; + std::cout << " - DIM: integer dimension of the nodes" << std::endl; + std::cout << "EXIT due to bad user input" << std::endl << std::endl; + return -1; + } + int size = atoi(argv[1]); + int dim = atoi(argv[2]); + + // Problem variables + //CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3; + SparseLU < SparseMatrix<double>, NaturalOrdering<int> > solver, solver2, solver3; + MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim); + SparseMatrix<double> H(dim,dim), + H_ordered(dim,dim), + H_b_ordered(dim,dim); + VectorXd b(dim), + b_ordered(dim), + b_b_ordered(dim), + x_b_ordered(dim), + x_ordered(dim), + x(dim); + + // ordering variables + SparseMatrix<int> factors(1,1), factors_ordered(1,1); + ArrayXi acc_permutation(dim), + acc_permutation_b(dim), + acc_permutation_factors(1); + acc_permutation = ArrayXi::LinSpaced(dim,0,dim-1); + acc_permutation_b = acc_permutation; + acc_permutation_factors(0) = 0; + + CCOLAMDOrdering<int> ordering; + VectorXi factor_ordering_constraints(1); + VectorXi ordering_constraints(1); + + // results variables + clock_t t1, t2, t3; + double time1=0, time2=0, time3=0; + + // INITIAL STATE + add_sparse_block(5*omega, H, 0, 0); + factors.insert(0,0) = 1; + b.head(dim) = VectorXd::LinSpaced(Sequential, dim, 0, dim-1); + + std::cout << "STARTING INCREMENTAL TEST" << std::endl << std::endl; + + // INCREMENTAL LOOP + for (unsigned int i = 1; i < size; i++) + { + std::cout << "========================= STEP " << i << ":" << std::endl; + // AUGMENT THE PROBLEM ---------------------------- + H.conservativeResize((i+1)*dim,(i+1)*dim); + H_ordered.conservativeResize((i+1)*dim,(i+1)*dim); + H_b_ordered.conservativeResize((i+1)*dim,(i+1)*dim); + b.conservativeResize((i+1)*dim); + b_ordered.conservativeResize((i+1)*dim); + b_b_ordered.conservativeResize((i+1)*dim); + x.conservativeResize((i+1)*dim); + x_ordered.conservativeResize((i+1)*dim); + x_b_ordered.conservativeResize((i+1)*dim); + factors.conservativeResize(i+1, i+1); + + // Odometry + add_sparse_block(5*omega, H, i*dim, i*dim); + add_sparse_block(omega, H, i*dim, (i-1)*dim); + add_sparse_block(omega, H, (i-1)*dim, i*dim); + factors.insert(i,i) = 1; + factors.insert(i,i-1) = 1; + factors.insert(i-1,i) = 1; + + // Loop Closure + if (i == size-1) + { + add_sparse_block(2*omega, H, 0, i*dim); + add_sparse_block(2*omega, H, i*dim, 0); + factors.insert(0,i) = 1; + factors.insert(i,0) = 1; + } + + // r.h.v + b.segment(i*dim, dim) = VectorXd::LinSpaced(Sequential, dim, dim*i, dim *(i+1)-1); + + + std::cout << "Solving factor graph:" << std::endl; + std::cout << "Factors: " << std::endl << factors * MatrixXi::Identity((i+1), (i+1)) << std::endl << std::endl; +// std::cout << "H: " << std::endl << H * MatrixXd::Identity(dim*(i+1), dim*(i+1)) << std::endl << std::endl; + + // SOLVING WITHOUT REORDERING ------------------------------------ + // solve Hx = b + t1 = clock(); + solver.compute(H); + if (solver.info() != Success) + { + std::cout << "decomposition failed" << std::endl; + return 0; + } + x = solver.solve(b); + time1 += ((double) clock() - t1) / CLOCKS_PER_SEC; + + + // SOLVING WITH REORDERING ------------------------------------ + // Order with previous orderings + acc_permutation.conservativeResize(dim*(i+1)); + acc_permutation.tail(dim) = ArrayXi::LinSpaced(dim,dim*i,dim*(i+1)-1); + PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(dim*(i+1)); + acc_permutation_matrix.indices() = acc_permutation; + b_ordered = acc_permutation_matrix * b; + H_ordered = H.twistedBy(acc_permutation_matrix); + + // ordering constraints + ordering_constraints.resize(dim*(i+1)); + ordering_constraints = ((H_ordered.rightCols(3) * MatrixXd::Ones(3,1)).array() == 0).select(VectorXi::Zero(dim*(i+1)),VectorXi::Ones(dim*(i+1))); + + // variable ordering + t2 = clock(); + H_ordered.makeCompressed(); + + PermutationMatrix<Dynamic, Dynamic, int> permutation_matrix(dim*(i+1)); + ordering(H_ordered, permutation_matrix, ordering_constraints.data()); + + // applying ordering + acc_permutation_matrix = permutation_matrix * acc_permutation_matrix; + acc_permutation = acc_permutation_matrix.indices(); + b_ordered = permutation_matrix * b_ordered; + H_ordered = H_ordered.twistedBy(permutation_matrix); + + // solve Hx = b + solver2.compute(H_ordered); + if (solver2.info() != Success) + { + std::cout << "decomposition failed" << std::endl; + return 0; + } + x_ordered = solver2.solve(b_ordered); + x_ordered = acc_permutation_matrix.inverse() * x_ordered; + time2 += ((double) clock() - t2) / CLOCKS_PER_SEC; + + + // SOLVING WITH BLOCK REORDERING ------------------------------------ + // Order with previous orderings + acc_permutation_b.conservativeResize(dim*(i+1)); + acc_permutation_b.tail(dim) = ArrayXi::LinSpaced(dim,dim*i,dim*(i+1)-1); + PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_b_matrix(dim*(i+1)); + acc_permutation_b_matrix.indices() = acc_permutation_b; + b_b_ordered = acc_permutation_b_matrix * b; + H_b_ordered = H.twistedBy(acc_permutation_b_matrix); + + acc_permutation_factors.conservativeResize(i+1); + acc_permutation_factors(i) = i; + PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_factors_matrix(dim*(i+1)); + acc_permutation_factors_matrix.indices() = acc_permutation_factors; + factors_ordered = factors.twistedBy(acc_permutation_factors_matrix); + + // ordering constraints + factor_ordering_constraints.resize(i); + factor_ordering_constraints = factors_ordered.rightCols(1); + + // block ordering + t3 = clock(); + factors_ordered.makeCompressed(); + + PermutationMatrix<Dynamic, Dynamic, int> permutation_factors_matrix(i+1); + ordering(factors_ordered, permutation_factors_matrix, factor_ordering_constraints.data()); + + // applying ordering + permutation_2_block_permutation(permutation_factors_matrix, permutation_matrix , dim, i+1); + acc_permutation_factors_matrix = permutation_factors_matrix * acc_permutation_factors_matrix; + acc_permutation_factors = acc_permutation_factors_matrix.indices(); + acc_permutation_b_matrix = permutation_matrix * acc_permutation_b_matrix; + acc_permutation_b = acc_permutation_b_matrix.indices(); + b_b_ordered = permutation_matrix * b_b_ordered; + H_b_ordered = H_b_ordered.twistedBy(permutation_matrix); + + // solve Hx = b + solver3.compute(H_b_ordered); + if (solver3.info() != Success) + { + std::cout << "decomposition failed" << std::endl; + return 0; + } + x_b_ordered = solver3.solve(b_b_ordered); + x_b_ordered = acc_permutation_b_matrix.inverse() * x_b_ordered; + time3 += ((double) clock() - t3) / CLOCKS_PER_SEC; + + + // RESULTS ------------------------------------ + std::cout << "========================= RESULTS " << i << ":" << std::endl; + std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl; + std::cout << "REORDERING: solved in " << time2*1e3 << " ms" << std::endl; + std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl; + std::cout << "x1 = " << x.transpose() << std::endl; + std::cout << "x2 = " << x_ordered.transpose() << std::endl; + std::cout << "x3 = " << x_b_ordered.transpose() << std::endl; + } + + // RESULTS ------------------------------------ + std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl; + std::cout << "REORDERING: solved in " << time2*1e3 << " ms" << std::endl; + std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl; + + //std::cout << "x = " << x.transpose() << std::endl; + //std::cout << "x = " << x_ordered.transpose() << std::endl; + //std::cout << "x = " << x_b_ordered.transpose() << std::endl; +} + + + + diff --git a/src/examples/test_permutations.cpp b/src/examples/test_permutations.cpp new file mode 100644 index 0000000000000000000000000000000000000000..37b9ed85f480ce0d463494c150304db885e3bf39 --- /dev/null +++ b/src/examples/test_permutations.cpp @@ -0,0 +1,91 @@ +/* + * test_permutations.cpp + * + * Created on: Jun 15, 2015 + * Author: jvallve + */ + + +//std includes +#include <cstdlib> +#include <iostream> +#include <fstream> +#include <memory> +#include <random> +#include <typeinfo> +#include <ctime> +#include <queue> + +// eigen includes +#include <eigen3/Eigen/OrderingMethods> +#include <eigen3/Eigen/CholmodSupport> +#include <eigen3/Eigen/SparseLU> + + +using namespace Eigen; + +//main +int main(int argc, char *argv[]) +{ + PermutationMatrix<Dynamic, Dynamic, int> P1(5), P2(5), P3(5), P4(5); + P1.setIdentity(); + P2.setIdentity(); + P3.setIdentity(); + + VectorXd a = VectorXd::LinSpaced(5,1,5); + MatrixXd A= a.asDiagonal(); + SparseMatrix<double> B = A.sparseView(); + B.makeCompressed(); + + std::cout << "A (dense)" << std::endl << A << std::endl << std::endl; + std::cout << "B (sparse)" << std::endl << B << std::endl << std::endl; + + P1.indices()(3) = 4; + P1.indices()(4) = 3; + + std::cout << "Permutation 1" << std::endl << P1.indices().transpose() << std::endl << std::endl; + + P2.indices()(0) = 4; + P2.indices()(4) = 0; + + std::cout << "Permutation 2" << std::endl << P2.indices().transpose() << std::endl << std::endl; + + std::cout << "Pre-multiplying: Permutating rows" << std::endl; + std::cout << "P1 * A" << std::endl << P1 * A << std::endl << std::endl; + std::cout << "P1 * B" << std::endl << P1 * B << std::endl << std::endl; + SparseMatrix<double> C = (P1 * B).sparseView(); + std::cout << "(P1 * B).bottomRows(1)" << std::endl << C.bottomRows(1) << std::endl << std::endl; + + std::cout << "Post-multiplying: Permutating cols" << std::endl; + std::cout << "A * P1.transpose()" << std::endl << A * P1.transpose()<< std::endl << std::endl; + std::cout << "B * P1.transpose()" << std::endl << B * P1.transpose()<< std::endl << std::endl; + + std::cout << "Pre&post-multiplying:" << std::endl; + std::cout << "P1 * A * P1.transpose()" << std::endl << P1 * A * P1.transpose() << std::endl << std::endl; + std::cout << "P2 * P1 * A * P1.transpose() * P2.transpose()" << std::endl << P2 * P1 * A * P1.transpose() * P2.transpose() << std::endl << std::endl; + std::cout << "P1 * P2 * A * P2.transpose() * P1.transpose()" << std::endl << P1 * P2 * A * P2.transpose() * P1.transpose() << std::endl << std::endl; + + P3 = P1 * P2; + + std::cout << "Permutation P3 = P1 * P2" << std::endl << P3.indices().transpose() << std::endl << std::endl; + std::cout << "P3 * A * P3.transpose()" << std::endl << P3 * A * P3.transpose() << std::endl << std::endl; + + std::cout << "Permutating indices" << std::endl; + ArrayXi acc_permutations(5); + acc_permutations << 0,1,2,3,4; + + std::cout << "acc_permutations: " << acc_permutations.transpose() << std::endl; + + std::cout << "P1: " << P1.indices().transpose() << std::endl; + std::cout << "P1 * acc_permutations: " << (P1 * acc_permutations.matrix()).transpose() << std::endl; + std::cout << "P1.inverse() * acc_permutations: " << (P1.inverse() * acc_permutations.matrix()).transpose() << std::endl; + + std::cout << "P2: " << P2.indices().transpose() << std::endl; + std::cout << "P2 * (P1 * acc_permutations): " << (P2 * (P1 * acc_permutations.matrix())).transpose() << std::endl; + std::cout << "(P2 * P1).inverse() * acc_permutations): " << ((P2 * P1).inverse() * acc_permutations.matrix()).transpose() << std::endl; + P4 = P1 * P2 * P3; + std::cout << "Permutation P4 = P1 * P2 * P3: " << P4.indices().transpose() << std::endl; + std::cout << "P3 * (P2 * (P1 * acc_permutations)): " << (P3 * (P2 * (P1 * acc_permutations.matrix()))).transpose() << std::endl; + +// Map<> +}