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