distances.h 파일 개요

이 헤더 파일은 PCL 라이브러리에서 자주 사용되는 거리 계산 함수들을 정의합니다. 주요 기능은 다음과 같습니다:

1. 선분 간 거리 계산 (lineToLineSegment, sqrPointToLineDistance)
2. 최대 거리 선분 탐색 (getMaxSegment)
3. 유클리드 거리 계산 (euclideanDistance, squaredEuclideanDistance)

이 함수들은 포인트 클라우드 분할, 매칭, 특징 추출 등의 고급 알고리즘에 필수적입니다.

선분 간 최단거리 계산

함수 시그니처

PCL_EXPORTS void
lineToLineSegment (const Eigen::VectorXf &line_a, 
                   const Eigen::VectorXf &line_b, 
                   Eigen::Vector4f &pt1_seg, 
                   Eigen::Vector4f &pt2_seg);

기능 설명

두 3차원 직선 사이의 최단 거리 선분을 계산하고, 해당 선분의 두 끝점을 반환합니다.

매개변수:

• line_a: 첫 번째 직선의 계수 (점 + 방향), 형식은 [px, py, pz, dx, dy, dz]
• line_b: 두 번째 직선의 계수 (점 + 방향), 동일한 형식
• pt1_seg: 결과 선분의 첫 번째 끝점 (line_a 상에 위치)
• pt2_seg: 결과 선분의 두 번째 끝점 (line_b 상에 위치)

수학적 원리

공간 내 두 직선의 최단 거리 문제:

• 직선 A: P = P₀ + t·d₀
• 직선 B: Q = Q₀ + s·d₁
• 최단 거리 선분은 두 직선 모두와 수직해야 합니다

실제 사용 예제

#include <pcl/common/distances.h>
#include <Eigen/Core>
#include <iostream>

int main()
{
    // 첫 번째 직선: 원점(0,0,0)을 지나며 X축 방향으로
    Eigen::VectorXf line_a(6);
    line_a << 0.0f, 0.0f, 0.0f,  // 직線上의 점
              1.0f, 0.0f, 0.0f;  // 방향 벡터

    // 두 번째 직선: 점(0,1,1)을 지나며 Y축 방향으로
    Eigen::VectorXf line_b(6);
    line_b << 0.0f, 1.0f, 1.0f,  // 직線上의 점
              0.0f, 1.0f, 0.0f;  // 방향 벡터

    // 결과 저장용 끝점
    Eigen::Vector4f pt1_seg, pt2_seg;

    // 최단 거리 선분 계산
    pcl::lineToLineSegment(line_a, line_b, pt1_seg, pt2_seg);

    std::cout << "최단 거리 선분의 첫 번째 끝점 (line_a 상): " 
              << pt1_seg.transpose() << std::endl;
    std::cout << "최단 거리 선분의 두 번째 끝점 (line_b 상): " 
              << pt2_seg.transpose() << std::endl;
    
    // 최단 거리 계산
    float min_distance = (pt1_seg - pt2_seg).norm();
    std::cout << "두 직선 간 최단 거리: " << min_distance << std::endl;

    return 0;
}

PCL 내 응용 사례

• 직선 피팅 후 거리 평가: RANSAC 직선 피팅 시 후보 직선 모델 평가
• 선형 특징 매칭: 두 포인트 클라우드에서 추출한 선 특징 비교
• 로봇 경로 계획: 두 경로 간 최소 간격 계산

점과 직선 간 거리 계산

첫 번째 오버로드 버전

double inline
sqrPointToLineDistance (const Eigen::Vector4f &pt, 
                        const Eigen::Vector4f &line_pt, 
                        const Eigen::Vector4f &line_dir)

기능: 점과 직선 사이의 제곱 거리 계산 (방향 벡터 길이 자동 계산)

수학 공식:

D² = ||line_dir × (line_pt - pt)||² / ||line_dir||²

매개변수:

• pt: 검사할 점 (pt[3] = 0 또는 1을 확실히 설정)
• line_pt: 직선 위의 한 점 (중요: line_pt[3]은 반드시 0이어야 함)
• line_dir: 직선의 방향 벡터

두 번째 오버로드 버전 (최적화된 버전)

double inline
sqrPointToLineDistance (const Eigen::Vector4f &pt, 
                        const Eigen::Vector4f &line_pt, 
                        const Eigen::Vector4f &line_dir, 
                        const double sqr_length)

기능: 점과 직선 사이의 제곱 거리 계산 (미리 계산된 방향 벡터 제곱 길이 사용)

장점: 여러 점을 같은 직선에 대해 계산할 때 미리 sqr_length = line_dir.squaredNorm()를 계산하여 반복 계산 방지

매개변수:

• 앞의 세 개 매개변수는 위와 동일
• sqr_length: 직선 방향 벡터의 제곱 길이 (미리 계산된 값)

두 버전 비교 예제

#include <pcl/common/distances.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <iostream>
#include <chrono>

int main()
{
    // 직선 정의: 원점에서 X축 방향으로
    Eigen::Vector4f line_pt(0.0f, 0.0f, 0.0f, 0.0f);  // 4번째 요소는 0이어야 함
    Eigen::Vector4f line_dir(1.0f, 0.0f, 0.0f, 0.0f);
    
    // 테스트 점 정의
    Eigen::Vector4f test_pt(2.0f, 3.0f, 4.0f, 0.0f);
    
    // === 방법1: 기본 버전 ===
    double dist_sqr_1 = pcl::sqrPointToLineDistance(test_pt, line_pt, line_dir);
    std::cout << "점과 직선의 제곱 거리 (방법1): " << dist_sqr_1 << std::endl;
    std::cout << "점과 직선의 거리: " << std::sqrt(dist_sqr_1) << std::endl;
    
    // === 방법2: 최적화 버전 (여러 점 계산 시 사용) ===
    // 1000개의 점을 같은 직선에 대해 계산하는 시뮬레이션
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->width = 1000;
    cloud->height = 1;
    cloud->points.resize(1000);
    
    // 임의의 점 채우기
    for (auto& pt : cloud->points) {
        pt.x = rand() % 100;
        pt.y = rand() % 100;
        pt.z = rand() % 100;
    }
    
    // 방향 벡터 제곱 길이 미리 계산
    double sqr_length = line_dir.squaredNorm();
    
    // 성능 비교
    auto start1 = std::chrono::high_resolution_clock::now();
    for (const auto& pt : cloud->points) {
        Eigen::Vector4f point = pt.getVector4fMap();
        double d = pcl::sqrPointToLineDistance(point, line_pt, line_dir);
    }
    auto end1 = std::chrono::high_resolution_clock::now();
    
    auto start2 = std::chrono::high_resolution_clock::now();
    for (const auto& pt : cloud->points) {
        Eigen::Vector4f point = pt.getVector4fMap();
        double d = pcl::sqrPointToLineDistance(point, line_pt, line_dir, sqr_length);
    }
    auto end2 = std::chrono::high_resolution_clock::now();
    
    std::cout << "\n성능 비교 (1000개 점):" << std::endl;
    std::cout << "방법1 소요 시간: " 
              << std::chrono::duration_cast<std::chrono::microseconds>(end1-start1).count() 
              << " μs" << std::endl;
    std::cout << "방법2 소요 시간: " 
              << std::chrono::duration_cast<std::chrono::microseconds>(end2-start2).count() 
              << " μs" << std::endl;
    
    return 0;
}

주의사항 및 일반적인 실수

중요 경고:

// ❌ 잘못된 사용: line_pt의 4번째 요소가 0이 아님
Eigen::Vector4f line_pt(1.0f, 2.0f, 3.0f, 1.0f);  // 오류!

// ✅ 올바른 사용
Eigen::Vector4f line_pt(1.0f, 2.0f, 3.0f, 0.0f);  // 올바름

코드 주석은 명확히 말합니다: line_pt[3] = 0이 되도록 하세요. 내부 검사는 없습니다!

PCL 내 응용

이 함수들은 다음과 같은 경우에 널리 사용됩니다:

1. RANSAC 평면/직선 피팅

// pcl::SACSegmentation에서 점과 모델 간 거리 평가
// 가상 코드
for (each point in inlier_candidates) {
    double dist = sqrPointToLineDistance(point, model_line_pt, model_line_dir);
    if (dist < threshold_squared)
        inliers.push_back(point);
}

2. 모서리 검출

// 점이 모서리 선 근처인지 확인
if (sqrPointToLineDistance(query_pt, edge_line_pt, edge_dir) < tolerance)
    is_near_edge = true;

3. 포인트 클라우드 직선 투영

// 가장 가까운 점 위치 계산
Eigen::Vector4f projection = line_pt + 
    ((pt - line_pt).dot(line_dir) / line_dir.squaredNorm()) * line_dir;

최대 선분 탐색 함수 (getMaxSegment)

이 함수들은 포인트 클라우드에서 가장 멀리 떨어진 두 점을 찾는 템플릿 함수들입니다 (즉, 포인트 클라우드의 "지름").

첫 번째 오버로드 버전 (전체 포인트 클라우드 처리)

template <typename PointT> double inline
getMaxSegment (const pcl::PointCloud<PointT> &cloud, 
               PointT &pmin, PointT &pmax)

기능: 전체 포인트 클라우드에서 가장 먼 두 점 찾기

매개변수:

• cloud: 입력 포인트 클라우드
• pmin: 최대 선분의 한 끝점 출력
• pmax: 최대 선분의 다른 끝점 출력
• 반환값: 최대 선분 길이 (유클리드 거리)

두 번째 오버로드 버전 (인덱스 서브셋 처리)

template <typename PointT> double inline
getMaxSegment (const pcl::PointCloud<PointT> &cloud, 
               const Indices &indices,
               PointT &pmin, PointT &pmax)

기능: 포인트 클라우드의 특정 인덱스 서브셋에서 가장 먼 두 점 찾기

매개변수:

• cloud: 입력 포인트 클라우드
• indices: 처리할 점의 인덱스 목록
• pmin, pmax: 출력 끝점
• 반환값: 최대 선분 길이

알고리즘 원리 분석

// 알고리즘 논리 표시
max_dist = 최소값
for i in 포인트 집합:
    for j in 포인트 집합(i부터):
        dist = 점i와 점j의 거리 제곱
        if dist > max_dist:
            max_dist = dist
            i와 j의 인덱스 기록
            
return sqrt(max_dist)  // 실제 거리 반환 (제곱 거리 아님)

시간 복잡도: O(n²), n은 점의 수 공간 복잡도: O(1)

완전한 사용 예제

#include <pcl/common/distances.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <iostream>

// 예제1: 전체 포인트 클라우드 처리
void example_full_cloud()
{
    std::cout << "=== 예제1: 전체 포인트 클라우드의 최대 선분 ===" << std::endl;
    
    // 포인트 클라우드 생성
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    
    // 일부 테스트 점 추가
    cloud->points.push_back(pcl::PointXYZ(0.0f, 0.0f, 0.0f));
    cloud->points.push_back(pcl::PointXYZ(1.0f, 0.0f, 0.0f));
    cloud->points.push_back(pcl::PointXYZ(0.0f, 1.0f, 0.0f));
    cloud->points.push_back(pcl::PointXYZ(5.0f, 5.0f, 5.0f));  // 가장 먼 점
    cloud->points.push_back(pcl::PointXYZ(-3.0f, -3.0f, -3.0f)); // 또 다른 가장 먼 점
    cloud->points.push_back(pcl::PointXYZ(0.5f, 0.5f, 0.5f));
    
    cloud->width = cloud->points.size();
    cloud->height = 1;
    cloud->is_dense = true;
    
    // 최대 선분 찾기
    pcl::PointXYZ pmin, pmax;
    double max_length = pcl::getMaxSegment(*cloud, pmin, pmax);
    
    std::cout << "최대 선분 길이: " << max_length << std::endl;
    std::cout << "끝점1: (" << pmin.x << ", " << pmin.y << ", " << pmin.z << ")" << std::endl;
    std::cout << "끝점2: (" << pmax.x << ", " << pmax.y << ", " << pmax.z << ")" << std::endl;
    
    // 계산 검증
    float dx = pmax.x - pmin.x;
    float dy = pmax.y - pmin.y;
    float dz = pmax.z - pmin.z;
    double verify_dist = std::sqrt(dx*dx + dy*dy + dz*dz);
    std::cout << "검증 거리: " << verify_dist << std::endl;
}

// 예제2: 인덱스를 사용한 포인트 클라우드 서브셋 처리
void example_with_indices()
{
    std::cout << "\n=== 예제2: 인덱스를 사용한 최대 선분 ===" << std::endl;
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    
    // 더 큰 포인트 클라우드 생성
    for (int i = 0; i < 20; ++i) {
        cloud->points.push_back(pcl::PointXYZ(i * 0.1f, i * 0.1f, 0.0f));
    }
    cloud->width = cloud->points.size();
    cloud->height = 1;
    
    // 일부 점만 처리 (인덱스 5~15)
    pcl::Indices indices;
    for (int i = 5; i < 15; ++i) {
        indices.push_back(i);
    }
    
    pcl::PointXYZ pmin, pmax;
    double max_length = pcl::getMaxSegment(*cloud, indices, pmin, pmax);
    
    std::cout << "서브셋에서의 최대 선분 길이: " << max_length << std::endl;
    std::cout << "끝점1: (" << pmin.x << ", " << pmin.y << ", " << pmin.z << ")" << std::endl;
    std::cout << "끝점2: (" << pmax.x << ", " << pmax.y << ", " << pmax.z << ")" << std::endl;
}

// 예제3: 실제 응용 - 포인트 클라우드 경계 박스 대각선 계산
void example_bounding_box_diagonal()
{
    std::cout << "\n=== 예제3: 경계 박스 대각선 추정 ===" << std::endl;
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    
    // 정육면체 포인트 클라우드 시뮬레이션
    for (float x = 0; x <= 10; x += 1.0f) {
        for (float y = 0; y <= 10; y += 1.0f) {
            for (float z = 0; z <= 10; z += 1.0f) {
                cloud->points.push_back(pcl::PointXYZ(x, y, z));
            }
        }
    }
    cloud->width = cloud->points.size();
    cloud->height = 1;
    
    pcl::PointXYZ pmin, pmax;
    double diagonal = pcl::getMaxSegment(*cloud, pmin, pmax);
    
    std::cout << "포인트 클라우드 수: " << cloud->points.size() << std::endl;
    std::cout << "경계 박스 대각선 길이: " << diagonal << std::endl;
    std::cout << "이론적 대각선 길이 (√(10²+10²+10²)): " 
              << std::sqrt(10*10 + 10*10 + 10*10) << std::endl;
}

int main()
{
    example_full_cloud();
    example_with_indices();
    example_bounding_box_diagonal();
    
    return 0;
}

특별한 경우 처리

코드에는 중요한 경계 조건 처리가 있습니다:

const auto token = std::numeric_limits<std::size_t>::max();
std::size_t i_min = token, i_max = token;

// ... 검색 과정 ...

if (i_min == token || i_max == token)
    return (max_dist = std::numeric_limits<double>::min());

이 코드는 어떤 상황을 처리하나요?

• 빈 포인트 클라우드 또는 하나의 점만 있는 경우
• 모든 점이 겹치는 극단적인 경우
• 오류 표시로 최소 double 값을 반환

성능 최적화 팁

알고리즘이 O(n²)이지만 몇 가지 최적화 전략이 있습니다:

#include <pcl/common/distances.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

// 최적화 전략1: 다운샘플링 후 계산
void optimized_getMaxSegment()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    
    // 많은 점들이 있다고 가정
    // ... cloud 채우기 ...
    
    // 먼저 다운샘플링하여 점 수 감소
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(cloud);
    vg.setLeafSize(0.01f, 0.01f, 0.01f);  // 1cm 리프 크기
    vg.filter(*cloud_filtered);
    
    // 다운샘플링된 포인트 클라우드에서 계산
    pcl::PointXYZ pmin, pmax;
    double max_length = pcl::getMaxSegment(*cloud_filtered, pmin, pmax);
    
    std::cout << "원본 점 수: " << cloud->size() << std::endl;
    std::cout << "다운샘플링 후: " << cloud_filtered->size() << std::endl;
    std::cout << "최대 선분: " << max_length << std::endl;
}

// 최적화 전략2: 볼록 껍질 사용
// 최대 선분은 반드시 볼록 껍질의 정점 사이에 존재
void convex_hull_optimization()
{
    // 참고: 볼록 껍질을 먼저 계산하고, 그 정점들만으로 getMaxSegment 수행
    // 이렇게 하면 비교해야 할 점 수를 크게 줄일 수 있음
    std::cout << "참고: pcl::ConvexHull과 결합하여 더욱 최적화 가능" << std::endl;
}

실제 PCL 응용 사례

응용 사례1: 자동 스케일링/정규화

// 포인트 클라우드를 단위 큐브로 정규화
void normalize_point_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::PointXYZ pmin, pmax;
    double max_extent = pcl::getMaxSegment(*cloud, pmin, pmax);
    
    // 중심점 계산
    float cx = (pmin.x + pmax.x) / 2.0f;
    float cy = (pmin.y + pmax.y) / 2.0f;
    float cz = (pmin.z + pmax.z) / 2.0f;
    
    // 정규화
    for (auto& pt : cloud->points) {
        pt.x = (pt.x - cx) / max_extent;
        pt.y = (pt.y - cy) / max_extent;
        pt.z = (pt.z - cz) / max_extent;
    }
}

응용 사례2: 샘플링 해상도 추정

// 포인트 클라우드 크기에 따라 자동으로 샘플링 파라미터 설정
void auto_sampling_resolution(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::PointXYZ pmin, pmax;
    double extent = pcl::getMaxSegment(*cloud, pmin, pmax);
    
    // 포인트 클라우드 크기에 따라 리프 크기 설정 (예: extent의 1/100)
    float voxel_size = extent / 100.0f;
    
    std::cout << "권장 리프 크기: " << voxel_size << std::endl;
}

응용 사례3: 초기 등록 추정

// ICP 등록 전의 대략적인 스케일 추정
void estimate_scale_for_registration(
    pcl::PointCloud<pcl::PointXYZ>::Ptr source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr target)
{
    pcl::PointXYZ s_min, s_max, t_min, t_max;
    double source_extent = pcl::getMaxSegment(*source, s_min, s_max);
    double target_extent = pcl::getMaxSegment(*target, t_min, t_max);
    
    double scale_ratio = target_extent / source_extent;
    std::cout << "소스 포인트 클라우드와 타겟 포인트 클라우드의 스케일 비율: " << scale_ratio << std::endl;
    
    // ICP 거리 임계값 조정에 사용 가능
    double icp_threshold = source_extent * 0.01;  // extent의 1%
}

다른 PCL 함수들과의 연계 사용

#include <pcl/common/distances.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>

// 종합 예제: 포인트 클라우드 전처리 프로세스
void preprocessing_pipeline(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    // 1. 무게 중심 계산
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*cloud, centroid);
    
    // 2. 최대 범위 계산
    pcl::PointXYZ pmin, pmax;
    double max_extent = pcl::getMaxSegment(*cloud, pmin, pmax);
    
    // 3. 중심화 및 정규화
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << -centroid[0], -centroid[1], -centroid[2];
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*cloud, *cloud_transformed, transform);
    
    // 단위 스케일로 축소
    for (auto& pt : cloud_transformed->points) {
        pt.x /= max_extent;
        pt.y /= max_extent;
        pt.z /= max_extent;
    }
    
    std::cout << "포인트 클라우드가 중심화되고 단위 스케일로 정규화됨" << std::endl;
}

유클리드 거리 함수

제곱 유클리드 거리 (squaredEuclideanDistance)

일반 버전 (3D 점)

template<typename PointType1, typename PointType2> inline float
squaredEuclideanDistance (const PointType1& p1, const PointType2& p2)
{
    float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y, diff_z = p2.z - p1.z;
    return (diff_x*diff_x + diff_y*diff_y + diff_z*diff_z);
}

특징:

• 템플릿 함수로 서로 다른 점 타입 간 거리 계산 지원
• 3D 점의 제곱 거리 계산
• float 타입 반환

특수화 버전 (2D 점 - PointXY)

template<> inline float
squaredEuclideanDistance (const PointXY& p1, const PointXY& p2)
{
    float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y;
    return (diff_x*diff_x + diff_y*diff_y);
}

특징:

• 2D 점(PointXY)에 특화된 버전
• x와 y 방향 거리만 계산
• 템플릿 특수화(template specialization)

유클리드 거리 (euclideanDistance)

template<typename PointType1, typename PointType2> inline float
euclideanDistance (const PointType1& p1, const PointType2& p2)
{
    return (std::sqrt (squaredEuclideanDistance (p1, p2)));
}

특징:

• squaredEuclideanDistance 기반 구현
• 실제 유클리드 거리 반환 (제곱근 계산 후)
• 제곱 거리보다 성능이 약간 낮음 (sqrt 연산 포함)

완전한 사용 예제

#include <pcl/common/distances.h>
#include <pcl/point_types.h>
#include <iostream>
#include <vector>

// 예제1: 기본 거리 계산
void basic_distance_calculation()
{
    std::cout << "=== 예제1: 기본 거리 계산 ===" << std::endl;
    
    // 3D 점 거리
    pcl::PointXYZ p1(1.0f, 2.0f, 3.0f);
    pcl::PointXYZ p2(4.0f, 6.0f, 8.0f);
    
    float dist_sqr = pcl::squaredEuclideanDistance(p1, p2);
    float dist = pcl::euclideanDistance(p1, p2);
    
    std::cout << "점1: (" << p1.x << ", " << p1.y << ", " << p1.z << ")" << std::endl;
    std::cout << "점2: (" << p2.x << ", " << p2.y << ", " << p2.z << ")" << std::endl;
    std::cout << "제곱 거리: " << dist_sqr << std::endl;
    std::cout << "유클리드 거리: " << dist << std::endl;
    
    // 검증
    float dx = p2.x - p1.x;
    float dy = p2.y - p1.y;
    float dz = p2.z - p1.z;
    float verify = std::sqrt(dx*dx + dy*dy + dz*dz);
    std::cout << "검증: " << verify << std::endl;
}

// 예제2: 2D 점 거리 (PointXY 특수화 버전)
void point_xy_distance()
{
    std::cout << "\n=== 예제2: 2D 점 거리 ===" << std::endl;
    
    pcl::PointXY p1, p2;
    p1.x = 0.0f; p1.y = 0.0f;
    p2.x = 3.0f; p2.y = 4.0f;
    
    float dist_sqr = pcl::squaredEuclideanDistance(p1, p2);
    float dist = pcl::euclideanDistance(p1, p2);
    
    std::cout << "2D 점1: (" << p1.x << ", " << p1.y << ")" << std::endl;
    std::cout << "2D 점2: (" << p2.x << ", " << p2.y << ")" << std::endl;
    std::cout << "제곱 거리: " << dist_sqr << std::endl;
    std::cout << "유클리드 거리: " << dist << " (5.0이어야 함)" << std::endl;
}

// 예제3: 서로 다른 점 타입 간 거리
void mixed_point_types()
{
    std::cout << "\n=== 예제3: 혼합 점 타입 ===" << std::endl;
    
    pcl::PointXYZ p1(1.0f, 2.0f, 3.0f);
    pcl::PointXYZRGB p2;
    p2.x = 4.0f; p2.y = 5.0f; p2.z = 6.0f;
    p2.r = 255; p2.g = 0; p2.b = 0;  // 색상 정보는 거리 계산에 영향 없음
    
    // 템플릿 함수는 서로 다른 타입의 점을 처리할 수 있음
    float dist = pcl::euclideanDistance(p1, p2);
    std::cout << "PointXYZ와 PointXYZRGB 간의 거리: " << dist << std::endl;
}

// 예제4: 성능 비교 - 언제 제곱 거리를 사용할 것인가
void performance_comparison()
{
    std::cout << "\n=== 예제4: 성능 고려 ===" << std::endl;
    
    pcl::PointXYZ p1(0.0f, 0.0f, 0.0f);
    
    // 시나리오: 거리 임계값 내 점 찾기
    float threshold = 5.0f;
    float threshold_sqr = threshold * threshold;  // 25.0
    
    std::vector<pcl::PointXYZ> points;
    for (int i = 0; i < 10; ++i) {
        points.push_back(pcl::PointXYZ(i * 1.0f, i * 0.5f, i * 0.3f));
    }
    
    // ✅ 효율적인 방식: 제곱 거리 사용
    std::cout << "제곱 거리로 판단 (추천):" << std::endl;
    for (size_t i = 0; i < points.size(); ++i) {
        float dist_sqr = pcl::squaredEuclideanDistance(p1, points[i]);
        if (dist_sqr < threshold_sqr) {
            std::cout << "  점" << i << " 임계값 내, 제곱 거리=" << dist_sqr << std::endl;
        }
    }
    
    // ❌ 비효율적인 방식: 유클리드 거리 사용 (불필요한 sqrt)
    std::cout << "유클리드 거리로 판단 (비추천):" << std::endl;
    for (size_t i = 0; i < points.size(); ++i) {
        float dist = pcl::euclideanDistance(p1, points[i]);
        if (dist < threshold) {
            std::cout << "  점" << i << " 임계값 내, 거리=" << dist << std::endl;
        }
    }
}

int main()
{
    basic_distance_calculation();
    point_xy_distance();
    mixed_point_types();
    performance_comparison();
    
    return 0;
}

언제 제곱 거리와 유클리드 거리를 사용할 것인가

제곱 거리 사용 시나리오 (추천):

// 1. 거리 비교 (실제 거리 값 필요 없음)
if (pcl::squaredEuclideanDistance(p1, p2) < pcl::squaredEuclideanDistance(p1, p3)) {
    // p2가 p1에 더 가까움
}

// 2. 임계값 판단
float threshold = 0.1f;
float threshold_sqr = threshold * threshold;
if (pcl::squaredEuclideanDistance(query, candidate) < threshold_sqr) {
    // 임계값 내
}

// 3. 최근접 이웃 검색
float min_dist_sqr = std::numeric_limits<float>::max();
int nearest_idx = -1;
for (size_t i = 0; i < cloud->size(); ++i) {
    float d = pcl::squaredEuclideanDistance(query, cloud->points[i]);
    if (d < min_dist_sqr) {
        min_dist_sqr = d;
        nearest_idx = i;
    }
}

유클리드 거리 사용 시나리오:

// 1. 실제 거리 값 보고 필요
float actual_distance = pcl::euclideanDistance(p1, p2);
std::cout << "두 점은 " << actual_distance << " 미터 떨어져 있습니다." << std::endl;

// 2. 거리에 대한 수학적 연산 필요 (예: 가중치)
float weight = 1.0f / pcl::euclideanDistance(query, neighbor);

// 3. 시각화 또는 사용자 인터페이스에 표시