CMakeLists.txt
cmake_minimum_required(VERSION 3.5) project(ICP_SVD_example) # Set C++ standard to C++11 set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Find Eigen library find_package(Eigen3 REQUIRED) # Include directories for Eigen include_directories(${EIGEN3_INCLUDE_DIR}) # Add executable target add_executable(icp_svd_example main.cpp) # Link Eigen library target_link_libraries(icp_svd_example Eigen3::Eigen)
main.cpp
#include <iostream> #include <Eigen/Dense> #include <vector> using namespace Eigen; using namespace std; void icp_3d3d(const vector<Vector3d>& source_points, const vector<Vector3d>& target_points, double& scale, Matrix3d& R, Vector3d& t) { int N = source_points.size(); // // 计算centroid_source和centroid_target的质心 Vector3d centroid_source = Vector3d::Zero(); Vector3d centroid_target = Vector3d::Zero(); for (const auto& p : source_points) { centroid_source += p; } centroid_source /= source_points.size(); for (const auto& q : target_points) { centroid_target += q; } centroid_target /= target_points.size(); ////归一化点 每一个点去中心化 vector<Vector3d> centered_source_points; vector<Vector3d> centered_target_points; for (const auto& p : source_points) { centered_source_points.push_back(p - centroid_source); } for (const auto& q : target_points) { centered_target_points.push_back(q - centroid_target); } // 计算协方差矩阵 H Matrix3d H = Matrix3d::Zero(); for (int i = 0; i < source_points.size(); ++i) { H += centered_source_points[i] * centered_target_points[i].transpose(); } // 使用奇异值分解H SVD JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV); Matrix3d U = svd.matrixU(); Matrix3d V = svd.matrixV(); // 计算旋转矩阵R if (U.determinant() * V.determinant() < 0) { for (int x = 0; x < 3; ++x) { U(x, 2) *= -1; } } R = U* V.transpose() ; //R = V * U.transpose(); // 计算尺度因子 s double trace = 0.0; for (int i = 0; i < N; ++i) { trace += centered_target_points[i].transpose() * R * centered_source_points[i]; } double src_norm = 0.0; for (int i = 0; i < N; ++i) { src_norm += centered_source_points[i].squaredNorm(); } scale = trace / src_norm; // 计算平移向量t t = centroid_target - scale * R * centroid_source; } // 单个测试 void Test_1(){ // Example: Generating random source and target point clouds vector<Vector3d> source_points; vector<Vector3d> target_points; // Generate random source points (in this example, 4 points) source_points.push_back(Vector3d(1, 0, 0)); source_points.push_back(Vector3d(0, 1, 0)); source_points.push_back(Vector3d(0, 0, 1)); source_points.push_back(Vector3d(0, 0, 0)); // Generate random target points (in this example, 4 points) target_points.push_back(Vector3d(2, 0, 0)); target_points.push_back(Vector3d(0, 2, 0)); target_points.push_back(Vector3d(0, 0, 2)); target_points.push_back(Vector3d(0, 0, 0)); // Variables to store results double scale; Matrix3d R; Vector3d t; // Perform 3D-3D ICP icp_3d3d(source_points, target_points, scale, R, t); // Output results cout << "Scale factor s: " << scale << endl; cout << "Rotation matrix R:\n" << R << endl; cout << "Translation vector t:\n" << t << endl; } // 测试2 指定矩阵计算 void Test_2(){ // Example: Generating random source and target point clouds vector<Vector3d> source_points; vector<Vector3d> target_points; // 原始点 source_points.push_back(Vector3d(1, 0, 0)); source_points.push_back(Vector3d(0, 1, 0)); source_points.push_back(Vector3d(0, 0, 1)); source_points.push_back(Vector3d(0, 0, 0)); // 目标点 // 自己构造矩阵 Matrix3d currentR; Vector3d currentT; double currentScale=2; currentR<<1,0,0,0,1,0,0,0,1; currentT<<1,1,1; int N = source_points.size(); for(int i=0;i<N;i++){ Vector3d dst_i = currentScale * currentR * source_points[i] + currentT; target_points.push_back(dst_i); cout<< i << " "<< dst_i <<endl; } // 保存计算结果 double scale; Matrix3d R; Vector3d t; // Perform 3D-3D ICP icp_3d3d(source_points, target_points, scale, R, t); // Output results cout << "Scale factor s: " << scale << endl; cout << "Rotation matrix R:\n" << R << endl; cout << "Translation vector t:\n" << t << endl; } // 测试3 使用ransac_icp_3d3d求解 #include <random> #include <chrono> // For random seed // 变换点和目标点计算误差 double computeError(const vector<Vector3d>& source_points, const vector<Vector3d>& target_points, const double scale, const Matrix3d& R, const Vector3d& t) { // Compute transformation error using point-to-point distances double error = 0.0; int num_points = source_points.size(); for (int i = 0; i < num_points; ++i) { Vector3d transformed_point = scale * R * source_points[i] + t; error += (transformed_point - target_points[i]).norm(); } return error / num_points; // Mean error } /* 随机样本选择: 从输入数据中随机选择一个小的子集作为初始样本集合(比如选择3个点来估计初始的变换参数)。 模型拟合: 使用选定的样本集合来估计模型的参数(比如平移、旋转等变换参数)。 一致性检验: 将所有其他数据点与估计模型进行比较,计算它们与模型之间的拟合误差或距离。如果数据点与模型的拟合误差在某个阈值范围内,则认为它们是一致的(inliers)。 重复迭代: 重复上述步骤多次(通常是预先设定的次数),选择具有最大一致性(最多inliers)的模型参数作为最终的拟合结果。 ICP(Iterative Closest Point) ICP是一种迭代优化算法,用于通过最小化点云或模型之间的距离来优化它们的配准。在3D点云配准中,ICP的基本步骤如下: 初始化: 初始时,假设两个点云或模型之间已经大致对齐。 最近点匹配: 对于每个点云中的点,找到其在另一个点云中的最近邻点。 计算变换: 基于最近点匹配,计算一个优化的变换(通常是刚体变换,如平移和旋转),使得点云之间的距离最小化。 更新: 应用计算出的变换将点云或模型移动到新的位置。 重复迭代: 重复上述步骤,直到收敛到设定的收敛条件(如变换参数变化很小或距离误差很小)。 RANSAC-ICP结合 RANSAC-ICP结合了RANSAC和ICP两种技术的优势: 粗配准阶段: 利用RANSAC的随机采样和一致性检验,对初始点云或模型进行粗略的配准。这一阶段主要用于去除离群点,选择可靠的初始匹配点集。 精细优化阶段: 利用ICP的迭代优化能力,通过最小化点云或模型之间的距离,进一步优化配准结果。这一阶段的目标是通过局部优化,使得点云或模型的配准更加精确。 迭代优化: RANSAC-ICP通常以迭代方式执行,每次迭代使用RANSAC选择和ICP优化,直到达到预定义的迭代次数或收敛条件。 */ void ransac_icp_3d3d(const vector<Vector3d>& source_points, const vector<Vector3d>& target_points, const int num_iterations, const double error_threshold, double& best_scale, Matrix3d& best_R, Vector3d& best_t) { int num_points = source_points.size(); int best_inlier_count = 0; // 出初始位姿 这里用不到 double s_initial = 1.0; Matrix3d R_initial = Matrix3d::Identity(); Vector3d t_initial = Vector3d::Zero(); // 使用 std::random_device 生成真随机数种子 std::random_device rd; // 使用梅森旋转算法引擎 mt19937,以 rd() 作为种子 std::mt19937 gen(rd()); // 定义一个均匀整数分布,范围是[1, 100] //std::uniform_int_distribution<int> dist(1, 100); uniform_int_distribution<int> dist(0, num_points - 1); // 生成随机数 //int random_number = dist(gen); // RANSAC loop for (int iteration = 0; iteration < num_iterations; ++iteration) { // Randomly select 3 points to form a minimal sample vector<Vector3d> source_points_temp; vector<Vector3d> target_points_temp; for (int num=0;num<10;num++){ int idx = dist(gen); source_points_temp.push_back(source_points[idx]); target_points_temp.push_back(target_points[idx]); } // int idx1 = dist(gen); // int idx2 = dist(gen); // int idx3 = dist(gen); // // 随机抽取3个原始点和对应目标点 // Vector3d p1 = source_points[idx1]; // Vector3d p2 = source_points[idx2]; // Vector3d p3 = source_points[idx3]; // Vector3d q1 = target_points[idx1]; // Vector3d q2 = target_points[idx2]; // Vector3d q3 = target_points[idx3]; // 本次计算结果 Perform ICP with these initial parameters double scale; Matrix3d R; Vector3d t; icp_3d3d(source_points_temp,target_points_temp, scale, R, t); // Compute error with all points and count inliers int inlier_count = 0; // 使用本次随机抽取样本计算的sRt结果 ,对所有原始点变换,计算误差是否在阈值内,统计内点最多的情况,记录对应sRt for (int i = 0; i < num_points; ++i) { double error = computeError({source_points[i]}, {target_points[i]}, scale, R, t); if (error < error_threshold) { inlier_count++; } } // Update best model if this model is better if (inlier_count > best_inlier_count) { best_inlier_count = inlier_count; best_scale = scale; best_R = R; best_t = t; } } } // 测试3 void Test_3(){ //1-1 构建 原始点 = (-10,10) vector<Vector3d> source_points; vector<Vector3d> target_points; // Generate random source points (in this example, 10 points) default_random_engine generator; auto seed = chrono::high_resolution_clock::now().time_since_epoch().count(); generator.seed(seed); // Random seed initialization uniform_real_distribution<double> distribution(-0, 100.0); for (int i = 0; i < 100; ++i) { source_points.push_back(Vector3d(distribution(generator), distribution(generator), distribution(generator))); } // 目标点 std::random_device rd; std::mt19937 gen(rd()); // std::uniform_int_distribution:均匀整数分布,用于生成指定范围内的整数。 // std::uniform_real_distribution:均匀实数分布,用于生成指定范围内的实数。 // std::normal_distribution:正态分布,用于生成服从正态分布的随机数。 double error=3; std::uniform_real_distribution<int> dist(-error, error); int random_number = dist(gen); // 自己构造矩阵 Matrix3d currentR; Vector3d currentT; double currentScale=2; currentR<<1,0,0,0,1,0,0,0,1; currentT<<1,1,1; int N = source_points.size(); for(int i=0;i<N;i++){ Vector3d dst_i = currentScale * currentR * source_points[i] + currentT; Vector3d noise(dist(gen) ,dist(gen) ,dist(gen)); target_points.push_back(dst_i + noise); //target_points.push_back(dst_i ); cout<< i << "======= \n "<< noise << "\n == \n"<<dst_i <<endl; } //2-1 记录最优结果 Variables to store results double scale; Matrix3d R; Vector3d t; //2-2 开始计算 Perform RANSAC-based 3D-3D ICP // 100 迭代次数 误差阈值 1 ransac_icp_3d3d(source_points, target_points, 100, error, scale, R, t); // Output results cout << "Scale factor s: " << scale << endl; cout << "Rotation matrix R:\n" << R << endl; cout << "Translation vector t:\n" << t << endl; } int main() { // Testone(); // Test_2(); Test_3(); return 0; }
标签:const,target,Vector3d,SVD,source,int,points,ICP,3D From: https://www.cnblogs.com/gooutlook/p/18305907