O algoritmo Iterativo de Pontos Mais Próximos (ICP) é um método de otimização iterativa utilizado para registrar duas nuvens de pontos, minimizando a distância entre pontos correspondentes. Dado um conjunto de pontos de origem A e um conjunto de pontos de destino B, o objetivo do ICP é encontrar a matriz de rotação R e o vetor de translação t que transformam A para alinhar-se com B com maior precisão. Devido à sua simplicidade e alta acurácia, o ICP é amplamente empregado em registro local.

O algoritmo segue um processo iterativo que se repete até a convergência:

• Para cada ponto no conjunto A, identificar o ponto correspondente mais próximo no conjunto B.
• Calcular a transformação rígida (rotação e translação) que minimize a soma das distâncias quadradas entre os pares correspondentes.
• Aplicar a transformação ao conjunto A, obtendo um novo conjunto transformado.
• Se a distância média entre o novo conjunto transformado e B for menor que um limiar pré-definido, encerrar; caso contrário, usar o novo conjunto como entrada para a próxima iteração.

A etapa mais demorada é encontrar os pontos mais próximos. Para eficiência, estruturas como kd-tree são utilizadas para acelerar a busca. A kd-tree organiza os pontos em uma estrutura de árvore binária baseada na divisão espacial, permitindo consultas rápidas de vizinhos próximos.

Para resolver a transformação rígida, métodos baseados em Decomposição em Valores Singluares (SVD) são comuns. O procedimento envolve centralizar os conjuntos de pontos, calcular a matriz de covariância, e aplicar SVD para obter a rotação e translação ótimas.

Exemplo em C++ com Eigen

O código a seguir demonstra uma implementação simplificada do ICP usando a biblioteca Eigen para operações lineares. As variáveis e estruturas foram renomeadas para clareza.

#include <Eigen/Dense>
#include <vector>

void alignPointClouds(const std::vector<Eigen::Vector3f>& source_points,
                      const std::vector<Eigen::Vector3f>& target_points,
                      Eigen::Matrix3f& rotation_matrix,
                      Eigen::Vector3f& translation_vector) {
    int num_points = source_points.size();
    Eigen::Vector3f source_center = Eigen::Vector3f::Zero();
    Eigen::Vector3f target_center = Eigen::Vector3f::Zero();

    // Calcular centros de massa
    for (int i = 0; i < num_points; ++i) {
        source_center += source_points[i];
        target_center += target_points[i];
    }
    source_center /= num_points;
    target_center /= num_points;

    // Centralizar pontos
    std::vector<Eigen::Vector3f> centered_source(num_points), centered_target(num_points);
    for (int i = 0; i < num_points; ++i) {
        centered_source[i] = source_points[i] - source_center;
        centered_target[i] = target_points[i] - target_center;
    }

    // Construir matriz de correlação
    Eigen::Matrix3f correlation_matrix = Eigen::Matrix3f::Zero();
    for (int i = 0; i < num_points; ++i) {
        correlation_matrix += centered_target[i] * centered_source[i].transpose();
    }

    // Decomposição SVD
    Eigen::JacobiSVD<Eigen::Matrix3f> svd(correlation_matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3f U = svd.matrixU();
    Eigen::Matrix3f V = svd.matrixV();

    // Calcular rotação e translação
    rotation_matrix = V * U.transpose();
    translation_vector = target_center - rotation_matrix * source_center;
}

int main() {
    // Dados de teste simplificados
    std::vector<Eigen::Vector3f> source_pts = { {1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f} };
    std::vector<Eigen::Vector3f> target_pts = { {0.0f, -1.0f, 0.0f}, {1.0f, 0.0f, 0.0f} };

    Eigen::Matrix3f R;
    Eigen::Vector3f t;
    alignPointClouds(source_pts, target_pts, R, t);

    // Saída dos resultados
    // ...
    return 0;
}

Utilizando a Biblioteca PCL

A Point Cloud Library (PCL) fornece uma implementação otimizada do ICP. O exemplo abaixo configura e executa o algoritmo, alterando nomes de variáveis e parâmetros para ilustração.

#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>

int main() {
    // Carregar nuvens de pontos (exemplo simplificado)
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr reference_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // Preencher dados (omitido para brevidade)
    // ...

    // Configurar parâmetros do ICP
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp_solver;
    icp_solver.setInputSource(input_cloud);
    icp_solver.setInputTarget(reference_cloud);
    icp_solver.setMaxCorrespondenceDistance(0.1);
    icp_solver.setMaximumIterations(100);
    icp_solver.setTransformationEpsilon(1e-6);
    icp_solver.setEuclideanFitnessEpsilon(0.01);

    // Executar registro
    pcl::PointCloud<pcl::PointXYZ> aligned_output;
    icp_solver.align(aligned_output);

    // Verificar convergência
    if (icp_solver.hasConverged()) {
        Eigen::Matrix4f final_transformation = icp_solver.getFinalTransformation();
        // Usar a transformação...
    }

    return 0;
}

Para compilar este código, é necessário configurar o ambiente com as bibliotecas Eigen e PCL, incluindo os cabeçalhos e links adequados no sistema de build.