MarginalizationError.hpp

Go to the documentation of this file.

/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 * 
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Sep 12, 2013
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

#ifndef INCLUDE_OKVIS_CERES_MARGINALIZATIONERROR_HPP_
#define INCLUDE_OKVIS_CERES_MARGINALIZATIONERROR_HPP_

#include <vector>
#include <deque>
#include <memory>
#include <Eigen/Core>
#include <okvis/kinematics/Transformation.hpp>
#include <okvis/Measurements.hpp>
#include <okvis/Variables.hpp>
#include "ceres/ceres.h"
#include <okvis/kinematics/Transformation.hpp>
#include <okvis/assert_macros.hpp>
#include <okvis/ceres/HomogeneousPointParameterBlock.hpp>
#include <okvis/ceres/HomogeneousPointLocalParameterization.hpp>
#include <okvis/ceres/PoseParameterBlock.hpp>
#include <okvis/ceres/SpeedAndBiasParameterBlock.hpp>
#include <okvis/ceres/Map.hpp>
#include <okvis/ceres/ErrorInterface.hpp>

namespace okvis {
namespace ceres {

// not sized, in order to be flexible.
class MarginalizationError : public ::ceres::CostFunction, public ErrorInterface
{
 public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)

  
  typedef ::ceres::CostFunction base_t;

  virtual ~MarginalizationError()
  {
  }

  MarginalizationError();

  MarginalizationError(Map& map);

  MarginalizationError(
      Map& map, std::vector< ::ceres::ResidualBlockId > & residualBlockIds);

  // initialization
  void setMap(Map& map);

  bool addResidualBlocks(
      const std::vector< ::ceres::ResidualBlockId > & residualBlockIds,
      const std::vector<bool> & keepResidualBlocks = std::vector<bool>());

  bool addResidualBlock(::ceres::ResidualBlockId residualBlockId,
                        bool keepResidualBlock = false);

  bool isParameterBlockConnected(uint64_t parameterBlockId);

  // marginalization

  bool marginalizeOut(const std::vector<uint64_t> & parameterBlockIds,
                      const std::vector<bool> & keepParameterBlocks =
                          std::vector<bool>());

  void updateErrorComputation();

  void getParameterBlockPtrs(
      std::vector<std::shared_ptr<okvis::ceres::ParameterBlock> >& parameterBlockPtrs);

  // error term and Jacobian implementation (inherited pure virtuals from ::ceres::CostFunction)
  virtual bool Evaluate(double const* const * parameters, double* residuals,
                        double** jacobians) const;

  bool EvaluateWithMinimalJacobians(double const* const * parameters,
                                    double* residuals, double** jacobians,
                                    double** jacobiansMinimal) const;

  // sizes (inherited pure virtuals from ::okvis::ceres::ErrorInterface)
  size_t residualDim() const
  {
    return base_t::num_residuals();
  }

  size_t parameterBlocks() const
  {
    return base_t::parameter_block_sizes().size();
  }

  size_t parameterBlockDim(size_t parameterBlockId) const
  {
    return base_t::parameter_block_sizes().at(parameterBlockId);
  }

  virtual std::string typeInfo() const
  {
    return "MarginalizationError";
  }


  template<typename Derived>
  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);

  template<typename Derived>
  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);

  template<typename Derived, int blockDim>
  static void blockPinverse(
      const Eigen::MatrixBase<Derived>& M_in,
      const Eigen::MatrixBase<Derived>& M_out, double epsilon =
          std::numeric_limits<typename Derived::Scalar>::epsilon());


  template<typename Derived, int blockDim>
  static void blockPinverseSqrt(
      const Eigen::MatrixBase<Derived>& M_in,
      const Eigen::MatrixBase<Derived>& M_out, double epsilon =
          std::numeric_limits<typename Derived::Scalar>::epsilon());

 protected:
  Map* mapPtr_; 
  ::ceres::ResidualBlockId residualBlockId_; 

  void check();

  bool computeDeltaChi(Eigen::VectorXd& DeltaChi) const;  // use the stored estimates

  bool computeDeltaChi(double const* const * parameters,
                       Eigen::VectorXd& DeltaChi) const;  // use the provided estimates

  template<typename Derived_A, typename Derived_U, typename Derived_W,
      typename Derived_V>
  static void splitSymmetricMatrix(
      const std::vector<std::pair<int, int> >& marginalizationStartIdxAndLengthPairs,
      const Eigen::MatrixBase<Derived_A>& A,  // input
      const Eigen::MatrixBase<Derived_U>& U,  // output
      const Eigen::MatrixBase<Derived_W>& W,  // output
      const Eigen::MatrixBase<Derived_V>& V);  // output

  template<typename Derived_b, typename Derived_b_a, typename Derived_b_b>
  static void splitVector(
      const std::vector<std::pair<int, int> >& marginalizationStartIdxAndLengthPairs,
      const Eigen::MatrixBase<Derived_b>& b,  // input
      const Eigen::MatrixBase<Derived_b_a>& b_a,  // output
      const Eigen::MatrixBase<Derived_b_b>& b_b);  // output

  Eigen::MatrixXd H_;  
  Eigen::VectorXd b0_;  
  Eigen::VectorXd e0_;  
  Eigen::MatrixXd J_;  
  Eigen::MatrixXd U_;  
  Eigen::VectorXd S_;  
  Eigen::VectorXd S_sqrt_;  
  Eigen::VectorXd S_pinv_;  
  Eigen::VectorXd S_pinv_sqrt_;  
  Eigen::VectorXd p_;
  Eigen::VectorXd p_inv_;
  volatile bool errorComputationValid_;  

  struct ParameterBlockInfo
  {
    OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
    uint64_t parameterBlockId;
    std::shared_ptr<ParameterBlock> parameterBlockPtr;
    size_t orderingIdx;
    size_t dimension;
    size_t minimalDimension;
    size_t localDimension;
    std::shared_ptr<double> linearizationPoint;
    bool isLandmark;
    ParameterBlockInfo()
        : parameterBlockId(0),
          parameterBlockPtr(std::shared_ptr<ParameterBlock>()),
          orderingIdx(0),
          dimension(0),
          minimalDimension(0),
          localDimension(0),
          isLandmark(false)
    {
    }
    ParameterBlockInfo(uint64_t parameterBlockId,
                       std::shared_ptr<ParameterBlock> parameterBlockPtr,
                       size_t orderingIdx, bool isLandmark)
        : parameterBlockId(parameterBlockId),
          parameterBlockPtr(parameterBlockPtr),
          orderingIdx(orderingIdx),
          isLandmark(isLandmark)
    {
      dimension = parameterBlockPtr->dimension();
      minimalDimension = parameterBlockPtr->minimalDimension();
      if (parameterBlockPtr->localParameterizationPtr()) {
        localDimension = parameterBlockPtr->localParameterizationPtr()
            ->LocalSize();
      } else {
        localDimension = minimalDimension;
      }
      if (parameterBlockPtr->fixed()) {
        minimalDimension = 0;
        localDimension = 0;
          }
      linearizationPoint.reset(new double[dimension],
                               std::default_delete<double[]>());
      memcpy(linearizationPoint.get(), parameterBlockPtr->parameters(),
             dimension * sizeof(double));
    }

    void resetLinearizationPoint(
        std::shared_ptr<ParameterBlock> parameterBlockPtr)
    {
      OKVIS_ASSERT_TRUE_DBG(Exception,dimension==parameterBlockPtr->dimension(),"not initialised.")
      memcpy(linearizationPoint.get(), parameterBlockPtr->parameters(),
             dimension * sizeof(double));
    }
  };

  std::vector<ParameterBlockInfo> parameterBlockInfos_;  
  std::map<uint64_t, size_t> parameterBlockId2parameterBlockInfoIdx_;  
  size_t denseIndices_;  


};

}  // namespace ceres
}  // namespace okvis

#include "implementation/MarginalizationError.hpp"

#endif /* INCLUDE_OKVIS_CERES_MARGINALIZATIONERROR_HPP_ */