Merge pull request #62 from cggos/develop

Develop

include/common/state.hpp

@@ -29,6 +29,8 @@ class State {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
+  using Ptr = std::shared_ptr<State>;
+
   // error-state
   MatrixSD cov;
 
@@ -245,6 +247,5 @@ class State {
     return 2.0 * q_res.vec() / q_res.w();
   }
 };
-using StatePtr = std::shared_ptr<State>;
 
 }  // namespace cg

include/estimator/ekf.hpp

@@ -7,30 +7,19 @@ namespace cg {
 
 class EKF : public KF {
  public:
-  StatePtr state_ptr_i_;  // for IEKF
-
-  EKF() = delete;
+  EKF() = default;
 
   EKF(const EKF &) = delete;
 
-  explicit EKF(double acc_n = 1e-2, double gyr_n = 1e-4, double acc_w = 1e-6, double gyr_w = 1e-8)
-      : KF(acc_n, gyr_n, acc_w, gyr_w) {
+  explicit EKF(double sigma_p, double sigma_v, double sigma_rp, double sigma_yaw, double sigma_ba, double sigma_bg) {
     state_ptr_i_ = std::make_shared<State>();
+
+    state_ptr_->set_cov(sigma_p, sigma_v, sigma_rp, sigma_yaw, sigma_ba, sigma_bg);
   }
 
-  /**
-   * @brief predict procedure
-   * @param last_imu
-   * @param curr_imu
-   */
-  void predict(ImuDataConstPtr last_imu, ImuDataConstPtr curr_imu) {
-    State last_state = *state_ptr_;
+  virtual ~EKF() {}
 
-    state_ptr_->timestamp = curr_imu->timestamp;
-
-    imu_model_.propagate_state(last_imu, curr_imu, last_state, *state_ptr_);
-    imu_model_.propagate_state_cov(last_imu, curr_imu, last_state, *state_ptr_);
-  }
+  virtual void predict(Predictor::Data::ConstPtr data_ptr) { predictor_ptr_->process(data_ptr); }
 
   template <class H_type, class R_type, class K_type>
   void update_K(const Eigen::MatrixBase<H_type> &H, const Eigen::MatrixBase<R_type> &R, Eigen::MatrixBase<K_type> &K) {
@@ -48,7 +37,8 @@ class EKF : public KF {
     state_ptr_->cov = I_KH * state_ptr_->cov * I_KH.transpose() + K * R * K.transpose();
   }
 
-  ~EKF() {}
+ public:
+  State::Ptr state_ptr_i_;  // for IEKF
 };
 
 using EKFPtr = std::unique_ptr<EKF>;

include/estimator/estimator.hpp

@@ -9,7 +9,7 @@ class StateEstimator {
   StateEstimator() { state_ptr_ = std::make_shared<State>(); }
 
  public:
-  StatePtr state_ptr_;
+  State::Ptr state_ptr_;
 };
 
 }  // namespace cg

include/estimator/kf.hpp

@@ -1,24 +1,25 @@
 #pragma once
 
 #include "estimator/estimator.hpp"
+#include "fusion/observer.hpp"
 #include "fusion/predictor.hpp"
-#include "fusion/updator.hpp"
 
 namespace cg {
 
-class KF : public StateEstimator, public Predictor, public Updator {
+class KF : public StateEstimator {
  public:
-  KF() {}
+  KF() = default;
 
-  KF(double acc_n, double gyr_n, double acc_w, double gyr_w) : Predictor(state_ptr_, acc_n, gyr_n, acc_w, gyr_w) {}
-
-  virtual void predict(ImuDataConstPtr last_imu, ImuDataConstPtr curr_imu) = 0;
+  virtual void predict(Predictor::Data::ConstPtr data_ptr) = 0;
 
   // virtual void update() = 0;
 
   virtual ~KF() {}
 
  public:
+  Predictor::Ptr predictor_ptr_;
+  Observer::Ptr observer_ptr_;
+
   Eigen::MatrixXd measurement_cov_;
   Eigen::MatrixXd measurement_noise_cov_;
 };

include/estimator/map.hpp

@@ -2,26 +2,23 @@
 
 #include "estimator/estimator.hpp"
 #include "fusion/predictor.hpp"
-#include "fusion/updator.hpp"
 
 namespace cg {
 
-class MAP : public StateEstimator, public Predictor, public Updator {
+class MAP : public StateEstimator {
  public:
-  MAP(double acc_n = 1e-2, double gyr_n = 1e-4, double acc_w = 1e-6, double gyr_w = 1e-8)
-      : Predictor(state_ptr_, acc_n, gyr_n, acc_w, gyr_w) {}
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  void predict(ImuDataConstPtr last_imu, ImuDataConstPtr curr_imu) {
-    State last_state = *state_ptr_;
+  using Ptr = std::shared_ptr<MAP>;
 
-    state_ptr_->timestamp = curr_imu->timestamp;
+  MAP() = default;
 
-    imu_model_.propagate_state(last_imu, curr_imu, last_state, *state_ptr_);
-  }
+  void predict(Predictor::Data::ConstPtr data_ptr) { predictor_ptr_->process(data_ptr); }
 
-  ~MAP() {}
-};
+  virtual ~MAP() {}
 
-using MAPPtr = std::shared_ptr<MAP>;
+ public:
+  Predictor::Ptr predictor_ptr_;
+};
 
 }  // namespace cg

include/estimator/map_cs.hpp

@@ -54,7 +54,7 @@ class MAPCostFunctor : public ceres::SizedCostFunction<6, 3, 4> {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  MAPCostFunctor(const FactorPtr& factor_odom6dof_ptr,
+  MAPCostFunctor(const Factor::Ptr& factor_odom6dof_ptr,
                  const Eigen::Isometry3d& Tcb,
                  const Eigen::Isometry3d& Tvw,
                  const Eigen::Isometry3d& TvoB,
@@ -110,7 +110,7 @@ class MAPCostFunctor : public ceres::SizedCostFunction<6, 3, 4> {
   Eigen::Isometry3d Tvo_obs_;
   Eigen::Matrix<double, kMeasDim, kMeasDim> Lt_;
 
-  FactorPtr factor_odom6dof_ptr_;
+  Factor::Ptr factor_odom6dof_ptr_;
 };
 
 }  // namespace cg

include/estimator/map_g2o.hpp

@@ -36,7 +36,7 @@ class EdgePose : public g2o::BaseUnaryEdge<6, Eigen::Isometry3d, VertexPose> {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  EdgePose(const FactorPtr& factor_odom6dof_ptr) : BaseUnaryEdge(), factor_odom6dof_ptr_(factor_odom6dof_ptr) {}
+  EdgePose(const Factor::Ptr& factor_odom6dof_ptr) : BaseUnaryEdge(), factor_odom6dof_ptr_(factor_odom6dof_ptr) {}
 
   virtual bool read(std::istream& in) {}
 
@@ -75,7 +75,7 @@ class EdgePose : public g2o::BaseUnaryEdge<6, Eigen::Isometry3d, VertexPose> {
   const float th_huber_ = 0.8 * std::sqrt(12.592);  // chi-square P for 6DoF
 
  private:
-  FactorPtr factor_odom6dof_ptr_;
+  Factor::Ptr factor_odom6dof_ptr_;
 };
 
 }  // namespace cg

include/estimator/ukf.hpp

@@ -3,6 +3,7 @@
 #include <boost/math/distributions/chi_squared.hpp>
 
 #include "estimator/kf.hpp"
+#include "sensor/imu.hpp"
 #include "sensor/odom_6dof.hpp"
 
 namespace cg {
@@ -11,12 +12,13 @@ constexpr int kStateDimAug = kStateDim + kNoiseDim;
 
 class UKF : public KF {
  public:
-  UKF() = delete;
+  UKF() = default;
 
   UKF(const UKF &) = delete;
 
-  explicit UKF(double acc_n = 1e-2, double gyr_n = 1e-4, double acc_w = 1e-6, double gyr_w = 1e-8)
-      : KF(acc_n, gyr_n, acc_w, gyr_w) {
+  explicit UKF(double sigma_p, double sigma_v, double sigma_rp, double sigma_yaw, double sigma_ba, double sigma_bg) {
+    state_ptr_->set_cov(sigma_p, sigma_v, sigma_rp, sigma_yaw, sigma_ba, sigma_bg);
+
     sigma_points_num_ = is_Q_aug_ ? 2 * kStateDimAug + 1 : 2 * kStateDim + 1;
 
     double weight_m0, weight_c0, weight_i;
@@ -34,15 +36,27 @@ class UKF : public KF {
     }
   }
 
-  void predict(ImuDataConstPtr last_imu, ImuDataConstPtr curr_imu) {
-    const double dt = curr_imu->timestamp - last_imu->timestamp;
+  virtual ~UKF() {}
+
+  virtual void predict(Predictor::Data::ConstPtr data_ptr) {
+    predictor_ptr_->process(data_ptr, std::bind(&UKF::predict_imu, this, std::placeholders::_1, std::placeholders::_2));
+  }
+
+  void predict_imu(Predictor::Data::ConstPtr last_ptr, Predictor::Data::ConstPtr curr_ptr) {
+    if (!last_ptr || !curr_ptr) {
+      return;
+    }
+
+    const double dt = curr_ptr->timestamp_ - last_ptr->timestamp_;
 
-    state_ptr_->timestamp = curr_imu->timestamp;
+    state_ptr_->timestamp = curr_ptr->timestamp_;
 
     const Eigen::MatrixXd &sp_mat0 = generate_sigma_points(dt);
 
-    predict_sigma_points(last_imu, curr_imu, sp_mat0);
+    auto last_imu = std::make_shared<ImuData>(last_ptr->timestamp_, last_ptr->data_.head(3), last_ptr->data_.tail(3));
+    auto curr_imu = std::make_shared<ImuData>(curr_ptr->timestamp_, curr_ptr->data_.head(3), curr_ptr->data_.tail(3));
 
+    predict_sigma_points(last_imu, curr_imu, sp_mat0);
     predict_sigma_points_mean_cov(last_imu, curr_imu);
   }
 
@@ -79,8 +93,6 @@ class UKF : public KF {
     state_ptr_->cov = predicted_P;
   }
 
-  virtual ~UKF() {}
-
  private:
   void compute_scale_weights(double &scale, double &weight_m0, double &weight_c0, double &weight_i) {
     int N = is_Q_aug_ ? kStateDimAug : kStateDim;
@@ -113,7 +125,7 @@ class UKF : public KF {
     x0.head(kStateDim) = state_ptr_->vec();
 
     P0.topLeftCorner(kStateDim, kStateDim) = state_ptr_->cov;
-    if (is_Q_aug_) P0.bottomRightCorner(kNoiseDim, kNoiseDim) = imu_model_.noise_cov(dt);  // TODO: Q with dt or not ?
+    if (is_Q_aug_) P0.bottomRightCorner(kNoiseDim, kNoiseDim) = imu_model_->noise_cov(dt);  // TODO: Q with dt or not ?
 
     Eigen::MatrixXd L;
     if (!is_SVD_P_)
@@ -137,22 +149,22 @@ class UKF : public KF {
     return sp_mat0;
   }
 
-  void predict_sigma_points(ImuDataConstPtr last_imu, ImuDataConstPtr curr_imu, const Eigen::MatrixXd &sp_mat0) {
+  void predict_sigma_points(ImuData::ConstPtr last_imu, ImuData::ConstPtr curr_imu, const Eigen::MatrixXd &sp_mat0) {
     predicted_sp_mat_ = Eigen::MatrixXd::Zero(kStateDim, sigma_points_num_);
     for (int i = 0; i < sigma_points_num_; i++) {
       const Eigen::VectorXd &sp = sp_mat0.col(i);
       State last_state, state;
       last_state.from_vec(sp.head(kStateDim));
       if (is_Q_aug_) {
         const Eigen::VectorXd &noise_vec = sp.segment<kNoiseDim>(kStateDim);
-        imu_model_.propagate_state(last_imu, curr_imu, last_state, state, true, false, noise_vec);
+        imu_model_->propagate_state(last_imu, curr_imu, last_state, state, true, false, noise_vec);
       } else
-        imu_model_.propagate_state(last_imu, curr_imu, last_state, state, true, true);
+        imu_model_->propagate_state(last_imu, curr_imu, last_state, state, true, true);
       predicted_sp_mat_.col(i) = state.vec();
     }
   }
 
-  void predict_sigma_points_mean_cov(ImuDataConstPtr last_imu, ImuDataConstPtr curr_imu) {
+  void predict_sigma_points_mean_cov(ImuData::ConstPtr last_imu, ImuData::ConstPtr curr_imu) {
     // predict sigma points mean
     Eigen::VectorXd predicted_x = Eigen::VectorXd::Zero(kStateDim);
     for (int c = 0; c < sigma_points_num_; c++) {
@@ -168,13 +180,18 @@ class UKF : public KF {
         dx = predicted_sp_mat_.col(c) - predicted_x;
         predicted_P += weights_c_[c] * dx * dx.transpose();
       }
-      const double dt = curr_imu->timestamp - last_imu->timestamp;
-      state_ptr_->cov = predicted_P + imu_model_.noise_cov_discret_time(dt);  // will diverge if not add Q
+      const double dt = curr_imu->timestamp_ - last_imu->timestamp_;
+      state_ptr_->cov = predicted_P + imu_model_->noise_cov_discret_time(dt);  // will diverge if not add Q
     } else
-      imu_model_.propagate_state_cov(last_imu, curr_imu, *state_ptr_, *state_ptr_);
+      imu_model_->propagate_state_cov(last_imu, curr_imu, *state_ptr_, *state_ptr_);
   }
 
   void predict_measurement_sigma_points(const Eigen::Isometry3d &Tvw, const Eigen::Isometry3d &Tcb) {
+    if (predicted_sp_mat_.size() == 0) {
+      std::cerr << __FUNCTION__ << ": Empty predicted_sp_mat_ !!!" << std::endl;
+      return;
+    }
+
     Eigen::Isometry3d Twb;
     predicted_meas_sp_mat_ = Eigen::MatrixXd::Zero(kMeasDim, sigma_points_num_);
     for (int i = 0; i < sigma_points_num_; i++) {
@@ -231,6 +248,9 @@ class UKF : public KF {
       return true;
   }
 
+ public:
+  IMU::Ptr imu_model_;
+
  private:
   bool is_Q_aug_ = true;   // P  <--  [P Q]  or  P + Q
   bool is_SVD_P_ = false;  // LLT or SVD for P0 (for not-PSD cov matrix)

include/fusion/factor.hpp

@@ -6,6 +6,10 @@ namespace cg {
 
 class Factor {
  public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  using Ptr = std::shared_ptr<Factor>;
+
   Factor() = default;
 
   Factor(const Factor &) = delete;
@@ -18,6 +22,5 @@ class Factor {
 
   virtual ~Factor() {}
 };
-using FactorPtr = std::shared_ptr<Factor>;
 
 }  // namespace cg

include/fusion/observer.hpp

@@ -10,6 +10,10 @@ enum JACOBIAN_MEASUREMENT { HX_X, NEGATIVE_RX_X };  // h(x)/delta X, -r(x)/delta
 
 class Observer : public Factor {
  public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  using Ptr = std::shared_ptr<Observer>;
+
   Observer() = default;
 
   Observer(const Observer &) = delete;
@@ -18,6 +22,5 @@ class Observer : public Factor {
 
   virtual Eigen::MatrixXd measurement_function(const Eigen::MatrixXd &mat_x) = 0;
 };
-using ObserverPtr = std::shared_ptr<Observer>;
 
 }  // namespace cg