Dominant View 3D-BBS

This repository provides a heuristic-driven optimization for the 3D-BBS (3D Branch-and-Bound Scan Matching) global localization algorithm. Designed for structured urban environments (Manhattan-world layouts), it extracts the dominant structural orientation from a LiDAR scan to significantly prune the rotational search space.

Methodology

• Orientation Extraction: Converts the input point cloud to polar coordinates and uses a Disjoint Set Union (DSU) clustering algorithm to find the dominant structural heading. The clustering step operates efficiently in O(n * m) time.
• Search Space Pruning: Restricts the 3D-BBS yaw search to four discrete intervals centered around the dominant direction and its perpendiculars: theta, theta + pi/2, theta + pi, and theta + 3pi/2.
• Noise Margin: Applies a 10-degree (0.1745 rad) search margin around these intervals to account for noise and minor variations in the dominant direction estimation.

Performance

Evaluated on Manhattan-world portions of the KITTI dataset, this method achieves up to a 4x speedup while maintaining strict accuracy criteria (translation error <= 2.0 m, rotational error <= 0.05 rad).

Sequence	Method	Avg. Translation Error	Avg. Rotational Error	Avg. Time
00	3D-BBS (Vanilla)	0.7616 m	0.0203 rad	7487.45 ms
00	3D-BBS (Ours)	0.7615 m	0.0203 rad	1817.95 ms
05	3D-BBS (Vanilla)	0.7849 m	0.0159 rad	3883.25 ms
05	3D-BBS (Ours)	0.7877 m	0.0157 rad	1051.75 ms

Usage

#include <dominant_view/dominant_view.hpp>

// Initialize and configure DominantView
DominantView dv;
dv.setParameters(0.01, 100.0, 0.25, 0.25);
dv.setPointCloud(filtered_cloud);

// Extract the dominant direction
auto dominant_directions = dv.getDominantDirections(1, 5);
float yaw = -std::atan2(dominant_directions[0].second, dominant_directions[0].first);

// Configure 3D-BBS with the 4 discrete intervals
std::vector<std::pair<float, float>> yaw_angles;
const int tree_count = 4;
for (int i = 0; i < tree_count; ++i) {
    float curr_yaw = yaw + i * (2 * M_PI / tree_count);
    yaw_angles.emplace_back(std::make_pair(curr_yaw - 0.1745f, curr_yaw + 0.1745f));
}

bbs3d_ptr->set_yaw_angles(yaw_angles);
bbs3d_ptr->localize(1);

CMake Integration

To use this in your own project, ensure you link against the dominant_view library as well as your existing gpu_bbs3d and PCL dependencies:

cmake_minimum_required(VERSION 3.10)
project(my_localization_project)

find_package(PCL REQUIRED)
find_package(Eigen3 REQUIRED)

add_executable(my_node main.cpp)

# Include directories
target_include_directories(my_node PRIVATE 
    ${PCL_INCLUDE_DIRS}
    ${EIGEN3_INCLUDE_DIRS}
    path/to/dominant_view/include
)

# Link libraries
target_link_libraries(my_node PRIVATE 
    ${PCL_LIBRARIES}
    dominant_view
    gpu_bbs3d
)

Acknowledgments

This project builds upon the original 3D-BBS implementation. The baseline global localization algorithm and its source code can be found at the official KOKIAOKI/3d_bbs GitHub repository.