1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 // All rights reserved. 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are 7 // met: 8 // 9 // * Redistributions of source code must retain the above copyright notice, 10 // this list of conditions and the following disclaimer. 11 // * Redistributions in binary form must reproduce the above copyright 12 // notice, this list of conditions and the following disclaimer in the 13 // documentation and/or other materials provided with the distribution. 14 // 15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 27 #ifndef G2O_MARGINAL_COVARIANCE_CHOLESKY_H 28 #define G2O_MARGINAL_COVARIANCE_CHOLESKY_H 29 30 #include "sparse_block_matrix.h" 31 32 #include <vector> 33 34 #include <unordered_map> 35 36 #include "g2o_core_api.h" 37 38 namespace g2o { 39 40 /** 41 * \brief computing the marginal covariance given a cholesky factor (lower triangle of the factor) 42 */ 43 class G2O_CORE_API MarginalCovarianceCholesky { 44 protected: 45 /** 46 * hash struct for storing the matrix elements needed to compute the covariance 47 */ 48 typedef std::unordered_map<int, number_t> LookupMap; 49 50 public: 51 MarginalCovarianceCholesky(); 52 ~MarginalCovarianceCholesky(); 53 54 /** 55 * compute the marginal cov for the given block indices, write the result to the covBlocks memory (which has to 56 * be provided by the caller). 57 */ 58 void computeCovariance(number_t** covBlocks, const std::vector<int>& blockIndices); 59 60 61 /** 62 * compute the marginal cov for the given block indices, write the result in spinv). 63 */ 64 void computeCovariance(SparseBlockMatrix<MatrixX>& spinv, const std::vector<int>& rowBlockIndices, const std::vector< std::pair<int, int> >& blockIndices); 65 66 67 /** 68 * set the CCS representation of the cholesky factor along with the inverse permutation used to reduce the fill-in. 69 * permInv might be 0, will then not permute the entries. 70 * 71 * The pointers provided by the user need to be still valid when calling computeCovariance(). The pointers 72 * are owned by the caller, MarginalCovarianceCholesky does not free the pointers. 73 */ 74 void setCholeskyFactor(int n, int* Lp, int* Li, number_t* Lx, int* permInv); 75 76 protected: 77 // information about the cholesky factor (lower triangle) 78 int _n; ///< L is an n X n matrix 79 int* _Ap; ///< column pointer of the CCS storage 80 int* _Ai; ///< row indices of the CCS storage 81 number_t* _Ax; ///< values of the cholesky factor 82 int* _perm; ///< permutation of the cholesky factor. Variable re-ordering for better fill-in 83 84 LookupMap _map; ///< hash look up table for the already computed entries 85 std::vector<number_t> _diag; ///< cache 1 / H_ii to avoid recalculations 86 87 //! compute the index used for hashing computeIndex(int r,int c)88 int computeIndex(int r, int c) const { /*assert(r <= c);*/ return r*_n + c;} 89 /** 90 * compute one entry in the covariance, r and c are values after applying the permutation, and upper triangular. 91 * May issue recursive calls to itself to compute the missing values. 92 */ 93 number_t computeEntry(int r, int c); 94 }; 95 96 } 97 98 #endif 99