some old code i had for compressing matrices due to active/inactive sets

active_set.h

#ifndef INDEX_MANAGEMENT_H
#define INDEX_MANAGEMENT_H

#include <vector>
#include <Eigen/Dense>
#include <Eigen/Sparse>












class IndexManager {
    public:
        enum VariableType { Active, Inactive};
        constexpr static VariableType other(VariableType v) {
            if(v == VariableType::Active) {
                return VariableType::Inactive;
            } else {
                return VariableType::Active;
            }
        }
        using Index = int;
        using IndexList = std::vector<int>;

        struct IndexPair {
            IndexList indices;
            IndexList inverse;
            size_t size() const { return indices.size(); }
            void updateInverse(size_t total_size);
        };


        size_t active_size() const { return m_active_pair.size(); }
        size_t reduced_size() const { return active_size(); }
        template <VariableType VarType>
            size_t var_size() const { return getIndexPair<VarType>().size();}

        template <VariableType VarType=VariableType::Active>
        void updateOther();
        void updateOther(const IndexPair& a, IndexPair& b) const;

        IndexManager(int k = -1): m_total_size(k) {}

        void set_total_size(int k) { m_total_size = k; }


        template <VariableType VarType>
            void setVariableType(const IndexList& indices);

        template <typename Collection>
        void setActive(const Collection& indices);


        template <VariableType VarType=VariableType::Active>
        const IndexPair& getIndexPair() const;
        template <VariableType VarType=VariableType::Active>
        IndexPair& getIndexPair() ;


        template <VariableType VarType=VariableType::Active>
        const IndexList& getIndices() const { return getIndexPair<VarType>().indices; }
        template <VariableType VarType=VariableType::Active>
        const IndexList& getInverseIndices() const { return getIndexPair<VarType>().inverse; }

        const IndexList& getActiveIndices() const;
        const IndexList& getInactiveIndices() const;
        const IndexList& getActiveInverseIndices() const;
        const IndexList& getInactiveInverseIndices() const;

        template <VariableType VarType=VariableType::Active>
        Index reduce(Index i) const { return getIndices<VarType>()[i]; }
        template <VariableType VarType=VariableType::Active>
        Index expand(Index i) const { return getInverseIndices<VarType>()[i]; }

        template <VariableType VarType, typename Derived, typename RetType=typename Eigen::internal::remove_all<typename Eigen::internal::eval<Derived>::type>::type>
             RetType reduce_vartype(const Eigen::MatrixBase<Derived>& M) const;

        template <VariableType VarType, typename Derived, typename RetType=typename Eigen::internal::remove_all<typename Eigen::internal::eval<Derived>::type>::type>
             RetType expand_vartype(const Eigen::MatrixBase<Derived>& M) const;

        template <typename Derived, typename RetType=typename Eigen::internal::remove_all<typename Eigen::internal::eval<Derived>::type>::type>
             RetType reduce(const Eigen::MatrixBase<Derived>& M) const;

        template <typename Derived, typename RetType=typename Eigen::internal::remove_all<typename Eigen::internal::eval<Derived>::type>::type>
             RetType expand(const Eigen::MatrixBase<Derived>& M) const;

        template <typename T>
            Eigen::SparseMatrix<T> reduce(const Eigen::SparseMatrix<T>& A) const;
        template <VariableType VarType, typename T>
            Eigen::SparseMatrix<T> reduce_vartype(const Eigen::SparseMatrix<T>& A) const;
        template <IndexManager::VariableType VarType, IndexManager::VariableType VarType2, typename T>
            Eigen::SparseMatrix<T> reduceMatrix(const Eigen::SparseMatrix<T>& A) const;
    private:
        Index m_total_size;
        IndexPair m_active_pair;
        IndexPair m_inactive_pair;
};

template <>
const IndexManager::IndexPair& IndexManager::getIndexPair<IndexManager::VariableType::Active>() const;
template <>
const IndexManager::IndexPair& IndexManager::getIndexPair<IndexManager::VariableType::Inactive>() const;

template <>
IndexManager::IndexPair& IndexManager::getIndexPair<IndexManager::VariableType::Active>() ;
template <>
IndexManager::IndexPair& IndexManager::getIndexPair<IndexManager::VariableType::Inactive>() ;


template <IndexManager::VariableType VarType>
void IndexManager::updateOther() {
    constexpr VariableType o = other(VarType);
    updateOther(getIndexPair<VarType>(), getIndexPair<o>());
}


template <IndexManager::VariableType VarType>
void IndexManager::setVariableType(const IndexList& indices) {
    auto&& pair = getIndexPair<VarType>();
    pair.indices = indices;
    pair.updateInverse(m_total_size);
}

template <IndexManager::VariableType VarType, typename Derived, typename RetType>
RetType IndexManager::expand_vartype(const Eigen::MatrixBase<Derived>& M) const {
    RetType R = RetType::Zero(m_total_size,M.cols());

    auto&& pair = getIndexPair<VarType>();
    assert(M.rows() == pair.size() );
    for(int i = 0; i < pair.size(); ++i) {

        R.row(pair.indices[i]) = M.row(i);
    }

    return R;
}

template <IndexManager::VariableType VarType, typename Derived, typename RetType>
RetType IndexManager::reduce_vartype(const Eigen::MatrixBase<Derived>& M) const {
    RetType R = RetType::Zero(active_size(),M.cols());

    assert(M.rows() == m_total_size);

    auto&& pair = getIndexPair<VarType>();
    for(int i = 0; i < pair.size(); ++i) {

        R.row(i) = M.row(pair.indices[i]);
    }

    return R;
}


template <typename Derived, typename RetType>
RetType IndexManager::expand(const Eigen::MatrixBase<Derived>& M) const {
    return expand_vartype<IndexManager::VariableType::Active,Derived,RetType>(M);
}

template <typename Derived, typename RetType>
RetType IndexManager::reduce(const Eigen::MatrixBase<Derived>& M) const {
    return reduce_vartype<IndexManager::VariableType::Active,Derived,RetType>(M);
}



template <typename T>
Eigen::SparseMatrix<T> IndexManager::reduce(const Eigen::SparseMatrix<T>& M) const {

    constexpr static IndexManager::VariableType VarType = IndexManager::VariableType::Active;
    return reduce_vartype<VarType,T>(M);
}

template <IndexManager::VariableType VarType, typename T>
Eigen::SparseMatrix<T> IndexManager::reduce_vartype(const Eigen::SparseMatrix<T>& M) const {
    return reduceMatrix<VarType,VarType,T>(M);
}

template <IndexManager::VariableType VarType, IndexManager::VariableType VarType2, typename T>
Eigen::SparseMatrix<T> IndexManager::reduceMatrix(const Eigen::SparseMatrix<T>& M) const {
    auto&& pair = getIndexPair<VarType>();
    auto&& pair2 = getIndexPair<VarType2>();


    assert(M.rows() == m_total_size);
    assert(M.cols() == m_total_size);

    Eigen::SparseMatrix<T> A(pair.size(), pair2.size());
    std::vector<Eigen::Triplet<T>> trips;
    for(int i = 0; i < pair2.size(); ++i) {

        int k = pair2.indices[i];

        for(typename Eigen::SparseMatrix<T>::InnerIterator it(M,k); it; ++it) {
            if(pair.inverse[it.row()] != -1) {
                int r = pair.inverse[it.row()];
                int c = pair2.inverse[it.col()];
                trips.emplace_back(r,c,it.value());
            }
        }
    }

    A.setFromTriplets(trips.begin(),trips.end());
    return A;
}

template <>
void IndexManager::setActive(const IndexList& indices);

template <typename Collection>
void IndexManager::setActive(const Collection& indices) {
    IndexList inds;
    std::copy(indices.begin(), indices.end(), std::back_inserter(inds));
    setActive(inds);
}
#endif//ACTIVE_SET_H