correspondence.hpp

Go to the documentation of this file.

#pragma once
 
#include <cpp-toolbox/pcl/correspondence/base_correspondence_generator.hpp>
#include <cpp-toolbox/pcl/correspondence/knn_correspondence_generator.hpp>
#include <cpp-toolbox/pcl/correspondence/brute_force_correspondence_generator.hpp>
#include <cpp-toolbox/logger/thread_logger.hpp>
#include <memory>
#include <vector>
 
// Logger macros
#define LOG_INFO_S toolbox::logger::thread_logger_t::instance().info_s()
 
namespace toolbox::pcl
{
 
template<typename DataType, typename Signature, typename KNN>
std::vector<correspondence_t> generate_correspondences_knn(
    const std::shared_ptr<toolbox::types::point_cloud_t<DataType>>& src_cloud,
    const std::shared_ptr<std::vector<Signature>>& src_descriptors,
    const std::shared_ptr<std::vector<std::size_t>>& src_keypoints,
    const std::shared_ptr<toolbox::types::point_cloud_t<DataType>>& dst_cloud,
    const std::shared_ptr<std::vector<Signature>>& dst_descriptors,
    const std::shared_ptr<std::vector<std::size_t>>& dst_keypoints,
    float ratio = 0.8f,
    bool mutual = true)
{
    knn_correspondence_generator_t<DataType, Signature, KNN> gen;
    
    auto knn = std::make_shared<KNN>();
    gen.set_knn(knn);
    gen.set_source(src_cloud, src_descriptors, src_keypoints);
    gen.set_destination(dst_cloud, dst_descriptors, dst_keypoints);
    gen.set_ratio(ratio);
    gen.set_mutual_verification(mutual);
    
    std::vector<correspondence_t> correspondences;
    gen.compute(correspondences);
    
    return correspondences;
}
 
template<typename DataType, typename Signature>
std::vector<correspondence_t> generate_correspondences_brute_force(
    const std::shared_ptr<toolbox::types::point_cloud_t<DataType>>& src_cloud,
    const std::shared_ptr<std::vector<Signature>>& src_descriptors,
    const std::shared_ptr<std::vector<std::size_t>>& src_keypoints,
    const std::shared_ptr<toolbox::types::point_cloud_t<DataType>>& dst_cloud,
    const std::shared_ptr<std::vector<Signature>>& dst_descriptors,
    const std::shared_ptr<std::vector<std::size_t>>& dst_keypoints,
    float ratio = 0.8f,
    bool mutual = true,
    bool parallel = false)
{
    brute_force_correspondence_generator_t<DataType, Signature> gen;
    
    gen.enable_parallel(parallel);
    gen.set_source(src_cloud, src_descriptors, src_keypoints);
    gen.set_destination(dst_cloud, dst_descriptors, dst_keypoints);
    gen.set_ratio(ratio);
    gen.set_mutual_verification(mutual);
    
    std::vector<correspondence_t> correspondences;
    gen.compute(correspondences);
    
    return correspondences;
}
 
inline size_t filter_correspondences_by_distance(
    std::vector<correspondence_t>& correspondences,
    float max_distance)
{
    size_t original_size = correspondences.size();
    
    correspondences.erase(
        std::remove_if(correspondences.begin(), correspondences.end(),
                      [max_distance](const correspondence_t& corr) {
                          return corr.distance > max_distance;
                      }),
        correspondences.end());
    
    return original_size - correspondences.size();
}
 
inline std::tuple<float, float, float, float> compute_correspondence_statistics(
    const std::vector<correspondence_t>& correspondences)
{
    if (correspondences.empty()) {
        return {0.0f, 0.0f, 0.0f, 0.0f};
    }
    
    float sum = 0.0f;
    float min_dist = std::numeric_limits<float>::max();
    float max_dist = std::numeric_limits<float>::lowest();
    
    for (const auto& corr : correspondences) {
        sum += corr.distance;
        min_dist = std::min(min_dist, corr.distance);
        max_dist = std::max(max_dist, corr.distance);
    }
    
    float mean = sum / correspondences.size();
    
    float variance = 0.0f;
    for (const auto& corr : correspondences) {
        float diff = corr.distance - mean;
        variance += diff * diff;
    }
    variance /= correspondences.size();
    float std_dev = std::sqrt(variance);
    
    return {mean, std_dev, min_dist, max_dist};
}
 
inline void print_correspondences(
    const std::vector<correspondence_t>& correspondences,
    size_t max_display = 10)
{
    LOG_INFO_S << "对应关系数量 / Number of correspondences: " << correspondences.size();
    
    size_t count = std::min(max_display, correspondences.size());
    for (size_t i = 0; i < count; ++i) {
        const auto& corr = correspondences[i];
        LOG_INFO_S << "  [" << i << "] " << corr.src_idx << " <-> " << corr.dst_idx 
                   << ", 距离 / distance = " << corr.distance;
    }
    
    if (correspondences.size() > max_display) {
        LOG_INFO_S << "  ... 还有 / and " << (correspondences.size() - max_display)
                   << " 个对应关系 / more correspondences";
    }
}
 
 
}  // namespace toolbox::pcl