msf_measurement.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland
 * You can contact the author at <slynen at ethz dot ch>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#ifndef MEASUREMENT_H_
#define MEASUREMENT_H_

#include <Eigen/Dense>
#include <boost/shared_ptr.hpp>

#include <msf_core/msf_fwds.h>
#include <msf_core/msf_types.hpp>

namespace msf_core {
template<typename EKFState_T>
class MSF_MeasurementBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  MSF_MeasurementBase(bool isabsoluteMeasurement, int sensorID);
  virtual ~MSF_MeasurementBase() {
  }
  virtual void apply(shared_ptr<EKFState_T> stateWithCovariance,
                     MSF_Core<EKFState_T>& core) = 0;
  virtual std::string type() = 0;
  int sensorID_;
  bool isabsolute_;
  double time;  
 protected:
  template<class H_type, class Res_type, class R_type>
  void calculateAndApplyCorrection(
      shared_ptr<EKFState_T> state, MSF_Core<EKFState_T>& core,
      const Eigen::MatrixBase<H_type>& H,
      const Eigen::MatrixBase<Res_type>& residual,
      const Eigen::MatrixBase<R_type>& R);

  void calculateAndApplyCorrection(shared_ptr<EKFState_T> state,
                                   MSF_Core<EKFState_T>& core,
                                   const Eigen::MatrixXd& H,
                                   const Eigen::MatrixXd& residual,
                                   const Eigen::MatrixXd& R);

  template<class H_type, class Res_type, class R_type>
  void calculateAndApplyCorrectionRelative(
      shared_ptr<EKFState_T> state_old, shared_ptr<EKFState_T> state_new,
      MSF_Core<EKFState_T>& core, const Eigen::MatrixBase<H_type>& H_old,
      const Eigen::MatrixBase<H_type>& H_new,
      const Eigen::MatrixBase<Res_type>& residual,
      const Eigen::MatrixBase<R_type>& R);

};

template<typename EKFState_T>
class MSF_InvalidMeasurement : public MSF_MeasurementBase<EKFState_T> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  virtual void apply(shared_ptr<EKFState_T> UNUSEDPARAM(stateWithCovariance),
                     MSF_Core<EKFState_T>& UNUSEDPARAM(core)) {
    MSF_ERROR_STREAM(
        "Called apply() on an MSF_InvalidMeasurement object. This should never "
        "happen.");
  }
  virtual std::string type() {
    return "invalid";
  }
  MSF_InvalidMeasurement()
      : MSF_MeasurementBase<EKFState_T>(true, -1) {
  }
  virtual ~MSF_InvalidMeasurement() {
  }
};

template<typename T, typename RMAT_T, typename EKFState_T>
class MSF_Measurement : public MSF_MeasurementBase<EKFState_T> {
 private:
  virtual void makeFromSensorReadingImpl(
      const boost::shared_ptr<T const> reading) = 0;
 protected:
  RMAT_T R_;
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  typedef T Measurement_type;
  typedef boost::shared_ptr<T const> Measurement_ptr;

  MSF_Measurement(bool isAbsoluteMeasurement, int sensorID)
      : MSF_MeasurementBase<EKFState_T>(isAbsoluteMeasurement, sensorID) {
    R_.setZero();
  }
  virtual ~MSF_Measurement() {
  }
  ;
  void makeFromSensorReading(const boost::shared_ptr<T const> reading,
                             double timestamp) {
    this->time = timestamp;

    makeFromSensorReadingImpl(reading);

    // Check whether the user has set R.
    if (R_.minCoeff() == 0.0 && R_.maxCoeff() == 0.0) {
      MSF_WARN_STREAM_THROTTLE(
          2,
          "The measurement covariance matrix seems to be not set for the current "
          "measurement. Please double check!");
    }

    for (int i = 0; i < R_.RowsAtCompileTime; ++i) {
      if (R_(i, i) == 0.0) {
        MSF_WARN_STREAM_THROTTLE(
            2,
            "The measurement covariance matrix has some diagonal elements set to "
            "zero. Please double check!");
      }
    }
  }
// Apply is implemented by respective sensor measurement types.
};

template<typename EKFState_T>
class MSF_InitMeasurement : public MSF_MeasurementBase<EKFState_T> {
 private:
  EKFState_T InitState;  

  bool ContainsInitialSensorReadings_;
  typedef typename EKFState_T::StateSequence_T StateSequence_T;
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW MSF_InitMeasurement(
      bool ContainsInitialSensorReadings)
      : MSF_MeasurementBase<EKFState_T>(true, -1) {
    ContainsInitialSensorReadings_ = ContainsInitialSensorReadings;
    this->time = ros::Time::now().toSec();
  }
  virtual ~MSF_InitMeasurement() {
  }
  ;
  virtual std::string type() {
    return "init";
  }
  typename EKFState_T::P_type& get_P() {
    return InitState.P;
  }
  Eigen::Matrix<double, 3, 1>& get_w_m() {
    return InitState.w_m;
  }
  Eigen::Matrix<double, 3, 1>& get_a_m() {
    return InitState.a_m;
  }

  template<int INDEX, typename T>
  void setStateInitValue(const T& initvalue) {
    InitState.template getStateVar<INDEX>().state_ = initvalue;
    InitState.template getStateVar<INDEX>().hasResetValue = true;
  }

  template<int INDEX>
  void resetStateInitValue() {
    InitState.template get<INDEX>().hasResetValue = false;
  }

  template<int INDEX>
  const typename msf_tmp::StripReference<typename boost::fusion::result_of::at_c
  <StateSequence_T, INDEX >::type>::result_t::value_t&
  getStateInitValue() const {
    return InitState.template get<INDEX>();
  }

  virtual void apply(shared_ptr<EKFState_T> stateWithCovariance, MSF_Core<EKFState_T>& core);
};

template<typename EKFState_T>
class sortMeasurements {
 public:
  bool operator()(const MSF_MeasurementBase<EKFState_T>& lhs,
                  const MSF_MeasurementBase<EKFState_T>&rhs) const {
    return (lhs.time_ < rhs.time_);
  }
};

}

#include <msf_core/implementation/msf_measurement.hpp>

#endif  // MEASUREMENT_H_