commit db25b6bb2845d7389d86824b5221ade1c6538bb3 Author: duongtd Date: Tue Mar 17 14:45:48 2026 +0700 first commit diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..66d5839 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,142 @@ +cmake_minimum_required(VERSION 3.0.2) +project(laser_filter VERSION 1.0.0 LANGUAGES CXX) + +if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) + set(BUILDING_WITH_CATKIN TRUE) + message(STATUS "Building laser_filter with Catkin") + +else() + set(BUILDING_WITH_CATKIN FALSE) + message(STATUS "Building laser_filter with Standalone CMake") +endif() + +# C++ Standard - must be set before find_package +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +if (NOT BUILDING_WITH_CATKIN) + + # Enable Position Independent Code + set(CMAKE_POSITION_INDEPENDENT_CODE ON) + + # Cấu hình RPATH để tránh cycle trong runtime search path + set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE) + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}") + + set(PACKAGES_DIR + robot_sensor_msgs + ) + +else() + +# ======================================================== +# Catkin specific configuration +# ======================================================== + find_package(catkin REQUIRED COMPONENTS + robot_sensor_msgs + ) + + catkin_package( + INCLUDE_DIRS include + # LIBRARIES không cần vì đây là header-only library + CATKIN_DEPENDS robot_sensor_msgs + ) + + include_directories( + include + ${catkin_INCLUDE_DIRS} + ) +endif() + +# Tìm tất cả header files +file(GLOB_RECURSE HEADERS "include/laser_filter/*.h") + +# Tạo INTERFACE library (header-only) +add_library(${PROJECT_NAME} INTERFACE) + +if(BUILDING_WITH_CATKIN) + add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + + # Set include directories + target_include_directories(${PROJECT_NAME} + INTERFACE + $ + $ + ) + + # Link dependencies (header-only, chỉ cần include paths) + target_link_libraries(${PROJECT_NAME} + INTERFACE + ${catkin_LIBRARIES} + ) + +else() + + # Set include directories + target_include_directories(${PROJECT_NAME} + INTERFACE + $ + $ + ) + + # Link dependencies (header-only, chỉ cần include paths) + target_link_libraries(${PROJECT_NAME} + INTERFACE + ${PACKAGES_DIR} + ) + +endif() + +if(BUILDING_WITH_CATKIN) + ## Mark libraries for installation + ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html + install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + + ## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) + +else() + + install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) + + # Export targets + install(EXPORT ${PROJECT_NAME}-targets + FILE ${PROJECT_NAME}-targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION lib/cmake/${PROJECT_NAME} + ) + + ## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) + + # Print configuration info + message(STATUS "=================================") + message(STATUS "Project: ${PROJECT_NAME} (Header-Only)") + message(STATUS "Version: ${PROJECT_VERSION}") + message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") + message(STATUS "Headers found:") + foreach(hdr ${HEADERS}) + message(STATUS " - ${hdr}") + endforeach() + message(STATUS "Dependencies: robot_sensor_msgs") + message(STATUS "=================================") +endif() diff --git a/include/laser_filter/laser_filter.h b/include/laser_filter/laser_filter.h new file mode 100644 index 0000000..9cb838e --- /dev/null +++ b/include/laser_filter/laser_filter.h @@ -0,0 +1,167 @@ +#ifndef LASER_FILTER_LASER_FILTER_H +#define LASER_FILTER_LASER_FILTER_H + +#include +#include +#include + +/** + * LaserScanSOR sor; + * sor.setMeanK(10); // xét 10 điểm láng giềng gần nhất + * sor.setStddevMulThresh(1.0); // ngưỡng = mean + 1.0 * stddev + * robot_sensor_msgs::LaserScan robot_laser_scan_filter = sor.filter(robot_laser_scan); + */ + +namespace laser_filter +{ + class LaserScanSOR + { + public: + LaserScanSOR() = default; + + /// Number of nearest neighbors to use for mean-distance estimation. + void setMeanK(int k) { mean_k_ = k; } + int getMeanK() const { return mean_k_; } + + /// Outlier threshold = global_mean + std_mul * global_stddev. + void setStddevMulThresh(double mul) { std_mul_ = mul; } + double getStddevMulThresh() const { return std_mul_; } + + /** + * Window search radius = mean_k_ * search_multiplier_. + * Increase if your scan has many consecutive invalid ranges and the + * filter misses outliers near those gaps. Default 4 is safe for + * 800–900 ray scans with typical obstacle density. + */ + void setSearchMultiplier(int m) { search_multiplier_ = m; } + + /** + * If true (default), outlier ranges become NaN — scan structure preserved. + */ + void setKeepOrganized(bool keep) { keep_organized_ = keep; } + + /** + * If true, invert the filter: keep outliers, remove inliers. + */ + void setNegative(bool neg) { negative_ = neg; } + + // ----------------------------------------------------------------------- + robot_sensor_msgs::LaserScan filter(const robot_sensor_msgs::LaserScan& input) const + { + robot_sensor_msgs::LaserScan output = input; + + const int n = static_cast(input.ranges.size()); + if (n == 0) return output; + + // ------------------------------------------------------------------ + // 1. Convert valid ranges → 2-D Cartesian + // ------------------------------------------------------------------ + struct Point2D { float x = 0, y = 0; bool valid = false; }; + std::vector pts(n); + + for (int i = 0; i < n; ++i) + { + float r = input.ranges[i]; + if (std::isfinite(r) && r >= input.range_min && r <= input.range_max) + { + float angle = input.angle_min + i * input.angle_increment; + pts[i] = { r * std::cos(angle), r * std::sin(angle), true }; + } + } + + // ------------------------------------------------------------------ + // 2. Window-based neighbor search + // window_half = mean_k_ * search_multiplier_ + // For 800 rays, mean_k=10, multiplier=4 => window = ±40 indices + // ------------------------------------------------------------------ + const int window_half = mean_k_ * search_multiplier_; + + std::vector mean_dist(n, 0.f); + int valid_count = 0; + + std::vector dists; + dists.reserve(2 * window_half); + + for (int ci = 0; ci < n; ++ci) + { + if (!pts[ci].valid) continue; + + dists.clear(); + + int lo = std::max(0, ci - window_half); + int hi = std::min(n - 1, ci + window_half); + + for (int ni = lo; ni <= hi; ++ni) + { + if (ni == ci || !pts[ni].valid) continue; + float dx = pts[ci].x - pts[ni].x; + float dy = pts[ci].y - pts[ni].y; + dists.push_back(std::sqrt(dx * dx + dy * dy)); + } + + if (dists.empty()) + { + mean_dist[ci] = 0.f; + valid_count++; + continue; + } + + int k = std::min(mean_k_, static_cast(dists.size())); + std::nth_element(dists.begin(), dists.begin() + k, dists.end()); + + float sum = 0.f; + for (int j = 0; j < k; ++j) sum += dists[j]; + mean_dist[ci] = sum / static_cast(k); + valid_count++; + } + + if (valid_count < 2) return output; + + // ------------------------------------------------------------------ + // 3. Global mean & stddev over valid points only + // ------------------------------------------------------------------ + double sum = 0.0, sq_sum = 0.0; + for (int i = 0; i < n; ++i) + { + if (!pts[i].valid) continue; + double d = mean_dist[i]; + sum += d; + sq_sum += d * d; + } + + double global_mean = sum / valid_count; + double variance = (sq_sum - sum * sum / valid_count) + / (valid_count - 1); + double stddev = std::sqrt(variance); + double threshold = global_mean + std_mul_ * stddev; + + // ------------------------------------------------------------------ + // 4. Mark outliers as NaN + // ------------------------------------------------------------------ + constexpr float NaN = std::numeric_limits::quiet_NaN(); + + for (int i = 0; i < n; ++i) + { + if (!pts[i].valid) continue; + + bool is_outlier = negative_ + ? (mean_dist[i] <= threshold) + : (mean_dist[i] > threshold); + + if (is_outlier) + output.ranges[i] = NaN; + } + + return output; + } + + private: + int mean_k_ = 10; + double std_mul_ = 1.0; + int search_multiplier_ = 4; + bool keep_organized_ = true; + bool negative_ = false; + }; +} + +#endif // LASER_FILTER_LASER_FILTER_H \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..c71ee7a --- /dev/null +++ b/package.xml @@ -0,0 +1,25 @@ + + laser_filter + 0.7.10 + + laser_filter is the second generation of the transform library, which lets + the user keep track of multiple coordinate frames over time. laser_filter + maintains the relationship between coordinate frames in a tree + structure buffered in time, and lets the user transform points, + vectors, etc between any two coordinate frames at any desired + point in time. + + Tully Foote + Eitan Marder-Eppstein + Wim Meeussen + Tully Foote + BSD + + http://www.ros.org/wiki/laser_filter + + catkin + + robot_sensor_msgs + robot_sensor_msgs + + \ No newline at end of file