OKVIS ROS
 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 12, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_CERES_MARGINALIZATIONERROR_HPP_
40 #define INCLUDE_OKVIS_CERES_MARGINALIZATIONERROR_HPP_
41 
42 #include <vector>
43 #include <deque>
44 #include <memory>
45 #include <Eigen/Core>
47 #include <okvis/Measurements.hpp>
48 #include <okvis/Variables.hpp>
49 #include "ceres/ceres.h"
51 #include <okvis/assert_macros.hpp>
56 #include <okvis/ceres/Map.hpp>
58 
60 namespace okvis {
62 namespace ceres {
63 
64 // not sized, in order to be flexible.
65 class MarginalizationError : public ::ceres::CostFunction, public ErrorInterface
66 {
67  public:
68 
69  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
71 
72 
73  typedef ::ceres::CostFunction base_t;
74 
77  {
78  }
79 
82 
86 
91  Map& map, std::vector< ::ceres::ResidualBlockId > & residualBlockIds);
92 
93  // initialization
96  void setMap(Map& map);
97 
103  bool addResidualBlocks(
104  const std::vector< ::ceres::ResidualBlockId > & residualBlockIds,
105  const std::vector<bool> & keepResidualBlocks = std::vector<bool>());
106 
112  bool addResidualBlock(::ceres::ResidualBlockId residualBlockId,
113  bool keepResidualBlock = false);
114 
117  bool isParameterBlockConnected(uint64_t parameterBlockId);
118 
119  // marginalization
120 
125  bool marginalizeOut(const std::vector<uint64_t> & parameterBlockIds,
126  const std::vector<bool> & keepParameterBlocks =
127  std::vector<bool>());
128 
131  void updateErrorComputation();
132 
136  std::vector<std::shared_ptr<okvis::ceres::ParameterBlock> >& parameterBlockPtrs);
137 
138  // error term and Jacobian implementation (inherited pure virtuals from ::ceres::CostFunction)
146  virtual bool Evaluate(double const* const * parameters, double* residuals,
147  double** jacobians) const;
148 
158  bool EvaluateWithMinimalJacobians(double const* const * parameters,
159  double* residuals, double** jacobians,
160  double** jacobiansMinimal) const;
161 
162  // sizes (inherited pure virtuals from ::okvis::ceres::ErrorInterface)
164  size_t residualDim() const
165  {
166  return base_t::num_residuals();
167  }
168 
170  size_t parameterBlocks() const
171  {
172  return base_t::parameter_block_sizes().size();
173  }
174 
178  size_t parameterBlockDim(size_t parameterBlockId) const
179  {
180  return base_t::parameter_block_sizes().at(parameterBlockId);
181  }
182 
184  virtual std::string typeInfo() const
185  {
186  return "MarginalizationError";
187  }
188 
189 
201  template<typename Derived>
202  static bool pseudoInverseSymm(
203  const Eigen::MatrixBase<Derived>&a,
204  const Eigen::MatrixBase<Derived>&result, double epsilon =
205  std::numeric_limits<typename Derived::Scalar>::epsilon(), int * rank = 0);
206 
218  template<typename Derived>
219  static bool pseudoInverseSymmSqrt(
220  const Eigen::MatrixBase<Derived>&a,
221  const Eigen::MatrixBase<Derived>&result, double epsilon =
222  std::numeric_limits<typename Derived::Scalar>::epsilon(),
223  int* rank = NULL);
224 
236  template<typename Derived, int blockDim>
237  static void blockPinverse(
238  const Eigen::MatrixBase<Derived>& M_in,
239  const Eigen::MatrixBase<Derived>& M_out, double epsilon =
240  std::numeric_limits<typename Derived::Scalar>::epsilon());
241 
242 
255  template<typename Derived, int blockDim>
256  static void blockPinverseSqrt(
257  const Eigen::MatrixBase<Derived>& M_in,
258  const Eigen::MatrixBase<Derived>& M_out, double epsilon =
259  std::numeric_limits<typename Derived::Scalar>::epsilon());
260 
261  protected:
263  ::ceres::ResidualBlockId residualBlockId_;
264 
266  void check();
267 
269  bool computeDeltaChi(Eigen::VectorXd& DeltaChi) const; // use the stored estimates
270 
272  bool computeDeltaChi(double const* const * parameters,
273  Eigen::VectorXd& DeltaChi) const; // use the provided estimates
274 
276  template<typename Derived_A, typename Derived_U, typename Derived_W,
277  typename Derived_V>
278  static void splitSymmetricMatrix(
279  const std::vector<std::pair<int, int> >& marginalizationStartIdxAndLengthPairs,
280  const Eigen::MatrixBase<Derived_A>& A, // input
281  const Eigen::MatrixBase<Derived_U>& U, // output
282  const Eigen::MatrixBase<Derived_W>& W, // output
283  const Eigen::MatrixBase<Derived_V>& V); // output
284 
286  template<typename Derived_b, typename Derived_b_a, typename Derived_b_b>
287  static void splitVector(
288  const std::vector<std::pair<int, int> >& marginalizationStartIdxAndLengthPairs,
289  const Eigen::MatrixBase<Derived_b>& b, // input
290  const Eigen::MatrixBase<Derived_b_a>& b_a, // output
291  const Eigen::MatrixBase<Derived_b_b>& b_b); // output
292 
303  Eigen::MatrixXd H_;
304  Eigen::VectorXd b0_;
305  Eigen::VectorXd e0_;
306  Eigen::MatrixXd J_;
307  Eigen::MatrixXd U_;
308  Eigen::VectorXd S_;
309  Eigen::VectorXd S_sqrt_;
310  Eigen::VectorXd S_pinv_;
311  Eigen::VectorXd S_pinv_sqrt_;
312  Eigen::VectorXd p_;
313  Eigen::VectorXd p_inv_;
314  volatile bool errorComputationValid_;
315 
318  {
319  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
322  size_t orderingIdx;
323  size_t dimension;
326  std::shared_ptr<double> linearizationPoint;
329  : parameterBlockId(0),
330  parameterBlockPtr(std::shared_ptr<ParameterBlock>()),
331  orderingIdx(0),
332  dimension(0),
333  minimalDimension(0),
334  localDimension(0),
335  isLandmark(false)
336  {
337  }
339  std::shared_ptr<ParameterBlock> parameterBlockPtr,
340  size_t orderingIdx, bool isLandmark)
341  : parameterBlockId(parameterBlockId),
342  parameterBlockPtr(parameterBlockPtr),
343  orderingIdx(orderingIdx),
344  isLandmark(isLandmark)
345  {
346  dimension = parameterBlockPtr->dimension();
347  minimalDimension = parameterBlockPtr->minimalDimension();
348  if (parameterBlockPtr->localParameterizationPtr()) {
349  localDimension = parameterBlockPtr->localParameterizationPtr()
350  ->LocalSize();
351  } else {
353  }
354  if (parameterBlockPtr->fixed()) {
355  minimalDimension = 0;
356  localDimension = 0;
357  }
358  linearizationPoint.reset(new double[dimension],
359  std::default_delete<double[]>());
360  memcpy(linearizationPoint.get(), parameterBlockPtr->parameters(),
361  dimension * sizeof(double));
362  }
363 
366  std::shared_ptr<ParameterBlock> parameterBlockPtr)
367  {
368  OKVIS_ASSERT_TRUE_DBG(Exception,dimension==parameterBlockPtr->dimension(),"not initialised.")
369  memcpy(linearizationPoint.get(), parameterBlockPtr->parameters(),
370  dimension * sizeof(double));
371  }
372  };
373 
374  std::vector<ParameterBlockInfo> parameterBlockInfos_;
375  std::map<uint64_t, size_t> parameterBlockId2parameterBlockInfoIdx_;
376  size_t denseIndices_;
377 
379 
380 };
381 
382 } // namespace ceres
383 } // namespace okvis
384 
386 
387 #endif /* INCLUDE_OKVIS_CERES_MARGINALIZATIONERROR_HPP_ */
size_t residualDim() const
Residual dimension.
Definition: MarginalizationError.hpp:164
Eigen::MatrixXd U_
H_ = _U*_S*_U^T lhs Eigen decomposition.
Definition: MarginalizationError.hpp:307
Header file for the HomogeneousPointLocalParameterization class.
Header file for the ErrorInterface class. A simple interface class that other error classes should in...
virtual std::string typeInfo() const
Return parameter block type as string.
Definition: MarginalizationError.hpp:184
Eigen::VectorXd S_
singular values
Definition: MarginalizationError.hpp:308
Eigen::VectorXd p_inv_
Definition: MarginalizationError.hpp:313
Eigen::VectorXd b0_
rhs constant part
Definition: MarginalizationError.hpp:304
Eigen::VectorXd S_pinv_
pseudo inverse of _S
Definition: MarginalizationError.hpp:310
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
uint64_t parameterBlockId
Definition: MarginalizationError.hpp:320
Header file for the PoseParameterBlock class.
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
Header file for the Map class. This essentially encapsulates the ceres::Problem.
volatile bool errorComputationValid_
adding residual blocks will invalidate this. before optimizing, call updateErrorComputation() ...
Definition: MarginalizationError.hpp:314
Eigen::MatrixXd H_
lhs - Hessian
Definition: MarginalizationError.hpp:303
bool isLandmark
Definition: MarginalizationError.hpp:327
Book-keeping of the ordering.
Definition: MarginalizationError.hpp:317
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
bool addResidualBlocks(const std::vector< ::ceres::ResidualBlockId > &residualBlockIds, const std::vector< bool > &keepResidualBlocks=std::vector< bool >())
Add some residuals to this marginalisation error. This means, they will get linearised.
Definition: MarginalizationError.cpp:111
bool addResidualBlock(::ceres::ResidualBlockId residualBlockId, bool keepResidualBlock=false)
Add one residual to this marginalisation error. This means, it will get linearised.
Definition: MarginalizationError.cpp:127
This file contains the templated measurement structs, structs encapsulating Sensor data and related t...
Simple interface class the errors implemented here should inherit from.
Definition: ErrorInterface.hpp:53
void getParameterBlockPtrs(std::vector< std::shared_ptr< okvis::ceres::ParameterBlock > > &parameterBlockPtrs)
Call this in order to (re-)add this error term after whenever it had been modified.
Definition: MarginalizationError.cpp:497
Header file for the HomogeneousPointParameterBlock class.
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
size_t dimension
Definition: MarginalizationError.hpp:323
std::vector< ParameterBlockInfo > parameterBlockInfos_
Book keeper.
Definition: MarginalizationError.hpp:374
Eigen::MatrixXd J_
Jacobian such that _J^T * J == _H.
Definition: MarginalizationError.hpp:306
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
This evaluates the error term and additionally computes the Jacobians.
Definition: MarginalizationError.cpp:885
This file contains some useful assert macros.
Header file for the SpeedAndBiasParameterBlock class.
bool marginalizeOut(const std::vector< uint64_t > &parameterBlockIds, const std::vector< bool > &keepParameterBlocks=std::vector< bool >())
Marginalise out a set of parameter blocks.
Definition: MarginalizationError.cpp:507
void resetLinearizationPoint(std::shared_ptr< ParameterBlock > parameterBlockPtr)
Reset the linearisation point. Use with caution.
Definition: MarginalizationError.hpp:365
std::map< uint64_t, size_t > parameterBlockId2parameterBlockInfoIdx_
Maps parameter block Ids to index in _parameterBlockInfos.
Definition: MarginalizationError.hpp:375
Eigen::VectorXd p_
Definition: MarginalizationError.hpp:312
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef::ceres::CostFunction base_t
The base class type.
Definition: MarginalizationError.hpp:73
bool computeDeltaChi(Eigen::VectorXd &DeltaChi) const
Computes the linearized deviation from the references (linearization points)
Definition: MarginalizationError.cpp:849
This file contains some typedefs related to state variables.
void setMap(Map &map)
Set the underlying okvis::ceres::Map.
Definition: MarginalizationError.cpp:105
std::shared_ptr< double > linearizationPoint
Definition: MarginalizationError.hpp:326
ParameterBlockInfo(uint64_t parameterBlockId, std::shared_ptr< ParameterBlock > parameterBlockPtr, size_t orderingIdx, bool isLandmark)
Definition: MarginalizationError.hpp:338
void check()
Checks the internal datastructure (debug)
Definition: MarginalizationError.cpp:451
Eigen::VectorXd S_sqrt_
cwise sqrt of _S, i.e. _S_sqrt*_S_sqrt=_S; _J=_U^T*_S_sqrt
Definition: MarginalizationError.hpp:309
::ceres::ResidualBlockId residualBlockId_
The residual block id of this.
Definition: MarginalizationError.hpp:263
void updateErrorComputation()
This must be called before optimization after adding residual blocks and/or marginalizing, since it performs all the lhs and rhs computations on from a given _H and _b.
Definition: MarginalizationError.cpp:806
std::shared_ptr< ParameterBlock > parameterBlockPtr
Definition: MarginalizationError.hpp:321
size_t denseIndices_
Keep track of the size of the dense part of the equation system.
Definition: MarginalizationError.hpp:376
Header file for the Transformation class.
bool isParameterBlockConnected(uint64_t parameterBlockId)
Info: is this parameter block connected to this marginalization error?
Definition: MarginalizationError.cpp:438
size_t minimalDimension
Definition: MarginalizationError.hpp:324
Eigen::VectorXd S_pinv_sqrt_
cwise sqrt of _S_pinv, i.e. pinv(J^T)=_U^T*_S_pinv_sqrt
Definition: MarginalizationError.hpp:311
Definition: MarginalizationError.hpp:65
size_t localDimension
Definition: MarginalizationError.hpp:325
MarginalizationError()
Default constructor. Initialises a new okvis::ceres::Map.
Definition: MarginalizationError.cpp:76
The Map class. This keeps track of how parameter blocks are connected to residual blocks...
Definition: Map.hpp:63
Header implementation file for the MarginalizationError class.
bool EvaluateWithMinimalJacobians(double const *const *parameters, double *residuals, double **jacobians, double **jacobiansMinimal) const
This evaluates the error term and additionally computes the Jacobians in the minimal internal represe...
Definition: MarginalizationError.cpp:893
size_t parameterBlockDim(size_t parameterBlockId) const
Dimension of an individual parameter block.
Definition: MarginalizationError.hpp:178
#define OKVIS_ASSERT_TRUE_DBG(exceptionType, condition, message)
Definition: assert_macros.hpp:211
Map * mapPtr_
The underlying map.
Definition: MarginalizationError.hpp:262
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
size_t orderingIdx
Definition: MarginalizationError.hpp:322
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
size_t parameterBlocks() const
Number of parameter blocks.
Definition: MarginalizationError.hpp:170
Base class providing the interface for parameter blocks.
Definition: ParameterBlock.hpp:53
Eigen::VectorXd e0_
_e0 := pinv(J^T) * _b0
Definition: MarginalizationError.hpp:305