Estimator.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: Dec 30, 2014
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *    Modified: Andreas Forster (an.forster@gmail.com)
 *********************************************************************************/

#ifndef INCLUDE_OKVIS_ESTIMATOR_HPP_
#define INCLUDE_OKVIS_ESTIMATOR_HPP_

#include <memory>
#include <mutex>
#include <array>

#include <ceres/ceres.h>
#include <okvis/kinematics/Transformation.hpp>

#include <okvis/assert_macros.hpp>
#include <okvis/VioBackendInterface.hpp>
#include <okvis/MultiFrame.hpp>
#include <okvis/FrameTypedefs.hpp>
#include <okvis/Measurements.hpp>
#include <okvis/Variables.hpp>
#include <okvis/ceres/PoseParameterBlock.hpp>
#include <okvis/ceres/SpeedAndBiasParameterBlock.hpp>
#include <okvis/ceres/HomogeneousPointParameterBlock.hpp>
#include <okvis/ceres/Map.hpp>
#include <okvis/ceres/MarginalizationError.hpp>
#include <okvis/ceres/ReprojectionError.hpp>
#include <okvis/ceres/CeresIterationCallback.hpp>

namespace okvis {


class Estimator : public VioBackendInterface
{
 public:
  OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error)
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Estimator();

  Estimator(std::shared_ptr<okvis::ceres::Map> mapPtr);
  virtual ~Estimator();


  int addCamera(
      const okvis::ExtrinsicsEstimationParameters & extrinsicsEstimationParameters);

  int addImu(const okvis::ImuParameters & imuParameters);

  void clearCameras();

  void clearImus();


  bool addStates(okvis::MultiFramePtr multiFrame,
                 const okvis::ImuMeasurementDeque & imuMeasurements,
                 bool asKeyframe);

  void printStates(uint64_t poseId, std::ostream & buffer) const;

  bool addLandmark(uint64_t landmarkId,
                   const Eigen::Vector4d & landmark);

  template<class GEOMETRY_TYPE>
  ::ceres::ResidualBlockId addObservation(uint64_t landmarkId, uint64_t poseId,
                                          size_t camIdx, size_t keypointIdx);
  bool removeObservation(uint64_t landmarkId, uint64_t poseId,  size_t camIdx,
                         size_t keypointIdx);

  bool applyMarginalizationStrategy(size_t numKeyframes, size_t numImuFrames,
                                    okvis::MapPointVector& removedLandmarks);

  static bool initPoseFromImu(
      const okvis::ImuMeasurementDeque & imuMeasurements,
      okvis::kinematics::Transformation & T_WS);

  void optimize(size_t numIter, size_t numThreads = 1, bool verbose = false);

  bool setOptimizationTimeLimit(double timeLimit, int minIterations);

  bool isLandmarkAdded(uint64_t landmarkId) const {
    bool isAdded = landmarksMap_.find(landmarkId) != landmarksMap_.end();
    OKVIS_ASSERT_TRUE_DBG(Exception, isAdded == mapPtr_->parameterBlockExists(landmarkId),
                   "id="<<landmarkId<<" inconsistent. isAdded = " << isAdded);
    return isAdded;
  }

  bool isLandmarkInitialized(uint64_t landmarkId) const;


  bool getLandmark(uint64_t landmarkId, okvis::MapPoint& mapPoint) const;

  size_t getLandmarks(okvis::PointMap & landmarks) const;

  size_t getLandmarks(okvis::MapPointVector & landmarks) const;

  okvis::MultiFramePtr multiFrame(uint64_t frameId) const {
    OKVIS_ASSERT_TRUE_DBG(Exception, multiFramePtrMap_.find(frameId)!=multiFramePtrMap_.end(),
                       "Requested multi-frame does not exist in estimator.");
    return multiFramePtrMap_.at(frameId);
  }

  bool get_T_WS(uint64_t poseId, okvis::kinematics::Transformation & T_WS) const;

  // the following access the optimization graph, so are not very fast.
  // Feel free to implement caching for them...
  bool getSpeedAndBias(uint64_t poseId, uint64_t imuIdx, okvis::SpeedAndBias & speedAndBias) const;

  bool getCameraSensorStates(uint64_t poseId, size_t cameraIdx,
                              okvis::kinematics::Transformation & T_SCi) const;

  size_t numFrames() const {
    return statesMap_.size();
  }

  size_t numLandmarks() const {
    return landmarksMap_.size();
  }

  uint64_t currentKeyframeId() const;

  uint64_t frameIdByAge(size_t age) const;

  uint64_t currentFrameId() const;


  bool isKeyframe(uint64_t frameId) const {
    return statesMap_.at(frameId).isKeyframe;
  }

  bool isInImuWindow(uint64_t frameId) const;


  okvis::Time timestamp(uint64_t frameId) const {
    return statesMap_.at(frameId).timestamp;
  }


  bool set_T_WS(uint64_t poseId, const okvis::kinematics::Transformation & T_WS);

  bool setSpeedAndBias(uint64_t poseId, size_t imuIdx, const okvis::SpeedAndBias & speedAndBias);

  bool setCameraSensorStates(uint64_t poseId, size_t cameraIdx,
                              const okvis::kinematics::Transformation & T_SCi);

  bool setLandmark(uint64_t landmarkId, const Eigen::Vector4d & landmark);

  void setLandmarkInitialized(uint64_t landmarkId, bool initialized);

  void setKeyframe(uint64_t frameId, bool isKeyframe){
    statesMap_.at(frameId).isKeyframe = isKeyframe;
  }

  void setMap(std::shared_ptr<okvis::ceres::Map> mapPtr) {
    mapPtr_ = mapPtr;
  }

 private:

  bool removeObservation(::ceres::ResidualBlockId residualBlockId);

  struct StateInfo
  {
    StateInfo(uint64_t id = 0, bool isRequired = true, bool exists = false)
        : id(id),
          isRequired(isRequired),
          exists(exists)
    {
    }
    uint64_t id; 
    bool isRequired; 
    bool exists; 
  };

  enum GlobalStates
  {
    T_WS = 0, 
    MagneticZBias = 1, 
    Qff = 2, 
    T_GW = 3, 
  };

  enum SensorStates
  {
    Camera = 0, 
    Imu = 1, 
    Position = 2, 
    Gps = 3, 
    Magnetometer = 4, 
    StaticPressure = 5, 
    DynamicPressure = 6 
  };

  enum CameraSensorStates
  {
    T_SCi = 0, 
    Intrinsics = 1, 
  };

  enum ImuSensorStates
  {
    SpeedAndBias = 0 
  };

  enum PositionSensorStates
  {
    T_PiW = 0,  
    PositionSensorB_t_BA = 1  
  };

  enum GpsSensorStates
  {
    GpsB_t_BA = 0  
  };

  enum MagnetometerSensorStates
  {
    MagnetometerBias = 0 
  };

  enum StaticPressureSensorStates
  {
    StaticPressureBias = 0 
  };

  enum DynamicPressureSensorStates
  {
    DynamicPressureBias = 0 
  };

  // getters
  bool getGlobalStateParameterBlockPtr(uint64_t poseId, int stateType,
                                    std::shared_ptr<ceres::ParameterBlock>& stateParameterBlockPtr) const;
  template<class PARAMETER_BLOCK_T>
  bool getGlobalStateParameterBlockAs(uint64_t poseId, int stateType,
                                      PARAMETER_BLOCK_T & stateParameterBlock) const;
  template<class PARAMETER_BLOCK_T>
  bool getGlobalStateEstimateAs(uint64_t poseId, int stateType,
                                typename PARAMETER_BLOCK_T::estimate_t & state) const;

  bool getSensorStateParameterBlockPtr(uint64_t poseId, int sensorIdx,
                                    int sensorType, int stateType,
                                    std::shared_ptr<ceres::ParameterBlock>& stateParameterBlockPtr) const;
  template<class PARAMETER_BLOCK_T>
  bool getSensorStateParameterBlockAs(uint64_t poseId, int sensorIdx,
                                      int sensorType, int stateType,
                                      PARAMETER_BLOCK_T & stateParameterBlock) const;
  template<class PARAMETER_BLOCK_T>
  bool getSensorStateEstimateAs(uint64_t poseId, int sensorIdx, int sensorType,
                                int stateType, typename PARAMETER_BLOCK_T::estimate_t & state) const;

  // setters
  template<class PARAMETER_BLOCK_T>
  bool setGlobalStateEstimateAs(uint64_t poseId, int stateType,
                                const typename PARAMETER_BLOCK_T::estimate_t & state);
  template<class PARAMETER_BLOCK_T>
  bool setSensorStateEstimateAs(uint64_t poseId, int sensorIdx, int sensorType,
                                int stateType, const typename PARAMETER_BLOCK_T::estimate_t & state);

  // the following are just fixed-size containers for related parameterBlockIds:
  typedef std::array<StateInfo, 6> GlobalStatesContainer; 
  typedef std::vector<StateInfo> SpecificSensorStatesContainer;  
  typedef std::array<std::vector<SpecificSensorStatesContainer>, 7> AllSensorStatesContainer; 

  struct States
  {
    States() : isKeyframe(false), id(0) {}
    States(bool isKeyframe, uint64_t id, okvis::Time timestamp)
      : isKeyframe(isKeyframe), id(id), timestamp(timestamp) {}
    GlobalStatesContainer global;
    AllSensorStatesContainer sensors;
    bool isKeyframe;
    uint64_t id;
    okvis::Time timestamp;
  };

  // the following keeps track of all the states at different time instances (key=poseId)
  std::map<uint64_t, States> statesMap_; 
  std::map<uint64_t, okvis::MultiFramePtr> multiFramePtrMap_; 
  std::shared_ptr<okvis::ceres::Map> mapPtr_; 

  // this is the reference pose
  uint64_t referencePoseId_; 

  // the following are updated after the optimization
  okvis::PointMap landmarksMap_; 
  mutable std::mutex statesMutex_;  

  // parameters
  std::vector<okvis::ExtrinsicsEstimationParameters,
      Eigen::aligned_allocator<okvis::ExtrinsicsEstimationParameters> > extrinsicsEstimationParametersVec_; 
  std::vector<okvis::ImuParameters, Eigen::aligned_allocator<okvis::ImuParameters> > imuParametersVec_; 

  // loss function for reprojection errors
  std::shared_ptr< ::ceres::LossFunction> cauchyLossFunctionPtr_; 
  std::shared_ptr< ::ceres::LossFunction> huberLossFunctionPtr_; 

  // the marginalized error term
  std::shared_ptr<ceres::MarginalizationError> marginalizationErrorPtr_; 
  ::ceres::ResidualBlockId marginalizationResidualId_; 

  // ceres iteration callback object
  std::unique_ptr<okvis::ceres::CeresIterationCallback> ceresCallback_; 
};

}  // namespace okvis

#include "implementation/Estimator.hpp"

#endif /* INCLUDE_OKVIS_ESTIMATOR_HPP_ */