msf_state.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 MSF_STATE_H_
#define MSF_STATE_H_

#include <msf_core/msf_types.hpp>
#include <msf_core/msf_tmp.h>
#include <msf_core/msf_statevisitor.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <vector>
#include <utility>
#include <msf_core/eigen_conversions.h>
#include <sensor_fusion_comm/ExtState.h>
#include <sensor_fusion_comm/DoubleArrayStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

namespace msf_core {

template<typename type_T, int name_T, int STATETYPE, int OPTIONS>
struct StateVar_T {
  typedef type_T value_t;
  typedef const StateVar_T<type_T, name_T>& constRef_T;
  typedef const StateVar_T<type_T, name_T>* constPtr_T;
  typedef StateVar_T<type_T, name_T>& Ref_T;
  typedef StateVar_T<type_T, name_T>* Ptr_T;
  enum {
    statetype_ = STATETYPE,
    options_ = OPTIONS,
    name_ = name_T,
    sizeInCorrection_ = msf_tmp::CorrectionStateLengthForType<
        const StateVar_T<type_T, name_T>&>::value,
    sizeInState_ = msf_tmp::StateLengthForType<const StateVar_T<type_T, name_T>&>::value
  };
  typedef Eigen::Matrix<double, sizeInCorrection_, sizeInCorrection_> Q_T;

  Q_T Q;  
  value_t state_;  
  bool hasResetValue;  //<Indicating that this statevariable has a reset value
                       // to be applied to the state on init.
  StateVar_T() {
    hasResetValue = false;
    Q.setZero();
  }
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template<typename StateSeq_T, typename StateDef_T>
struct GenericState_T {
 public:
  typedef StateSeq_T StateSequence_T;
  typedef StateDef_T StateDefinition_T;

  friend class msf_core::MSF_Core<
      GenericState_T<StateSequence_T, StateDefinition_T> >;
  friend struct msf_core::copyNonPropagationStates<GenericState_T>;
  friend class msf_core::MSF_InitMeasurement<
      GenericState_T<StateSequence_T, StateDefinition_T> >;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  enum {
    nStateVarsAtCompileTime = boost::fusion::result_of::size < StateSequence_T
        > ::type::value,  
    nErrorStatesAtCompileTime = msf_tmp::CountStates<StateSequence_T,
        msf_tmp::CorrectionStateLengthForType>::value,  
    nStatesAtCompileTime = msf_tmp::CountStates<StateSequence_T,
        msf_tmp::StateLengthForType>::value,  
    nCoreStatesAtCompileTime = msf_tmp::CountStates<StateSequence_T,
        msf_tmp::CoreStateLengthForType>::value,  
    nPropagatedCoreStatesAtCompileTime = msf_tmp::CountStates<StateSequence_T,
        msf_tmp::PropagatedCoreStateLengthForType>::value,  
    nPropagatedCoreErrorStatesAtCompileTime = msf_tmp::CountStates<
        StateSequence_T, msf_tmp::PropagatedCoreErrorStateLengthForType>::value  
  };

 private:

  template<int INDEX>
  inline typename boost::fusion::result_of::at_c<StateSequence_T, INDEX>::type
  getStateVar();

  template<int INDEX>
  inline typename msf_tmp::StripReference<
      typename boost::fusion::result_of::at_c<StateSequence_T, INDEX>::type>::result_t::value_t&
  get();

 public:

  typedef Eigen::Matrix<double, nErrorStatesAtCompileTime,
      nErrorStatesAtCompileTime> P_type;  
                                          // covariance matrix.
  typedef P_type F_type;
  typedef P_type Q_type;

  StateSequence_T statevars;  

  // system inputs
  Eigen::Matrix<double, 3, 1> w_m;         
  Eigen::Matrix<double, 3, 1> a_m;         

  double time;  
  P_type P;  
  F_type Fd;   
  Q_type Qd;   

  GenericState_T() {
    time = -1;
    P.setZero();
    Qd.setZero();
    Fd.setIdentity();
    reset();
  }

  inline void correct(
      const Eigen::Matrix<double, nErrorStatesAtCompileTime, 1>& correction);

  template<int INDEX>
  inline typename msf_tmp::StripReference<
      typename boost::fusion::result_of::at_c<StateSequence_T, INDEX>::type>::result_t::Q_T&
  getQBlock();

  template<int INDEX>
  inline const typename msf_tmp::StripReference<
      typename boost::fusion::result_of::at_c<StateSequence_T, INDEX>::type>::result_t::Q_T&
  getQBlock() const;

  void reset(
      msf_core::StateVisitor<GenericState_T<StateSequence_T, StateDefinition_T> >* usercalc =
          nullptr);

  void getPoseCovariance(
      geometry_msgs::PoseWithCovariance::_covariance_type & cov);

  void toPoseMsg(geometry_msgs::PoseWithCovarianceStamped & pose);

  void toExtStateMsg(sensor_fusion_comm::ExtState & state);

  /***
   * \brief Assemble a DoubleArrayStamped message from the state.
   * \note It does not set the header.
   */
  void toFullStateMsg(sensor_fusion_comm::DoubleArrayStamped & state);

  void toCoreStateMsg(sensor_fusion_comm::DoubleArrayStamped & state);

  Eigen::Matrix<double, nCoreStatesAtCompileTime, 1> toEigenVector();

  void calculateIndicesInErrorState(
      std::vector<std::tuple<int, int, int> >& vec);

  std::string print();

  bool checkStateForNumeric();

  template<int INDEX>
  inline const typename msf_tmp::StripReference<
      typename boost::fusion::result_of::at_c<StateSequence_T, INDEX>::type>::result_t::value_t&
  get() const;

  template<int INDEX>
  inline typename msf_tmp::AddConstReference<
      typename boost::fusion::result_of::at_c<StateSequence_T, INDEX>::type>::result_t
  getStateVar() const;

  template<int INDEX>
  inline void
  set(const typename msf_tmp::StripConstReference<
      typename boost::fusion::result_of::at_c<StateSequence_T, INDEX>::type>::result_t::value_t& newvalue);

  /*
   * Clears the crosscovariance entries of a given state in P
   */
  template<int INDEX>
  inline void
  clearCrossCov();

};

template<typename stateSequence_T, typename stateDefinition_T>
class sortStates {
 public:
  bool operator()(
      const GenericState_T<stateSequence_T, stateDefinition_T>& lhs,
      const GenericState_T<stateSequence_T, stateDefinition_T>&rhs) const {
    return (lhs.time_ < rhs.time_);
  }
};

}

#include <msf_core/implementation/msf_state_.hpp>

#endif /* MSF_STATE_H_ */