Punto de cierre iterativo (ICP) y código de implementación de c ++

Hay dos conjuntos de puntos correspondientes (conjuntos de puntos correspondientes):

Encontrar la transformación euclidiana   hace que:

El algoritmo ICP se calcula iterativamente en función del método de mínimos cuadrados para que la suma de los errores al cuadrado alcance un valor mínimo:

Se puede resolver mediante los siguientes tres pasos:

(1) Defina el centroide de dos conjuntos de puntos, simplifique la función objetivo

 

 

La parte de término cruzado es  cero después de la suma, por lo que la función objetivo se simplifica para:

El primer término solo está relacionado con la matriz de rotación R, siempre que se obtenga R y el segundo término sea cero, se puede obtener t. 

(2) Calcule las coordenadas del centroide de cada punto y calcule la matriz de rotación R

En donde el segundo término y no tiene nada que ver R, R y el tercer elemento relacionado. Por lo tanto, la función objetivo se convierte en: 

Para resolver por Descomposición de valor singular (SVD), primero defina la matriz:

W es una matriz, W es la SVD para obtener: 

Donde  es una matriz diagonal compuesta de valores singulares. Cuando W es rango completo, R es: 

(3) Calcular la matriz de traducción t

 

 (4) slam visual 14 conferencia Código ICP:

void pose_estimation_3d3d(const vector<Point3f>& pts1,
                          const vector<Point3f>& pts2,
                          Mat& R, Mat& t)
{
    // center of mass
    Point3f p1, p2;
    int N = pts1.size();
    for (int i=0; i<N; i++)
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 /= N;
    p2 /= N;

    // subtract COM
    vector<Point3f> q1(N), q2(N);
    for (int i=0; i<N; i++)
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i=0; i<N; i++)
    {
        W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x,
                q2[i].y, q2[i].z).transpose();
    }
    cout << "W=" << W << endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    cout << "U=" << U << endl;
    cout << "V=" << V << endl;

    Eigen::Matrix3d R_ = U * (V.transpose());
    Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

    // convert to cv::Mat
    R = (Mat_<double>(3, 3) <<
            R_(0, 0), R_(0, 1), R_(0,2),
            R_(1, 0), R_(1, 1), R_(1,2),
            R_(2, 0), R_(2, 1), R_(2,2));
    t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}