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

#ifndef INCLUDE_OKVIS_CERES_IMUERROR_HPP_
#define INCLUDE_OKVIS_CERES_IMUERROR_HPP_

#include <vector>
#include <mutex>
#include "ceres/ceres.h"
#include <okvis/FrameTypedefs.hpp>
#include <okvis/Time.hpp>
#include <okvis/assert_macros.hpp>
#include <okvis/Measurements.hpp>
#include <okvis/Variables.hpp>
#include <okvis/Parameters.hpp>
#include <okvis/ceres/ErrorInterface.hpp>

namespace okvis {
namespace ceres {

class ImuError :
    public ::ceres::SizedCostFunction<15 /* number of residuals */,
        7 /* size of first parameter (PoseParameterBlock k) */,
        9 /* size of second parameter (SpeedAndBiasParameterBlock k) */,
        7 /* size of third parameter (PoseParameterBlock k+1) */,
        9 /* size of fourth parameter (SpeedAndBiasParameterBlock k+1) */>,
    public ErrorInterface {
 public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)

  
  typedef ::ceres::SizedCostFunction<15, 7, 9, 7, 9> base_t;

  static const int kNumResiduals = 15;

  typedef Eigen::Matrix<double, 15, 15> covariance_t;

  typedef covariance_t information_t;

  typedef Eigen::Matrix<double, 15, 15> jacobian_t;

  typedef Eigen::Matrix<double, 15, 7> jacobian0_t;

  typedef Eigen::Matrix<double, 15, 9> jacobian1_t;

  ImuError() {
  }

  virtual ~ImuError() {
  }

  ImuError(const okvis::ImuMeasurementDeque & imuMeasurements,
           const okvis::ImuParameters & imuParameters, const okvis::Time& t_0,
           const okvis::Time& t_1);

  static int propagation(const okvis::ImuMeasurementDeque & imuMeasurements,
                         const okvis::ImuParameters & imuParams,
                         okvis::kinematics::Transformation& T_WS,
                         okvis::SpeedAndBias & speedAndBiases,
                         const okvis::Time& t_start, const okvis::Time& t_end,
                         covariance_t* covariance = 0,
                         jacobian_t* jacobian = 0);

  int redoPreintegration(const okvis::kinematics::Transformation& T_WS,
                         const okvis::SpeedAndBias & speedAndBiases) const;

  // setters

  void setImuParameters(const okvis::ImuParameters& imuParameters) {
    imuParameters_ = imuParameters;
  }

  void setImuMeasurements(const okvis::ImuMeasurementDeque& imuMeasurements) {
    imuMeasurements_ = imuMeasurements;
  }

  void setT0(const okvis::Time& t_0) {
    t0_ = t_0;
  }

  void setT1(const okvis::Time& t_1) {
    t1_ = t_1;
  }

  // getters

  const okvis::ImuParameters& imuParameters() const {
    return imuParameters_;
  }

  const okvis::ImuMeasurementDeque& imuMeasurements() const {
    return imuMeasurements_;
  }

  okvis::Time t0() const {
    return t0_;
  }

  okvis::Time t1() const {
    return t1_;
  }

  // error term and Jacobian implementation
  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
  size_t residualDim() const {
    return kNumResiduals;
  }

  virtual size_t parameterBlocks() const {
    return 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 "ImuError";
  }

 protected:
  // parameters
  okvis::ImuParameters imuParameters_; 

  // measurements
  okvis::ImuMeasurementDeque imuMeasurements_; 

  // times
  okvis::Time t0_; 
  okvis::Time t1_; 

  // preintegration stuff. the mutable is a TERRIBLE HACK, but what can I do.
  mutable std::mutex preintegrationMutex_; //< Protect access of intermediate results.
  // increments (initialise with identity)
  mutable Eigen::Quaterniond Delta_q_ = Eigen::Quaterniond(1,0,0,0); 
  mutable Eigen::Matrix3d C_integral_ = Eigen::Matrix3d::Zero(); 
  mutable Eigen::Matrix3d C_doubleintegral_ = Eigen::Matrix3d::Zero(); 
  mutable Eigen::Vector3d acc_integral_ = Eigen::Vector3d::Zero(); 
  mutable Eigen::Vector3d acc_doubleintegral_ = Eigen::Vector3d::Zero(); 

  // cross matrix accumulatrion
  mutable Eigen::Matrix3d cross_ = Eigen::Matrix3d::Zero(); 

  // sub-Jacobians
  mutable Eigen::Matrix3d dalpha_db_g_ = Eigen::Matrix3d::Zero(); 
  mutable Eigen::Matrix3d dv_db_g_ = Eigen::Matrix3d::Zero(); 
  mutable Eigen::Matrix3d dp_db_g_ = Eigen::Matrix3d::Zero(); 

  mutable Eigen::Matrix<double,15,15> P_delta_ = Eigen::Matrix<double,15,15>::Zero();

  mutable SpeedAndBiases speedAndBiases_ref_ = SpeedAndBiases::Zero();

  mutable bool redo_ = true; 
  mutable int redoCounter_ = 0; 

  // information matrix and its square root
  mutable information_t information_; 
  mutable information_t squareRootInformation_; 

};

}  // namespace ceres
}  // namespace okvis

#endif /* INCLUDE_OKVIS_CERES_IMUERROR_HPP_ */