DuongTD/laser_filter
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
duongtd
2026-03-17 14:45:48 +07:00
commit db25b6bb28
3 changed files with 334 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

142
CMakeLists.txt Normal file
View File

@@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# 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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# 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()

167
include/laser_filter/laser_filter.h Normal file
View File

@@ -0,0 +1,167 @@
#ifndef LASER_FILTER_LASER_FILTER_H
#define LASER_FILTER_LASER_FILTER_H
#include <robot_sensor_msgs/LaserScan.h>
#include <robot/time.h>
#include <cmath>
/**
* 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<int>(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<Point2D> 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<float> mean_dist(n, 0.f);
int valid_count = 0;
std::vector<float> 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<int>(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<float>(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<float>::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

25
package.xml Normal file
View File

@@ -0,0 +1,25 @@
<package>
<name>laser_filter</name>
<version>0.7.10</version>
<description>
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.
</description>
<author>Tully Foote</author>
<author>Eitan Marder-Eppstein</author>
<author>Wim Meeussen</author>
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/laser_filter</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>robot_sensor_msgs</build_depend>
<run_depend>robot_sensor_msgs</run_depend>
</package>