27 #ifndef G2O_LINEAR_SOLVER_EIGEN_H 28 #define G2O_LINEAR_SOLVER_EIGEN_H 30 #include <Eigen/Sparse> 31 #include <Eigen/SparseCholesky> 33 #include "../core/linear_solver.h" 34 #include "../core/batch_stats.h" 35 #include "../stuff/timeutil.h" 37 #include "../core/eigen_types.h" 50 template <
typename MatrixType>
64 using Eigen::SimplicialLDLT< SparseMatrix, Eigen::Upper>::analyzePattern_preordered;
69 m_P = permutation.inverse();
71 SparseMatrix ap(size, size);
72 ap.selfadjointView<Eigen::Upper>() = a.selfadjointView<UpLo>().twistedBy(m_P);
73 analyzePattern_preordered(ap,
true);
105 if (
_cholesky.info() != Eigen::Success) {
107 std::cerr <<
"Cholesky failure, writing debug.txt (Hessian loadable by Octave)" << std::endl;
150 if (! _blockOrdering) {
151 _cholesky.analyzePattern(_sparseMatrix);
156 Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic> blockP;
159 std::vector<Triplet> triplets;
160 for (
size_t c = 0; c < A.
blockCols().size(); ++c){
163 const int& r = it->first;
164 if (r > static_cast<int>(c))
166 triplets.push_back(
Triplet(r, c, 0.));
173 auxBlockMatrix.setFromTriplets(triplets.begin(), triplets.end());
174 typename CholeskyDecomposition::CholMatrixType C;
175 C = auxBlockMatrix.selfadjointView<Eigen::Upper>();
176 Eigen::internal::minimum_degree_ordering(C, blockP);
180 assert(rows == A.
cols() &&
"Matrix A is not square");
183 PermutationMatrix scalarP;
184 scalarP.resize(rows);
186 for (
int i = 0; i < blockP.size(); ++i) {
187 const int& p = blockP.indices()(i);
190 for (
int j = 0; j < nCols; ++j)
191 scalarP.indices()(scalarIdx++) = base++;
193 assert(scalarIdx == rows &&
"did not completely fill the permutation matrix");
206 A.
fillCCS(_sparseMatrix.valuePtr(),
true);
210 std::vector<Triplet> triplets;
212 for (
size_t c = 0; c < A.
blockCols().size(); ++c) {
217 const MatrixType& m = *(it->second);
218 for (
int cc = 0; cc < m.cols(); ++cc) {
219 int aux_c = colBaseOfBlock + cc;
220 for (
int rr = 0; rr < m.rows(); ++rr) {
221 int aux_r = rowBaseOfBlock + rr;
224 triplets.push_back(
Triplet(aux_r, aux_c, m(rr, cc)));
229 _sparseMatrix.setFromTriplets(triplets.begin(), triplets.end());
virtual ~LinearSolverEigen()
int colBaseOfBlock(int c) const
where does the col at block-col r starts?
statistics about the optimization
virtual bool writeDebug() const
write a debug dump of the system matrix if it is not SPD in solve
int cols() const
columns of the matrix
Eigen::SparseMatrix< double, Eigen::ColMajor > SparseMatrix
int colsOfBlock(int c) const
how many cols does the block at block-col c has?
Sub-classing Eigen's SimplicialLDLT to perform ordering with a given ordering.
bool writeOctave(const char *filename, bool upperTriangle=true) const
int fillCCS(int *Cp, int *Ci, double *Cx, bool upperTriangle=false) const
void computeSymbolicDecomposition(const SparseBlockMatrix< MatrixType > &A)
Eigen::Triplet< double > Triplet
size_t nonZeros() const
number of non-zero elements
bool blockOrdering() const
do the AMD ordering on the blocks or on the scalar matrix
geometry_msgs::TransformStamped t
Eigen::PermutationMatrix< Eigen::Dynamic, Eigen::Dynamic > PermutationMatrix
size_t choleskyNNZ
number of non-zeros in the cholesky factor
linear solver which uses the sparse Cholesky solver from Eigen
static G2OBatchStatistics * globalStats()
double timeNumericDecomposition
numeric decomposition (0 if not done)
double get_monotonic_time()
std::map< int, SparseMatrixBlock * > IntBlockMap
void fillSparseMatrix(const SparseBlockMatrix< MatrixType > &A, bool onlyValues)
const std::vector< IntBlockMap > & blockCols() const
the block matrices per block-column
bool solve(const SparseBlockMatrix< MatrixType > &A, double *x, double *b)
double timeSymbolicDecomposition
symbolic decomposition (0 if not done)
void setBlockOrdering(bool blockOrdering)
int rowBaseOfBlock(int r) const
where does the row at block-row r starts?
CholeskyDecomposition _cholesky
Sparse matrix which uses blocks.
SparseMatrix _sparseMatrix
int rows() const
rows of the matrix
void analyzePatternWithPermutation(SparseMatrix &a, const PermutationMatrix &permutation)
virtual void setWriteDebug(bool b)