diff --git a/src/ekf-mono-slam/include/feature/cartesian_map_feature.h b/src/ekf-mono-slam/include/feature/depth_map_feature.h similarity index 85% rename from src/ekf-mono-slam/include/feature/cartesian_map_feature.h rename to src/ekf-mono-slam/include/feature/depth_map_feature.h index 3628040..2301d04 100644 --- a/src/ekf-mono-slam/include/feature/cartesian_map_feature.h +++ b/src/ekf-mono-slam/include/feature/depth_map_feature.h @@ -5,9 +5,9 @@ #include "map_feature.h" -class CartesianMapFeature final : public MapFeature { +class DepthMapFeature final : public MapFeature { public: - CartesianMapFeature( + DepthMapFeature( const Eigen::VectorXd& state, int position, const cv::Mat& descriptor_data, diff --git a/src/ekf-mono-slam/include/filter/ekf.h b/src/ekf-mono-slam/include/filter/ekf.h index 49f901d..e074d54 100644 --- a/src/ekf-mono-slam/include/filter/ekf.h +++ b/src/ekf-mono-slam/include/filter/ekf.h @@ -28,7 +28,7 @@ class EKF final { } [[nodiscard]] bool is_initilized() const { - return !state_->cartesian_features().empty() || + return !state_->depth_features().empty() || !state_->inverse_depth_features().empty(); } diff --git a/src/ekf-mono-slam/include/filter/state.h b/src/ekf-mono-slam/include/filter/state.h index 8235d7c..71f0a9f 100644 --- a/src/ekf-mono-slam/include/filter/state.h +++ b/src/ekf-mono-slam/include/filter/state.h @@ -6,13 +6,13 @@ #include #include -#include "feature/cartesian_map_feature.h" +#include "feature/depth_map_feature.h" #include "feature/image_feature_measurement.h" #include "feature/inverse_depth_map_feature.h" #include "feature/map_feature.h" class MapFeature; -class CartesianMapFeature; +class DepthMapFeature; class InverseDepthMapFeature; class CovarianceMatrix; @@ -61,15 +61,15 @@ class State final { return static_cast(inverse_depth_features_.size()); } - [[nodiscard]] int num_cartesian_features() const { - return static_cast(cartesian_features_.size()); + [[nodiscard]] int num_depth_features() const { + return static_cast(depth_features_.size()); } [[nodiscard]] int dimension() const { return dimension_; } - [[nodiscard]] const std::vector>& - cartesian_features() const { - return cartesian_features_; + [[nodiscard]] const std::vector>& + depth_features() const { + return depth_features_; } [[nodiscard]] std::vector> @@ -95,7 +95,7 @@ class State final { std::vector> features_; std::vector> inverse_depth_features_; - std::vector> cartesian_features_; + std::vector> depth_features_; int dimension_; diff --git a/src/ekf-mono-slam/src/feature/cartesian_map_feature.cpp b/src/ekf-mono-slam/src/feature/depth_map_feature.cpp similarity index 76% rename from src/ekf-mono-slam/src/feature/cartesian_map_feature.cpp rename to src/ekf-mono-slam/src/feature/depth_map_feature.cpp index 499fe36..5b46b9c 100644 --- a/src/ekf-mono-slam/src/feature/cartesian_map_feature.cpp +++ b/src/ekf-mono-slam/src/feature/depth_map_feature.cpp @@ -1,4 +1,4 @@ -#include "feature/cartesian_map_feature.h" +#include "feature/depth_map_feature.h" #include #include @@ -10,7 +10,7 @@ using CameraParameters::fx; using CameraParameters::fy; -CartesianMapFeature::CartesianMapFeature( +DepthMapFeature::DepthMapFeature( const Eigen::VectorXd& state, int position, const cv::Mat& descriptor_data, @@ -18,13 +18,13 @@ CartesianMapFeature::CartesianMapFeature( ) : MapFeature(state, position, descriptor_data, index) {} -Eigen::Vector3d CartesianMapFeature::directional_vector( +Eigen::Vector3d DepthMapFeature::directional_vector( const Eigen::Matrix3d& rotation_matrix, const Eigen::Vector3d& camera_position ) { return rotation_matrix * (state_ - camera_position); } // TODO: implement method -void CartesianMapFeature::measurement_jacobian( +void DepthMapFeature::measurement_jacobian( const State& state, const CovarianceMatrix& covariance_matrix ) {} diff --git a/src/ekf-mono-slam/src/feature/inverse_depth_map_feature.cpp b/src/ekf-mono-slam/src/feature/inverse_depth_map_feature.cpp index 41b0e1c..cd12881 100644 --- a/src/ekf-mono-slam/src/feature/inverse_depth_map_feature.cpp +++ b/src/ekf-mono-slam/src/feature/inverse_depth_map_feature.cpp @@ -86,13 +86,12 @@ void InverseDepthMapFeature::measurement_jacobian( const auto dhi_dyi = dhd_dhu * dhu_dhc * dhc_dyi; // Eq. (A.51) Eigen::Matrix2Xd dhi_dxm = Eigen::Matrix2Xd::Zero( - 2, - 6 * state.num_inverse_depth_features() + 3 * state.num_cartesian_features() + 2, 6 * state.num_inverse_depth_features() + 3 * state.num_depth_features() ); dhi_dxm.block(0, index_, 2, 6) = dhi_dyi; - // This matrix is 2x(13+6*num_inv_depth+3*num_cartesian) + // This matrix is 2x(13+6*num_inv_depth+3*num_depth) // If there's only 1 feature, it'd be 2x19 Eigen::MatrixXd dhi_dx = Eigen::MatrixXd::Zero(2, dhi_dxc.cols() + dhi_dxm.cols()); diff --git a/src/ekf-mono-slam/src/filter/state.cpp b/src/ekf-mono-slam/src/filter/state.cpp index 2c5409a..19a4579 100644 --- a/src/ekf-mono-slam/src/filter/state.cpp +++ b/src/ekf-mono-slam/src/filter/state.cpp @@ -114,12 +114,12 @@ void State::predict(const double delta_t) { * tracking. */ void State::remove(const std::shared_ptr& feature) { - if (const auto& cartesian_feature = - std::dynamic_pointer_cast(feature)) { + if (const auto& depth_feature = + std::dynamic_pointer_cast(feature)) { std::erase_if( - cartesian_features_, - [&cartesian_feature](const std::shared_ptr& f) { - return f == cartesian_feature; + depth_features_, + [&depth_feature](const std::shared_ptr& f) { + return f == depth_feature; } ); } else if (const auto& inverse_depth_feature = @@ -195,10 +195,10 @@ void State::add( * @param feature The MapFeature object to be added. */ void State::add(const std::shared_ptr& feature) { - if (const auto& cartesian_feature = - std::dynamic_pointer_cast(feature)) { - cartesian_features_.push_back(cartesian_feature); - features_.push_back(cartesian_feature); + if (const auto& depth_feature = + std::dynamic_pointer_cast(feature)) { + depth_features_.push_back(depth_feature); + features_.push_back(depth_feature); } else if (const auto& inverse_depth_feature = std::dynamic_pointer_cast(feature)) { inverse_depth_features_.push_back(inverse_depth_feature); diff --git a/src/ekf-mono-slam/test/itest_slam.cpp b/src/ekf-mono-slam/test/itest_slam.cpp index ee037ca..672fb98 100644 --- a/src/ekf-mono-slam/test/itest_slam.cpp +++ b/src/ekf-mono-slam/test/itest_slam.cpp @@ -37,7 +37,7 @@ TEST(SLAMIntegration, FindFeatureInStateAndCovariance) { ASSERT_EQ(ekf.state()->dimension(), 13 + 20 * 6); ASSERT_EQ(ekf.state()->inverse_depth_features().size(), 20); - ASSERT_EQ(ekf.state()->cartesian_features().size(), 0); + ASSERT_EQ(ekf.state()->depth_features().size(), 0); for (size_t i = 0; i < image_features.size(); i++) { ASSERT_EQ( diff --git a/src/ekf-mono-slam/test/utest_filter.cpp b/src/ekf-mono-slam/test/utest_filter.cpp index a04b453..36d1b8b 100644 --- a/src/ekf-mono-slam/test/utest_filter.cpp +++ b/src/ekf-mono-slam/test/utest_filter.cpp @@ -45,20 +45,19 @@ TEST(ExtendedKalmanFilter, AddMapFeatureToState) { std::make_shared( feature_state, 6, descriptor_data, 0 ); - const auto cartesian_map_feature = std::make_shared( - feature_state, 13, descriptor_data, 1 - ); + const auto depth_map_feature = + std::make_shared(feature_state, 13, descriptor_data, 1); state.add(inverse_depth_map_feature); - state.add(cartesian_map_feature); + state.add(depth_map_feature); const std::vector> inverse_depth_features = state.inverse_depth_features(); - const std::vector> - cartesian_map_features = state.cartesian_features(); + const std::vector> depth_map_features = + state.depth_features(); - ASSERT_THAT(cartesian_map_features, SizeIs(1)); - ASSERT_THAT(cartesian_map_features, Contains(cartesian_map_feature)); + ASSERT_THAT(depth_map_features, SizeIs(1)); + ASSERT_THAT(depth_map_features, Contains(depth_map_feature)); ASSERT_THAT(inverse_depth_features, SizeIs(1)); ASSERT_THAT(inverse_depth_features, Contains(inverse_depth_map_feature)); } @@ -74,23 +73,22 @@ TEST(ExtendedKalmanFilter, RemoveMapFeature) { const auto inverse_map_feature = std::make_shared( feature_state, 13, descriptor_data, 0 ); - const auto cartesian_map_feature = std::make_shared( - feature_state, 19, descriptor_data, 1 - ); + const auto depth_map_feature = + std::make_shared(feature_state, 19, descriptor_data, 1); state.add(inverse_map_feature); - state.add(cartesian_map_feature); + state.add(depth_map_feature); state.remove(inverse_map_feature); - state.remove(cartesian_map_feature); + state.remove(depth_map_feature); const std::vector> inverse_depth_features = state.inverse_depth_features(); - const std::vector> - cartesian_map_features = state.cartesian_features(); + const std::vector> depth_map_features = + state.depth_features(); ASSERT_THAT(inverse_depth_features, SizeIs(0)); - ASSERT_THAT(cartesian_map_features, SizeIs(0)); + ASSERT_THAT(depth_map_features, SizeIs(0)); } TEST(ExtendedKalmanFilter, AddImageFeatureMeasurement) { @@ -103,7 +101,7 @@ TEST(ExtendedKalmanFilter, AddImageFeatureMeasurement) { state.add(image_feature_measurement); ASSERT_THAT(state.inverse_depth_features().size(), Eq(1)); - ASSERT_THAT(state.cartesian_features().size(), Eq(0)); + ASSERT_THAT(state.depth_features().size(), Eq(0)); } TEST(ExtendedKalmanFilter, InitCovariance) {