marginal_covariance_cholesky.cpp
Go to the documentation of this file.
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 
28 
29 #include <algorithm>
30 #include <cassert>
31 using namespace std;
32 
33 namespace g2o {
34 
35 struct MatrixElem
36 {
37  int r, c;
38  MatrixElem(int r_, int c_) : r(r_), c(c_) {}
39  bool operator<(const MatrixElem& other) const
40  {
41  return c > other.c || (c == other.c && r > other.r);
42  }
43 };
44 
45 MarginalCovarianceCholesky::MarginalCovarianceCholesky() :
46  _n(0), _Ap(0), _Ai(0), _Ax(0), _perm(0)
47 {
48 }
49 
51 {
52 }
53 
54 void MarginalCovarianceCholesky::setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv)
55 {
56  _n = n;
57  _Ap = Lp;
58  _Ai = Li;
59  _Ax = Lx;
60  _perm = permInv;
61 
62  // pre-compute reciprocal values of the diagonal of L
63  _diag.resize(n);
64  for (int r = 0; r < n; ++r) {
65  const int& sc = _Ap[r]; // L is lower triangular, thus the first elem in the column is the diagonal entry
66  assert(r == _Ai[sc] && "Error in CCS storage of L");
67  _diag[r] = 1.0 / _Ax[sc];
68  }
69 }
70 
72 {
73  assert(r <= c);
74  int idx = computeIndex(r, c);
75 
76  LookupMap::const_iterator foundIt = _map.find(idx);
77  if (foundIt != _map.end()) {
78  return foundIt->second;
79  }
80 
81  // compute the summation over column r
82  double s = 0.;
83  const int& sc = _Ap[r];
84  const int& ec = _Ap[r+1];
85  for (int j = sc+1; j < ec; ++j) { // sum over row r while skipping the element on the diagonal
86  const int& rr = _Ai[j];
87  double val = rr < c ? computeEntry(rr, c) : computeEntry(c, rr);
88  s += val * _Ax[j];
89  }
90 
91  double result;
92  if (r == c) {
93  const double& diagElem = _diag[r];
94  result = diagElem * (diagElem - s);
95  } else {
96  result = -s * _diag[r];
97  }
98  _map[idx] = result;
99  return result;
100 }
101 
102 void MarginalCovarianceCholesky::computeCovariance(double** covBlocks, const std::vector<int>& blockIndices)
103 {
104  _map.clear();
105  int base = 0;
106  vector<MatrixElem> elemsToCompute;
107  for (size_t i = 0; i < blockIndices.size(); ++i) {
108  int nbase = blockIndices[i];
109  int vdim = nbase - base;
110  for (int rr = 0; rr < vdim; ++rr)
111  for (int cc = rr; cc < vdim; ++cc) {
112  int r = _perm ? _perm[rr + base] : rr + base; // apply permutation
113  int c = _perm ? _perm[cc + base] : cc + base;
114  if (r > c) // make sure it's still upper triangular after applying the permutation
115  swap(r, c);
116  elemsToCompute.push_back(MatrixElem(r, c));
117  }
118  base = nbase;
119  }
120 
121  // sort the elems to reduce the recursive calls
122  sort(elemsToCompute.begin(), elemsToCompute.end());
123 
124  // compute the inverse elements we need
125  for (size_t i = 0; i < elemsToCompute.size(); ++i) {
126  const MatrixElem& me = elemsToCompute[i];
127  computeEntry(me.r, me.c);
128  }
129 
130  // set the marginal covariance for the vertices, by writing to the blocks memory
131  base = 0;
132  for (size_t i = 0; i < blockIndices.size(); ++i) {
133  int nbase = blockIndices[i];
134  int vdim = nbase - base;
135  double* cov = covBlocks[i];
136  for (int rr = 0; rr < vdim; ++rr)
137  for (int cc = rr; cc < vdim; ++cc) {
138  int r = _perm ? _perm[rr + base] : rr + base; // apply permutation
139  int c = _perm ? _perm[cc + base] : cc + base;
140  if (r > c) // upper triangle
141  swap(r, c);
142  int idx = computeIndex(r, c);
143  LookupMap::const_iterator foundIt = _map.find(idx);
144  assert(foundIt != _map.end());
145  cov[rr*vdim + cc] = foundIt->second;
146  if (rr != cc)
147  cov[cc*vdim + rr] = foundIt->second;
148  }
149  base = nbase;
150  }
151 }
152 
153 
154 void MarginalCovarianceCholesky::computeCovariance(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<int>& rowBlockIndices, const std::vector< std::pair<int, int> >& blockIndices)
155 {
156  // allocate the sparse
157  spinv = SparseBlockMatrix<MatrixXd>(&rowBlockIndices[0],
158  &rowBlockIndices[0],
159  rowBlockIndices.size(),
160  rowBlockIndices.size(), true);
161  _map.clear();
162  vector<MatrixElem> elemsToCompute;
163  for (size_t i = 0; i < blockIndices.size(); ++i) {
164  int blockRow=blockIndices[i].first;
165  int blockCol=blockIndices[i].second;
166  assert(blockRow>=0);
167  assert(blockRow < (int)rowBlockIndices.size());
168  assert(blockCol>=0);
169  assert(blockCol < (int)rowBlockIndices.size());
170 
171  int rowBase=spinv.rowBaseOfBlock(blockRow);
172  int colBase=spinv.colBaseOfBlock(blockCol);
173 
174  MatrixXd *block=spinv.block(blockRow, blockCol, true);
175  assert(block);
176  for (int iRow=0; iRow<block->rows(); ++iRow)
177  for (int iCol=0; iCol<block->cols(); ++iCol){
178  int rr=rowBase+iRow;
179  int cc=colBase+iCol;
180  int r = _perm ? _perm[rr] : rr; // apply permutation
181  int c = _perm ? _perm[cc] : cc;
182  if (r > c)
183  swap(r, c);
184  elemsToCompute.push_back(MatrixElem(r, c));
185  }
186  }
187 
188  // sort the elems to reduce the number of recursive calls
189  sort(elemsToCompute.begin(), elemsToCompute.end());
190 
191  // compute the inverse elements we need
192  for (size_t i = 0; i < elemsToCompute.size(); ++i) {
193  const MatrixElem& me = elemsToCompute[i];
194  computeEntry(me.r, me.c);
195  }
196 
197  // set the marginal covariance
198  for (size_t i = 0; i < blockIndices.size(); ++i) {
199  int blockRow=blockIndices[i].first;
200  int blockCol=blockIndices[i].second;
201  int rowBase=spinv.rowBaseOfBlock(blockRow);
202  int colBase=spinv.colBaseOfBlock(blockCol);
203 
204  MatrixXd *block=spinv.block(blockRow, blockCol);
205  assert(block);
206  for (int iRow=0; iRow<block->rows(); ++iRow)
207  for (int iCol=0; iCol<block->cols(); ++iCol){
208  int rr=rowBase+iRow;
209  int cc=colBase+iCol;
210  int r = _perm ? _perm[rr] : rr; // apply permutation
211  int c = _perm ? _perm[cc] : cc;
212  if (r > c)
213  swap(r, c);
214  int idx = computeIndex(r, c);
215  LookupMap::const_iterator foundIt = _map.find(idx);
216  assert(foundIt != _map.end());
217  (*block)(iRow, iCol) = foundIt->second;
218  }
219  }
220 }
221 
222 } // end namespace
LookupMap _map
hash look up table for the already computed entries
void setCholeskyFactor(int n, int *Lp, int *Li, double *Lx, int *permInv)
XmlRpcServer s
SparseMatrixBlock * block(int r, int c, bool alloc=false)
returns the block at location r,c. if alloc=true he block is created if it does not exist ...
int * _Ai
row indices of the CCS storage
int * _Ap
column pointer of the CCS storage
void computeCovariance(double **covBlocks, const std::vector< int > &blockIndices)
double * _Ax
values of the cholesky factor
int colBaseOfBlock(int c) const
where does the col at block-col r starts?
int * _perm
permutation of the cholesky factor. Variable re-ordering for better fill-in
std::vector< double > _diag
cache 1 / H_ii to avoid recalculations
bool operator<(const MatrixElem &other) const
int rowBaseOfBlock(int r) const
where does the row at block-row r starts?
Sparse matrix which uses blocks.
int computeIndex(int r, int c) const
compute the index used for hashing


orb_slam2_with_maps_odom
Author(s): teng zhang
autogenerated on Fri Sep 25 2020 03:24:47