ITK 点集配准svd

 itk 点集配准

fixed= transform(moving)

void PointRegistration(
    const std::vector<itk::Point<double>>& fixed,
    const std::vector<itk::Point<double>>& moving, double* err)
{vnl_matrix<double> fixed_matrix(fixed.size(), 3);
    vnl_matrix<double> moving_matrix(moving.size(), 3);

    vnl_matrix<double>* matrix_list[2] = { &fixed_matrix, &moving_matrix };
      const std::vector<itk::Point<double>>* source_list[2] = { &fixed, &moving };
    itk::Vector<double> mean_vec[2];

   for (auto i = 0; i < 2; i++) {
        auto& matrix = *matrix_list[i];
        auto& points = *source_list[i];
        auto& mean = mean_vec[i];
        mean.Fill(0);
        for (int r = 0; r < points.size(); r++) {
            mean += itk::Vector<double>(points[r].GetDataPointer());
        }
        mean /= points.size();
        for (unsigned int r = 0; r < matrix.rows(); r++) {
            auto& p = points[r];
            matrix(r, 0) = p[0] - mean[0];
            matrix(r, 1) = p[1] - mean[1];
            matrix(r, 2) =p[2] - mean[2];
        }
    }
    auto target = moving_matrix.transpose() * fixed_matrix;
    vnl_svd<double> svd(target);

    auto rotation = svd.V() * (svd.U().transpose());

    if (vnl_determinant(rotation) < 0) {
        auto v = svd.V();
        v.set_column(2, v.get_column(2) * (-1));
        rotation = v * svd.U().transpose();
    }
    itk::Matrix<double> R(rotation);
    auto T = R * (-mean_vec[1]) + mean_vec[0];
    
}

最后得到的R  T 就是配准后的结果；