Clean code

gitshitou · Jul 1, 2020 · 2fc0b33 · 2fc0b33
1 parent 2f6cea0
commit 2fc0b33
Show file tree

Hide file tree

Showing 10 changed files with 6 additions and 107 deletions.
diff --git a/imu_gps_localizer/include/imu_gps_localizer/base_type.h b/imu_gps_localizer/include/imu_gps_localizer/base_type.h
@@ -21,14 +21,6 @@ struct GpsPositionData {
 };
 using GpsPositionDataPtr = std::shared_ptr<GpsPositionData>;
 
-struct GpsVelocityData {
-    double timestamp;
-
-    Eigen::Vector3d vel;
-    Eigen::Matrix3d cov;
-};
-using GpsVelocityDataPtr = std::shared_ptr<GpsVelocityData>;
-
 struct State {
     double timestamp;
 

diff --git a/imu_gps_localizer/include/imu_gps_localizer/gps_processor.h b/imu_gps_localizer/include/imu_gps_localizer/gps_processor.h
@@ -12,20 +12,13 @@ class GpsProcessor {
 
     bool UpdateStateByGpsPosition(const Eigen::Vector3d& init_lla, const GpsPositionDataPtr gps_data_ptr, State* state);
 
-    bool UpdateStateByGpsVelocity(const Eigen::Vector3d& init_lla, const GpsVelocityDataPtr gps_data_ptr, State* state);
-
 private:    
     void ComputeJacobianAndResidual(const Eigen::Vector3d& init_lla,  
                                     const GpsPositionDataPtr gps_data, 
                                     const State& state,
                                     Eigen::Matrix<double, 3, 15>* jacobian,
                                     Eigen::Vector3d* residual);
-
-    void ComputeJacobianAndResidual(const Eigen::Vector3d& init_lla,  
-                                    const GpsVelocityDataPtr gps_data, 
-                                    const State& state,
-                                    Eigen::Matrix<double, 3, 15>* jacobian,
-                                    Eigen::Vector3d* residual);
+
     const Eigen::Vector3d I_p_Gps_;  
 };
 

diff --git a/imu_gps_localizer/include/imu_gps_localizer/imu_gps_localizer.h b/imu_gps_localizer/include/imu_gps_localizer/imu_gps_localizer.h
@@ -9,8 +9,6 @@
 
 namespace ImuGpsLocalization {
 
-const double kGpsVelLimit = 3.; // m/s
-
 class ImuGpsLocalizer {
 public:
     ImuGpsLocalizer(const double acc_noise, const double gyro_noise,
@@ -21,8 +19,6 @@ class ImuGpsLocalizer {
 
     bool ProcessGpsPositionData(const GpsPositionDataPtr gps_data_ptr);
 
-    bool ProcessGpsVelocityData(const GpsVelocityDataPtr gps_vel_data_ptr);
-
 private:
     std::unique_ptr<Initializer>  initializer_;
     std::unique_ptr<ImuProcessor> imu_processor_;

diff --git a/imu_gps_localizer/include/imu_gps_localizer/initializer.h b/imu_gps_localizer/include/imu_gps_localizer/initializer.h
@@ -16,15 +16,12 @@ class Initializer {
     void AddImuData(const ImuDataPtr imu_data_ptr);
 
     bool AddGpsPositionData(const GpsPositionDataPtr gps_data_ptr, State* state);
-
-    void AddGpsVelocityData(const GpsVelocityDataPtr gps_vel_ptr);
 
 private:
     bool ComputeG_R_IFromImuData(Eigen::Matrix3d* G_R_I);
 
     Eigen::Vector3d init_I_p_Gps_;
     std::deque<ImuDataPtr> imu_buffer_;
-    GpsVelocityDataPtr latest_gps_vel_data_;
 };
 
 }  // namespace ImuGpsLocalization
diff --git a/imu_gps_localizer/include/imu_gps_localizer/utils.h b/imu_gps_localizer/include/imu_gps_localizer/utils.h
@@ -28,9 +28,9 @@ inline void ConvertENUToLLA(const Eigen::Vector3d& init_lla,
 
 inline Eigen::Matrix3d GetSkewMatrix(const Eigen::Vector3d& v) {
     Eigen::Matrix3d w;
-    w << 0.,    -v(2), v(1),
-         v(2),  0.,    -v(0),
-         -v(1), v(0),  0.;
+    w <<  0.,   -v(2),  v(1),
+          v(2),  0.,   -v(0),
+         -v(1),  v(0),  0.;
 
     return w;
 }

diff --git a/imu_gps_localizer/src/gps_processor.cpp b/imu_gps_localizer/src/gps_processor.cpp
@@ -25,25 +25,6 @@ bool GpsProcessor::UpdateStateByGpsPosition(const Eigen::Vector3d& init_lla, con
     state->cov = I_KH * P * I_KH.transpose() + K * V * K.transpose();
 }
 
-bool GpsProcessor::UpdateStateByGpsVelocity(const Eigen::Vector3d& init_lla, const GpsVelocityDataPtr gps_data_ptr, State* state) {
-    Eigen::Matrix<double, 3, 15> H;
-    Eigen::Vector3d residual;
-    ComputeJacobianAndResidual(init_lla, gps_data_ptr, *state, &H, &residual);
-    const Eigen::Matrix3d& V = gps_data_ptr->cov;
-
-    // EKF.
-    const Eigen::MatrixXd& P = state->cov;
-    const Eigen::MatrixXd K = P * H.transpose() * (H * P * H.transpose() + V).inverse();
-    const Eigen::VectorXd delta_x = K * residual;
-
-    // Add delta_x to state.
-    AddDeltaToState(delta_x, state);
-
-    // Covarance.
-    const Eigen::MatrixXd I_KH = Eigen::Matrix<double, 15, 15>::Identity() - K * H;
-    state->cov = I_KH * P * I_KH.transpose() + K * V * K.transpose();
-}
-
 void GpsProcessor::ComputeJacobianAndResidual(const Eigen::Vector3d& init_lla,  
                                               const GpsPositionDataPtr gps_data, 
                                               const State& state,
@@ -65,25 +46,6 @@ void GpsProcessor::ComputeJacobianAndResidual(const Eigen::Vector3d& init_lla,
     jacobian->block<3, 3>(0, 6)  = - G_R_I * GetSkewMatrix(I_p_Gps_);
 }
 
-void GpsProcessor::ComputeJacobianAndResidual(const Eigen::Vector3d& init_lla,  
-                                              const GpsVelocityDataPtr gps_data, 
-                                              const State& state,
-                                              Eigen::Matrix<double, 3, 15>* jacobian,
-                                              Eigen::Vector3d* residual) {
-    const Eigen::Vector3d& G_v_I   = state.G_v_I;
-    const Eigen::Matrix3d& G_R_I   = state.G_R_I;
-    const Eigen::Vector3d gyro_unbias = state.imu_data_ptr->gyro - state.gyro_bias;
-
-    // Compute residual.
-    *residual = gps_data->vel - (G_v_I + G_R_I * GetSkewMatrix(gyro_unbias) * I_p_Gps_);
-
-    // Compute jacobian.
-    jacobian->setZero();
-    jacobian->block<3, 3>(0, 3)  = Eigen::Matrix3d::Identity();
-    jacobian->block<3, 3>(0, 6)  = - G_R_I * GetSkewMatrix(GetSkewMatrix(gyro_unbias) * I_p_Gps_);
-    jacobian->block<3, 3>(0, 12)  = - G_R_I * GetSkewMatrix(I_p_Gps_);
-}
-
 void AddDeltaToState(const Eigen::Matrix<double, 15, 1>& delta_x, State* state) {
     state->G_p_I     += delta_x.block<3, 1>(0, 0);
     state->G_v_I     += delta_x.block<3, 1>(3, 0);

diff --git a/imu_gps_localizer/src/imu_gps_localizer.cpp b/imu_gps_localizer/src/imu_gps_localizer.cpp
@@ -53,19 +53,4 @@ bool ImuGpsLocalizer::ProcessGpsPositionData(const GpsPositionDataPtr gps_data_p
     return true;
 }
 
-bool ImuGpsLocalizer::ProcessGpsVelocityData(const GpsVelocityDataPtr gps_vel_data_ptr) {
-    if (gps_vel_data_ptr->vel.norm() < kGpsVelLimit) {
-        return false;
-    }
-
-    if (!initialized_) {
-        initializer_->AddGpsVelocityData(gps_vel_data_ptr);
-        return false;
-    }
-
-    gps_processor_->UpdateStateByGpsVelocity(init_lla_, gps_vel_data_ptr, &state_);
-
-    return true;
-}
-
 }  // namespace ImuGpsLocalization
diff --git a/imu_gps_localizer/src/initializer.cpp b/imu_gps_localizer/src/initializer.cpp
@@ -8,7 +8,7 @@
 namespace ImuGpsLocalization {
 
 Initializer::Initializer(const Eigen::Vector3d& init_I_p_Gps) 
-    : init_I_p_Gps_(init_I_p_Gps), latest_gps_vel_data_(nullptr) { }
+    : init_I_p_Gps_(init_I_p_Gps) { }
 
 void Initializer::AddImuData(const ImuDataPtr imu_data_ptr) {
     imu_buffer_.push_back(imu_data_ptr);
@@ -24,15 +24,9 @@ bool Initializer::AddGpsPositionData(const GpsPositionDataPtr gps_data_ptr, Stat
         return false;
     }
 
-    if (latest_gps_vel_data_ == nullptr) {
-        LOG(WARNING) << "[AddGpsPositionData]: No gps velocity data!";
-        return false;
-    }
-
     const ImuDataPtr last_imu_ptr = imu_buffer_.back();
     // TODO: synchronize all sensors.
-    if (std::abs(gps_data_ptr->timestamp - last_imu_ptr->timestamp) > 0.5 ||
-        std::abs(gps_data_ptr->timestamp - latest_gps_vel_data_->timestamp) > 0.5) {
+    if (std::abs(gps_data_ptr->timestamp - last_imu_ptr->timestamp) > 0.5) {
         LOG(ERROR) << "[AddGpsPositionData]: Gps and imu timestamps are not synchronized!";
         return false;
     }
@@ -75,10 +69,6 @@ bool Initializer::AddGpsPositionData(const GpsPositionDataPtr gps_data_ptr, Stat
     return true;
 }
 
-void Initializer::AddGpsVelocityData(const GpsVelocityDataPtr gps_vel_ptr) {
-    latest_gps_vel_data_ = gps_vel_ptr;
-}
-
 bool Initializer::ComputeG_R_IFromImuData(Eigen::Matrix3d* G_R_I) {
     // Compute mean and std of the imu buffer.
     Eigen::Vector3d sum_acc(0., 0., 0.);

diff --git a/ros_wrapper/include/localization_wrapper.h b/ros_wrapper/include/localization_wrapper.h
@@ -20,8 +20,6 @@ class LocalizationWrapper {
 
     void GpsPositionCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg_ptr);
 
-    void GpsVelocityCallback(const geometry_msgs::TwistStampedConstPtr& gps_vel_msg_ptr);
-
 private:
     void LogState(const ImuGpsLocalization::State& state);
     void LogGps(const ImuGpsLocalization::GpsPositionDataPtr gps_data);
@@ -30,7 +28,6 @@ class LocalizationWrapper {
 
     ros::Subscriber imu_sub_;
     ros::Subscriber gps_position_sub_;
-    ros::Subscriber gps_velocity_sub_;
     ros::Publisher state_pub_;
 
     std::ofstream file_state_;

diff --git a/ros_wrapper/src/localization_wrapper.cpp b/ros_wrapper/src/localization_wrapper.cpp
@@ -36,7 +36,6 @@ LocalizationWrapper::LocalizationWrapper(ros::NodeHandle& nh) {
     // Subscribe topics.
     imu_sub_ = nh.subscribe("/imu/data", 10,  &LocalizationWrapper::ImuCallback, this);
     gps_position_sub_ = nh.subscribe("/fix", 10,  &LocalizationWrapper::GpsPositionCallback, this);
-    gps_velocity_sub_ = nh.subscribe("/vel", 10,  &LocalizationWrapper::GpsVelocityCallback, this);
 
     state_pub_ = nh.advertise<nav_msgs::Path>("fused_path", 10);
 }
@@ -89,18 +88,6 @@ void LocalizationWrapper::GpsPositionCallback(const sensor_msgs::NavSatFixConstP
     LogGps(gps_data_ptr);
 }
 
-void LocalizationWrapper::GpsVelocityCallback(const geometry_msgs::TwistStampedConstPtr& gps_vel_msg_ptr) {
-    ImuGpsLocalization::GpsVelocityDataPtr gps_data_ptr = std::make_shared<ImuGpsLocalization::GpsVelocityData>();
-    gps_data_ptr->timestamp = gps_vel_msg_ptr->header.stamp.toSec();
-    gps_data_ptr->vel(0) = gps_vel_msg_ptr->twist.linear.x;
-    gps_data_ptr->vel(1) = gps_vel_msg_ptr->twist.linear.y;
-    gps_data_ptr->vel(2) = gps_vel_msg_ptr->twist.linear.z;
-    gps_data_ptr->cov = 2. * 2. * Eigen::Matrix3d::Identity();
-    gps_data_ptr->cov(2, 2) = 1e10;
-
-    imu_gps_localizer_ptr_->ProcessGpsVelocityData(gps_data_ptr);
-}
-
 void LocalizationWrapper::LogState(const ImuGpsLocalization::State& state) {
     const Eigen::Quaterniond G_q_I(state.G_R_I);
     file_state_ << std::fixed << std::setprecision(15)