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<>
+}