Skip to content
Snippets Groups Projects
Commit bd467fb1 authored by jvallve's avatar jvallve
Browse files

afegit test_ccolamd_blocks i mogut ccolamd_ordering.h a carpeta wolf_solver...

afegit test_ccolamd_blocks i mogut ccolamd_ordering.h a carpeta wolf_solver que contindrà tot el codi de solver de wolf
parent 15d8bd6f
No related branches found
No related tags found
No related merge requests found
......@@ -31,10 +31,14 @@ ADD_EXECUTABLE(test_ceres_odom_iterative test_ceres_odom_iterative.cpp)
TARGET_LINK_LIBRARIES(test_ceres_odom_iterative ${PROJECT_NAME})
# Testing a ccolamd test
# FIND_PACKAGE(Cholmod)
# include_directories(${CHOLMOD_INCLUDES})
# ADD_EXECUTABLE(test_ccolamd test_ccolamd.cpp)
# TARGET_LINK_LIBRARIES(test_ccolamd ${CHOLMOD_LIBRARIES} ${PROJECT_NAME})
#FIND_PACKAGE(Cholmod)
#include_directories(${CHOLMOD_INCLUDES})
#ADD_EXECUTABLE(test_ccolamd test_ccolamd.cpp)
#TARGET_LINK_LIBRARIES(test_ccolamd ${CHOLMOD_LIBRARIES} ${PROJECT_NAME})
#
# Testing a ccolamd test
#ADD_EXECUTABLE(test_ccolamd_blocks test_ccolamd_blocks.cpp)
#TARGET_LINK_LIBRARIES(test_ccolamd_blocks ${CHOLMOD_LIBRARIES} ${PROJECT_NAME})
# Building and populating the wolf tree
# ADD_EXECUTABLE(test_wolf_tree test_wolf_tree.cpp)
......
......@@ -24,63 +24,10 @@ typedef int IndexType;
#include <eigen3/Eigen/SparseLU>
// ccolamd
#include "ccolamd.h"
#include "../wolf_solver/ccolamd_ordering.h"
using namespace Eigen;
template<typename Index>
class CCOLAMDOrdering
{
public:
typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType;
typedef Matrix<Index, Dynamic, 1> IndexVector;
template<typename MatrixType>
void operator()(const MatrixType& mat, PermutationType& perm, Index* cmember = nullptr)
{
Index m = mat.rows();
Index n = mat.cols();
Index nnz = mat.nonZeros();
// Get the recommended value of Alen to be used by colamd
Index Alen = ccolamd_recommended(nnz, m, n);
// Set the default parameters
double knobs[CCOLAMD_KNOBS];
Index stats[CCOLAMD_STATS];
ccolamd_set_defaults(knobs);
IndexVector p(n + 1), A(Alen);
for (Index i = 0; i <= n; i++)
p(i) = mat.outerIndexPtr()[i];
for (Index i = 0; i < nnz; i++)
A(i) = mat.innerIndexPtr()[i];
// Call CColamd routine to compute the ordering
Index info = compute_ccolamd(m, n, Alen, A.data(), p.data(), knobs, stats, cmember);
if (!info)
assert(info && "COLAMD failed ");
perm.resize(n);
for (Index i = 0; i < n; i++)
perm.indices()(p(i)) = i;
}
private:
int compute_ccolamd(int &m, int &n, int &Alen, int* A, int* p, double* knobs, int* stats, int* cmember)
{
int info = ccolamd(m, n, Alen, A, p, knobs, stats, cmember);
//ccolamd_report (stats) ;
return info;
}
long int compute_ccolamd(long int &m, long int &n, long int &Alen, long int* A, long int* p, double* knobs, long int* stats, long int* cmember)
{
long int info = ccolamd_l(m, n, Alen, A, p, knobs, stats, cmember);
//ccolamd_l_report (stats) ;
return info;
}
};
//main
int main(int argc, char *argv[])
{
......@@ -91,13 +38,13 @@ int main(int argc, char *argv[])
std::cout << "EXIT due to bad user input" << std::endl << std::endl;
return -1;
}
IndexType size = atoi(argv[1]); //ordering enabled
IndexType size = atoi(argv[1]);
SparseMatrix<double, ColMajor, IndexType> A(size, size), Aordered(size, size);
CholmodSupernodalLLT < SparseMatrix<double, ColMajor, IndexType> > solver;
PermutationMatrix<Dynamic, Dynamic, IndexType> perm(size);
CCOLAMDOrdering<IndexType> ordering;
Matrix<IndexType, Dynamic, 1> ordering_constraints(size);
Matrix<IndexType, Dynamic, 1> ordering_constraints = Matrix<IndexType, Dynamic, 1>::Ones(size);
VectorXd b(size), bordered(size), xordered(size), x(size);
clock_t t1, t2, t3;
double time1, time2, time3;
......@@ -135,8 +82,9 @@ int main(int argc, char *argv[])
// SOLVING AFTER REORDERING ------------------------------------
// ordering constraints
ordering_constraints(size-1) = 1;
ordering_constraints(0) = 1;
ordering_constraints(size-1) = 2;
ordering_constraints(0) = 2;
// ordering
t2 = clock();
A.makeCompressed();
......
/*
* 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 "../wolf_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++)
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]);
SparseMatrix<double> H(size * dim, size * dim), H_ordered(size * dim, size * dim), H_block_ordered(size * dim, size * dim);
SparseMatrix<int> FactorMatrix(size,size);
CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3;
PermutationMatrix<Dynamic, Dynamic, int> perm(size), perm_blocks(size * dim);
CCOLAMDOrdering<int> ordering;
VectorXi block_ordering_constraints = VectorXi::Ones(size);
VectorXi ordering_constraints = VectorXi::Ones(size*dim);
VectorXd b(size * dim), b_ordered(size * dim), b_block_ordered(size * dim), x_block_ordered(size * dim), x_ordered(size * dim), x(size * dim);
clock_t t1, t2, t3;
double time1, time2, time3;
MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
// BUILD THE PROBLEM ----------------------------
//Fill H & b
for (unsigned int i = 0; i < size; i++)
{
add_sparse_block(5*omega, H, i*dim, i*dim);
FactorMatrix.insert(i,i) = 1;
if (i > 0)
{
add_sparse_block(omega, H, i*dim, (i-1)*dim);
add_sparse_block(omega, H, (i-1)*dim, i*dim);
FactorMatrix.insert(i,i-1) = 1;
FactorMatrix.insert(i-1,i) = 1;
}
b.segment(i*dim, dim) = VectorXd::Constant(dim, i+1);
}
add_sparse_block(2*omega, H, 0, (size - 1)*dim);
add_sparse_block(2*omega, H, (size-1)*dim, 0);
FactorMatrix.insert(0,size-1) = 1;
FactorMatrix.insert(size-1,0) = 1;
std::cout << "Solving factor graph:" << std::endl;
std::cout << "Factors: " << std::endl << FactorMatrix * MatrixXi::Identity(size,size) << 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 AFTER REORDERING ------------------------------------
// ordering constraints
ordering_constraints.segment(dim * (size-1), dim) = VectorXi::Constant(dim,2);
ordering_constraints.segment(0, dim) = VectorXi::Constant(dim,2);
// variable ordering
t2 = clock();
H.makeCompressed();
std::cout << "Reordering using CCOLAMD:" << std::endl;
std::cout << "ordering_constraints = " << std::endl << ordering_constraints.transpose() << std::endl << std::endl;
ordering(H, perm, ordering_constraints.data());
b_ordered = perm * b;
H_ordered = H.twistedBy(perm);
// 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 = perm.inverse() * x_ordered;
time2 = ((double) clock() - t2) / CLOCKS_PER_SEC;
// SOLVING AFTER BLOCK REORDERING ------------------------------------
// ordering constraints
block_ordering_constraints(size-1) = 2;
block_ordering_constraints(0) = 2;
// block ordering
t3 = clock();
FactorMatrix.makeCompressed();
std::cout << "Reordering using CCOLAMD:" << std::endl;
std::cout << "block_ordering_constraints = " << std::endl << block_ordering_constraints.transpose() << std::endl << std::endl;
ordering(FactorMatrix, perm_blocks, block_ordering_constraints.data());
// variable ordering
permutation_2_block_permutation(perm_blocks, perm , dim, size);
b_block_ordered = perm * b;
H_block_ordered = H.twistedBy(perm);
// solve Hx = b
solver3.compute(H_block_ordered);
if (solver3.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
x_block_ordered = solver3.solve(b_block_ordered);
x_block_ordered = perm.inverse() * x_block_ordered;
time3 = ((double) clock() - t3) / CLOCKS_PER_SEC;
// 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_block_ordered.transpose() << std::endl;
}
/*
* ccolamd_ordering.h
*
* Created on: Jun 12, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
#define TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
//std includes
#include <iostream>
// ccolamd
#include "ccolamd.h"
namespace Eigen{
template<typename Index>
class CCOLAMDOrdering
{
public:
typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType;
typedef Matrix<Index, Dynamic, 1> IndexVector;
template<typename MatrixType>
void operator()(const MatrixType& mat, PermutationType& perm, Index* cmember = nullptr)
{
Index m = mat.rows();
Index n = mat.cols();
Index nnz = mat.nonZeros();
// Get the recommended value of Alen to be used by colamd
Index Alen = ccolamd_recommended(nnz, m, n);
// Set the default parameters
double knobs[CCOLAMD_KNOBS];
Index stats[CCOLAMD_STATS];
ccolamd_set_defaults(knobs);
IndexVector p(n + 1), A(Alen);
for (Index i = 0; i <= n; i++)
p(i) = mat.outerIndexPtr()[i];
for (Index i = 0; i < nnz; i++)
A(i) = mat.innerIndexPtr()[i];
// std::cout << "p = " << std::endl << p.transpose() << std::endl;
// std::cout << "A = " << std::endl << A.head(nnz).transpose() << std::endl;
// Call CColamd routine to compute the ordering
Index info = compute_ccolamd(m, n, Alen, A.data(), p.data(), knobs, stats, cmember);
if (!info)
assert(info && "CCOLAMD failed ");
perm.resize(n);
for (Index i = 0; i < n; i++)
perm.indices()(p(i)) = i;
}
private:
int compute_ccolamd(int &m, int &n, int &Alen, int* A, int* p, double* knobs, int* stats, int* cmember)
{
int info = ccolamd(m, n, Alen, A, p, knobs, stats, cmember);
//ccolamd_report (stats) ;
return info;
}
long int compute_ccolamd(long int &m, long int &n, long int &Alen, long int* A, long int* p, double* knobs, long int* stats, long int* cmember)
{
long int info = ccolamd_l(m, n, Alen, A, p, knobs, stats, cmember);
//ccolamd_l_report (stats) ;
return info;
}
};
}
#endif /* TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_ */
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment