norm.hpp

Go to the documentation of this file.

#pragma once
 
#include <cpp-toolbox/pcl/norm/base_norm.hpp>
#include <cpp-toolbox/pcl/norm/pca_norm.hpp>
 
namespace toolbox::pcl
{
 
template<typename T>
inline auto create_normal_extractor() {
    return std::make_unique<pca_norm_extractor_t<T>>();
}
 
template<typename T>
void orient_normals_towards_viewpoint(
    toolbox::types::point_cloud_t<T>& normals,
    const point_t<T>& viewpoint,
    const toolbox::types::point_cloud_t<T>& cloud)
{
    for (size_t i = 0; i < normals.size(); ++i) {
        // 计算从点到视点的向量 / Compute vector from point to viewpoint
        T vx = viewpoint.x - cloud.points[i].x;
        T vy = viewpoint.y - cloud.points[i].y;
        T vz = viewpoint.z - cloud.points[i].z;
        
        // 计算点积 / Compute dot product
        T dot = normals.points[i].x * vx + 
                normals.points[i].y * vy + 
                normals.points[i].z * vz;
        
        // 如果法向量背离视点，则翻转 / If normal points away from viewpoint, flip it
        if (dot < 0) {
            normals.points[i].x = -normals.points[i].x;
            normals.points[i].y = -normals.points[i].y;
            normals.points[i].z = -normals.points[i].z;
        }
    }
}
 
template<typename T>
double validate_normals(const toolbox::types::point_cloud_t<T>& normals) {
    size_t valid_count = 0;
    const T epsilon = std::numeric_limits<T>::epsilon();
    
    for (const auto& normal : normals.points) {
        T length = std::sqrt(normal.x * normal.x + 
                            normal.y * normal.y + 
                            normal.z * normal.z);
        
        // 检查是否为单位向量且不包含NaN / Check if unit vector and not NaN
        if (std::abs(length - T(1)) < T(0.01) && 
            !std::isnan(normal.x) && 
            !std::isnan(normal.y) && 
            !std::isnan(normal.z)) {
            valid_count++;
        }
    }
    
    return static_cast<double>(valid_count) / normals.size();
}
 
 
}  // namespace toolbox::pcl