Map.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 8, 2013
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

#ifndef INCLUDE_OKVIS_CERES_MAP_HPP_
#define INCLUDE_OKVIS_CERES_MAP_HPP_

#include <memory>

#include <ceres/ceres.h>
#include <okvis/ceres/ParameterBlock.hpp>
#include <okvis/ceres/HomogeneousPointLocalParameterization.hpp>
#include <okvis/ceres/PoseLocalParameterization.hpp>
#include <okvis/Time.hpp>
#include <okvis/FrameTypedefs.hpp>
#include <okvis/assert_macros.hpp>
#include <unordered_map>
#include <okvis/ceres/ErrorInterface.hpp>

namespace okvis {
namespace ceres {

class Map {
 public:
  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)

  
  Map();

  // definitions
  struct ResidualBlockSpec {
    ResidualBlockSpec()
        : residualBlockId(0),
          lossFunctionPtr(0),
          errorInterfacePtr(std::shared_ptr<ErrorInterface>()) {
    }

    ResidualBlockSpec(::ceres::ResidualBlockId residualBlockId,
                      ::ceres::LossFunction* lossFunctionPtr,
                      std::shared_ptr<ErrorInterface> errorInterfacePtr)
        : residualBlockId(residualBlockId),
          lossFunctionPtr(lossFunctionPtr),
          errorInterfacePtr(errorInterfacePtr) {
    }

    ::ceres::ResidualBlockId residualBlockId;           
    ::ceres::LossFunction* lossFunctionPtr;             
    std::shared_ptr<ErrorInterface> errorInterfacePtr;  
  };
  typedef std::pair<uint64_t, std::shared_ptr<okvis::ceres::ParameterBlock> > ParameterBlockSpec;

  typedef std::vector<ResidualBlockSpec> ResidualBlockCollection;
  typedef std::vector<ParameterBlockSpec> ParameterBlockCollection;

  enum Parameterization {
    HomogeneousPoint,     
    Pose6d,               
    Pose3d,               
    Pose4d,               
    Pose2d,               
    Trivial               
  };

  bool parameterBlockExists(uint64_t parameterBlockId) const;


  void printParameterBlockInfo(uint64_t parameterBlockId) const;

  void printResidualBlockInfo(::ceres::ResidualBlockId residualBlockId) const;


  // for quality assessment
  void getLhs(uint64_t parameterBlockId, Eigen::MatrixXd& H);


  bool addParameterBlock(
      std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlock,
      int parameterization = Parameterization::Trivial, const int group = -1);

  bool removeParameterBlock(uint64_t parameterBlockId);

  bool removeParameterBlock(
      std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlock);

  ::ceres::ResidualBlockId addResidualBlock(
      std::shared_ptr< ::ceres::CostFunction> cost_function,
      ::ceres::LossFunction* loss_function,
      std::vector<std::shared_ptr<okvis::ceres::ParameterBlock> >& parameterBlockPtrs);

  void resetResidualBlock(
      ::ceres::ResidualBlockId residualBlockId,
      std::vector<std::shared_ptr<okvis::ceres::ParameterBlock> >& parameterBlockPtrs);

  ::ceres::ResidualBlockId addResidualBlock(
      std::shared_ptr< ::ceres::CostFunction> cost_function,
      ::ceres::LossFunction* loss_function,
      std::shared_ptr<okvis::ceres::ParameterBlock> x0,
      std::shared_ptr<okvis::ceres::ParameterBlock> x1 = std::shared_ptr<
          okvis::ceres::ParameterBlock>(),
      std::shared_ptr<okvis::ceres::ParameterBlock> x2 = std::shared_ptr<
          okvis::ceres::ParameterBlock>(),
      std::shared_ptr<okvis::ceres::ParameterBlock> x3 = std::shared_ptr<
          okvis::ceres::ParameterBlock>(),
      std::shared_ptr<okvis::ceres::ParameterBlock> x4 = std::shared_ptr<
          okvis::ceres::ParameterBlock>(),
      std::shared_ptr<okvis::ceres::ParameterBlock> x5 = std::shared_ptr<
          okvis::ceres::ParameterBlock>(),
      std::shared_ptr<okvis::ceres::ParameterBlock> x6 = std::shared_ptr<
          okvis::ceres::ParameterBlock>(),
      std::shared_ptr<okvis::ceres::ParameterBlock> x7 = std::shared_ptr<
          okvis::ceres::ParameterBlock>(),
      std::shared_ptr<okvis::ceres::ParameterBlock> x8 = std::shared_ptr<
          okvis::ceres::ParameterBlock>(),
      std::shared_ptr<okvis::ceres::ParameterBlock> x9 = std::shared_ptr<
          okvis::ceres::ParameterBlock>());

  bool removeResidualBlock(::ceres::ResidualBlockId id);



  bool setParameterBlockConstant(uint64_t parameterBlockId);

  bool setParameterBlockVariable(uint64_t parameterBlockId);

  bool setParameterBlockConstant(
      std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlock) {
    return setParameterBlockConstant(parameterBlock->id());
  }

  bool setParameterBlockVariable(
      std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlock) {
    return setParameterBlockVariable(parameterBlock->id());
  }

  bool resetParameterization(uint64_t parameterBlockId, int parameterization);

  bool setParameterization(
      uint64_t parameterBlockId,
      ::ceres::LocalParameterization* local_parameterization);

  bool setParameterization(
      std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlock,
      ::ceres::LocalParameterization* local_parameterization) {
    return setParameterization(parameterBlock->id(), local_parameterization);
  }



  std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlockPtr(
      uint64_t parameterBlockId);  // get a vertex

  std::shared_ptr<const okvis::ceres::ParameterBlock> parameterBlockPtr(
      uint64_t parameterBlockId) const;  // get a vertex

  std::shared_ptr<okvis::ceres::ErrorInterface> errorInterfacePtr(
      ::ceres::ResidualBlockId residualBlockId);  // get a vertex

  std::shared_ptr<const okvis::ceres::ErrorInterface> errorInterfacePtr(
      ::ceres::ResidualBlockId residualBlockId) const;  // get a vertex

  ResidualBlockCollection residuals(uint64_t parameterBlockId) const;

  ParameterBlockCollection parameters(
      ::ceres::ResidualBlockId residualBlockId) const;  // get the parameter blocks connected


  // Jacobian checker
  bool isJacobianCorrect(::ceres::ResidualBlockId residualBlockId,
                         double relTol = 1e-6) const;

  // access to the map as such
  typedef std::unordered_map<uint64_t,
      std::shared_ptr<okvis::ceres::ParameterBlock> > Id2ParameterBlock_Map;

  typedef std::unordered_map< ::ceres::ResidualBlockId, ResidualBlockSpec> ResidualBlockId2ResidualBlockSpec_Map;

  const Id2ParameterBlock_Map& id2parameterBlockMap() const {
    return id2ParameterBlock_Map_;
  }
  const ResidualBlockId2ResidualBlockSpec_Map& residualBlockId2ResidualBlockSpecMap() const {
    return residualBlockId2ResidualBlockSpec_Map_;
  }

  // these are public for convenient manipulation
  ::ceres::Solver::Options options;

  ::ceres::Solver::Summary summary;

  void solve() {
    Solve(options, problem_.get(), &summary);
  }

 protected:

  uint64_t residualCounter_;

  // member variables related to optimization
  std::shared_ptr< ::ceres::Problem> problem_;

  // the actual maps
  typedef std::unordered_multimap<uint64_t, ResidualBlockSpec> Id2ResidualBlock_Multimap;

  typedef std::unordered_map< ::ceres::ResidualBlockId,
      ParameterBlockCollection> ResidualBlockId2ParameterBlockCollection_Map;

  Id2ParameterBlock_Map id2ParameterBlock_Map_;

  ResidualBlockId2ResidualBlockSpec_Map residualBlockId2ResidualBlockSpec_Map_;

  Id2ResidualBlock_Multimap id2ResidualBlock_Multimap_;

  ResidualBlockId2ParameterBlockCollection_Map residualBlockId2ParameterBlockCollection_Map_;

  okvis::ceres::HomogeneousPointLocalParameterization homogeneousPointLocalParameterization_;

  okvis::ceres::PoseLocalParameterization poseLocalParameterization_;

  okvis::ceres::PoseLocalParameterization2d poseLocalParameterization2d_;

  okvis::ceres::PoseLocalParameterization3d poseLocalParameterization3d_;

  okvis::ceres::PoseLocalParameterization4d poseLocalParameterization4d_;

};

}  //namespace okvis
}  //namespace ceres

#endif /* INCLUDE_OKVIS_CERES_MAP_HPP_ */