OKVIS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
MarginalizationError.hpp
Go to the documentation of this file.
1 /*********************************************************************************
2  * OKVIS - Open Keyframe-based Visual-Inertial SLAM
3  * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
14  * its contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Created on: Sep 11, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 namespace okvis {
42 namespace ceres {
43 
44 // Split for Schur complement op.
45 template<typename Derived_A, typename Derived_U, typename Derived_W,
46  typename Derived_V>
48  const std::vector<std::pair<int, int> >& marginalizationStartIdxAndLengthPairs,
49  const Eigen::MatrixBase<Derived_A>& A, // input
50  const Eigen::MatrixBase<Derived_U>& U, // output
51  const Eigen::MatrixBase<Derived_W>& W, // output
52  const Eigen::MatrixBase<Derived_V>& V) { // output
53 
54  // sanity check
55  const int size = A.cols();
56  OKVIS_ASSERT_TRUE_DBG(Exception, size == A.rows(), "matrix not symmetric");
57  OKVIS_ASSERT_TRUE_DBG(Exception, V.cols() == V.rows(),
58  "matrix not symmetric");
59  OKVIS_ASSERT_TRUE_DBG(Exception, V.cols() == W.cols(),
60  "matrix not symmetric");
61  OKVIS_ASSERT_TRUE_DBG(Exception, U.rows() == W.rows(),
62  "matrix not symmetric");
64  Exception,
65  V.rows() + U.rows() == size,
66  "matrices supplied to be split into do not form exact upper triangular blocks of the original one");
67  OKVIS_ASSERT_TRUE_DBG(Exception, U.cols() == U.rows(),
68  "matrix not symmetric");
69 
70  std::vector<std::pair<int, int> > marginalizationStartIdxAndLengthPairs2 =
71  marginalizationStartIdxAndLengthPairs;
72  marginalizationStartIdxAndLengthPairs2.push_back(
73  std::pair<int, int>(size, 0));
74 
75  const size_t length = marginalizationStartIdxAndLengthPairs2.size();
76 
77  int lastIdx_row = 0;
78  int start_a_i = 0;
79  int start_b_i = 0;
80  for (size_t i = 0; i < length; ++i) {
81  int lastIdx_col = 0;
82  int start_a_j = 0;
83  int start_b_j = 0;
84  int thisIdx_row = marginalizationStartIdxAndLengthPairs2[i].first;
85  const int size_b_i = marginalizationStartIdxAndLengthPairs2[i].second; // kept
86  const int size_a_i = thisIdx_row - lastIdx_row; // kept
87  for (size_t j = 0; j < length; ++j) {
88  int thisIdx_col = marginalizationStartIdxAndLengthPairs2[j].first;
89  const int size_a_j = thisIdx_col - lastIdx_col; // kept
90  const int size_b_j = marginalizationStartIdxAndLengthPairs2[j].second; // kept
91 
92  // the kept part - only access and copy if non-zero
93  if (size_a_j > 0 && size_a_i > 0) {
94  const_cast<Eigen::MatrixBase<Derived_U>&>(U).block(start_a_i, start_a_j,
95  size_a_i, size_a_j) =
96  A.block(lastIdx_row, lastIdx_col, size_a_i, size_a_j);
97  }
98 
99  // now the mixed part
100  if (size_b_j > 0 && size_a_i > 0) {
101  const_cast<Eigen::MatrixBase<Derived_W>&>(W).block(start_a_i, start_b_j,
102  size_a_i, size_b_j) =
103  A.block(lastIdx_row, thisIdx_col, size_a_i, size_b_j);
104  }
105 
106  // and finally the marginalized part
107  if (size_b_j > 0 && size_b_i > 0) {
108  const_cast<Eigen::MatrixBase<Derived_V>&>(V).block(start_b_i, start_b_j,
109  size_b_i, size_b_j) =
110  A.block(thisIdx_row, thisIdx_col, size_b_i, size_b_j);
111  }
112 
113  lastIdx_col = thisIdx_col + size_b_j; // remember
114  start_a_j += size_a_j;
115  start_b_j += size_b_j;
116  }
117  lastIdx_row = thisIdx_row + size_b_i; // remember
118  start_a_i += size_a_i;
119  start_b_i += size_b_i;
120  }
121 }
122 
123 // Split for Schur complement op.
124 template<typename Derived_b, typename Derived_b_a, typename Derived_b_b>
126  const std::vector<std::pair<int, int> >& marginalizationStartIdxAndLengthPairs,
127  const Eigen::MatrixBase<Derived_b>& b, // input
128  const Eigen::MatrixBase<Derived_b_a>& b_a, // output
129  const Eigen::MatrixBase<Derived_b_b>& b_b) { // output
130 
131  const int size = b.rows();
132  // sanity check
133  OKVIS_ASSERT_TRUE_DBG(Exception, b.cols() == 1, "supplied vector not x-by-1");
134  OKVIS_ASSERT_TRUE_DBG(Exception, b_a.cols() == 1,
135  "supplied vector not x-by-1");
136  OKVIS_ASSERT_TRUE_DBG(Exception, b_b.cols() == 1,
137  "supplied vector not x-by-1");
139  Exception,
140  b_a.rows() + b_b.rows() == size,
141  "vector supplied to be split into cannot be concatenated to the original one");
142 
143  std::vector<std::pair<int, int> > marginalizationStartIdxAndLengthPairs2 =
144  marginalizationStartIdxAndLengthPairs;
145  marginalizationStartIdxAndLengthPairs2.push_back(
146  std::pair<int, int>(size, 0));
147 
148  const size_t length = marginalizationStartIdxAndLengthPairs2.size();
149 
150  int lastIdx_row = 0;
151  int start_a_i = 0;
152  int start_b_i = 0;
153  for (size_t i = 0; i < length; ++i) {
154  int thisIdx_row = marginalizationStartIdxAndLengthPairs2[i].first;
155  const int size_b_i = marginalizationStartIdxAndLengthPairs2[i].second; // kept
156  const int size_a_i = thisIdx_row - lastIdx_row; // kept
157 
158  // the kept part - only access and copy if non-zero
159  if (size_a_i > 0) {
160  const_cast<Eigen::MatrixBase<Derived_b_a>&>(b_a).segment(start_a_i,
161  size_a_i) = b
162  .segment(lastIdx_row, size_a_i);
163  }
164 
165  // and finally the marginalized part
166  if (size_b_i > 0) {
167  const_cast<Eigen::MatrixBase<Derived_b_b>&>(b_b).segment(start_b_i,
168  size_b_i) = b
169  .segment(thisIdx_row, size_b_i);
170  }
171 
172  lastIdx_row = thisIdx_row + size_b_i; // remember
173  start_a_i += size_a_i;
174  start_b_i += size_b_i;
175  }
176 }
177 
178 // Pseudo inversion of a symmetric matrix.
179 // attention: this uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
180 // (negative Eigenvalues are set to zero)
181 template<typename Derived>
183  const Eigen::MatrixBase<Derived>&a, const Eigen::MatrixBase<Derived>&result,
184  double epsilon, int * rank) {
185 
186  OKVIS_ASSERT_TRUE_DBG(Exception, a.rows() == a.cols(),
187  "matrix supplied is not quadratic");
188 
189  Eigen::SelfAdjointEigenSolver<Derived> saes(a);
190 
191  typename Derived::Scalar tolerance = epsilon * a.cols()
192  * saes.eigenvalues().array().maxCoeff();
193 
194  const_cast<Eigen::MatrixBase<Derived>&>(result) = (saes.eigenvectors())
195  * Eigen::VectorXd(
196  (saes.eigenvalues().array() > tolerance).select(
197  saes.eigenvalues().array().inverse(), 0)).asDiagonal()
198  * (saes.eigenvectors().transpose());
199 
200  if (rank) {
201  *rank = 0;
202  for (int i = 0; i < a.rows(); ++i) {
203  if (saes.eigenvalues()[i] > tolerance)
204  (*rank)++;
205  }
206  }
207 
208  return true;
209 }
210 
211 // Pseudo inversion and square root (Cholesky decomposition) of a symmetric matrix.
212 // attention: this uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
213 // (negative Eigenvalues are set to zero)
214 template<typename Derived>
216  const Eigen::MatrixBase<Derived>&a, const Eigen::MatrixBase<Derived>&result,
217  double epsilon, int * rank) {
218 
219  OKVIS_ASSERT_TRUE_DBG(Exception, a.rows() == a.cols(),
220  "matrix supplied is not quadratic");
221 
222  Eigen::SelfAdjointEigenSolver<Derived> saes(a);
223 
224  typename Derived::Scalar tolerance = epsilon * a.cols()
225  * saes.eigenvalues().array().maxCoeff();
226 
227  const_cast<Eigen::MatrixBase<Derived>&>(result) = (saes.eigenvectors())
228  * Eigen::VectorXd(
229  Eigen::VectorXd(
230  (saes.eigenvalues().array() > tolerance).select(
231  saes.eigenvalues().array().inverse(), 0)).array().sqrt())
232  .asDiagonal();
233 
234  if (rank) {
235  *rank = 0;
236  for (int i = 0; i < a.rows(); ++i) {
237  if (saes.eigenvalues()[i] > tolerance)
238  (*rank)++;
239  }
240  }
241 
242  return true;
243 }
244 
245 // Block-wise pseudo inversion of a symmetric matrix with non-zero diagonal blocks.
246 // attention: this uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
247 // (negative Eigenvalues are set to zero)
248 template<typename Derived, int blockDim>
250  const Eigen::MatrixBase<Derived>& M_in,
251  const Eigen::MatrixBase<Derived>& M_out, double epsilon) {
252 
253  OKVIS_ASSERT_TRUE_DBG(Exception, M_in.rows() == M_in.cols(),
254  "matrix supplied is not quadratic");
255 
256  const_cast<Eigen::MatrixBase<Derived>&>(M_out).resize(M_in.rows(),
257  M_in.rows());
258  const_cast<Eigen::MatrixBase<Derived>&>(M_out).setZero();
259  for (int i = 0; i < M_in.cols(); i += blockDim) {
260  Eigen::Matrix<double, blockDim, blockDim> inv;
261  const Eigen::Matrix<double, blockDim, blockDim> in = M_in
262  .template block<blockDim, blockDim>(i, i);
263  //const Eigen::Matrix<double,blockDim,blockDim> in1=0.5*(in+in.transpose());
264  pseudoInverseSymm(in, inv, epsilon);
265  const_cast<Eigen::MatrixBase<Derived>&>(M_out)
266  .template block<blockDim, blockDim>(i, i) = inv;
267  }
268 }
269 
270 // Block-wise pseudo inversion and square root (Cholesky decomposition)
271 // of a symmetric matrix with non-zero diagonal blocks.
272 // attention: this uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
273 // (negative Eigenvalues are set to zero)
274 template<typename Derived, int blockDim>
276  const Eigen::MatrixBase<Derived>& M_in,
277  const Eigen::MatrixBase<Derived>& M_out, double epsilon) {
278 
279  OKVIS_ASSERT_TRUE_DBG(Exception, M_in.rows() == M_in.cols(),
280  "matrix supplied is not quadratic");
281 
282  const_cast<Eigen::MatrixBase<Derived>&>(M_out).resize(M_in.rows(),
283  M_in.rows());
284  const_cast<Eigen::MatrixBase<Derived>&>(M_out).setZero();
285  for (int i = 0; i < M_in.cols(); i += blockDim) {
286  Eigen::Matrix<double, blockDim, blockDim> inv;
287  const Eigen::Matrix<double, blockDim, blockDim> in = M_in
288  .template block<blockDim, blockDim>(i, i);
289  //const Eigen::Matrix<double,blockDim,blockDim> in1=0.5*(in+in.transpose());
290  pseudoInverseSymmSqrt(in, inv, epsilon);
291  const_cast<Eigen::MatrixBase<Derived>&>(M_out)
292  .template block<blockDim, blockDim>(i, i) = inv;
293  }
294 }
295 
296 }
297 }
static void splitSymmetricMatrix(const std::vector< std::pair< int, int > > &marginalizationStartIdxAndLengthPairs, const Eigen::MatrixBase< Derived_A > &A, const Eigen::MatrixBase< Derived_U > &U, const Eigen::MatrixBase< Derived_W > &W, const Eigen::MatrixBase< Derived_V > &V)
Split for Schur complement op.
Definition: MarginalizationError.hpp:47
static void splitVector(const std::vector< std::pair< int, int > > &marginalizationStartIdxAndLengthPairs, const Eigen::MatrixBase< Derived_b > &b, const Eigen::MatrixBase< Derived_b_a > &b_a, const Eigen::MatrixBase< Derived_b_b > &b_b)
Split for Schur complement op.
Definition: MarginalizationError.hpp:125
static bool pseudoInverseSymmSqrt(const Eigen::MatrixBase< Derived > &a, const Eigen::MatrixBase< Derived > &result, double epsilon=std::numeric_limits< typename Derived::Scalar >::epsilon(), int *rank=NULL)
Pseudo inversion and square root (Cholesky decomposition) of a symmetric matrix.
Definition: MarginalizationError.hpp:215
static void blockPinverseSqrt(const Eigen::MatrixBase< Derived > &M_in, const Eigen::MatrixBase< Derived > &M_out, double epsilon=std::numeric_limits< typename Derived::Scalar >::epsilon())
Block-wise pseudo inversion and square root (Cholesky decomposition) of a symmetric matrix with non-z...
Definition: MarginalizationError.hpp:275
#define OKVIS_ASSERT_TRUE_DBG(exceptionType, condition, message)
Definition: assert_macros.hpp:211
static bool pseudoInverseSymm(const Eigen::MatrixBase< Derived > &a, const Eigen::MatrixBase< Derived > &result, double epsilon=std::numeric_limits< typename Derived::Scalar >::epsilon(), int *rank=0)
Pseudo inversion of a symmetric matrix.
Definition: MarginalizationError.hpp:182
static void blockPinverse(const Eigen::MatrixBase< Derived > &M_in, const Eigen::MatrixBase< Derived > &M_out, double epsilon=std::numeric_limits< typename Derived::Scalar >::epsilon())
Block-wise pseudo inversion of a symmetric matrix with non-zero diagonal blocks.
Definition: MarginalizationError.hpp:249