点云算法--拟合平面和拟合直线

下述是拟合平面:

cpp 复制代码
template<class Vector3>
std::pair<Vector3, Vector3> best_plane_from_points(const std::vector<Vector3> & c)
{
	// copy coordinates to  matrix in Eigen format
	size_t num_atoms = c.size();
	Eigen::Matrix< Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic > coord(3, num_atoms);
	for (size_t i = 0; i < num_atoms; ++i) coord.col(i) = c[i];

	// calculate centroid
	Vector3 centroid(coord.row(0).mean(), coord.row(1).mean(), coord.row(2).mean());

	// subtract centroid
	coord.row(0).array() -= centroid(0); coord.row(1).array() -= centroid(1); coord.row(2).array() -= centroid(2);

	// we only need the left-singular matrix here
	//  http://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points
	auto svd = coord.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
	Vector3 plane_normal = svd.matrixU().rightCols<1>();
	return std::make_pair(centroid, plane_normal);
}

拟合直线:

cpp 复制代码
template<class Vector3>
std::pair < Vector3, Vector3 > best_line_from_points(const std::vector<Vector3> & c)
{
	// copy coordinates to  matrix in Eigen format
	size_t num_atoms = c.size();
	Eigen::Matrix< Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic > centers(num_atoms, 3);
	for (size_t i = 0; i < num_atoms; ++i) centers.row(i) = c[i];

	Vector3 origin = centers.colwise().mean();
	Eigen::MatrixXd centered = centers.rowwise() - origin.transpose();
	Eigen::MatrixXd cov = centered.adjoint() * centered;
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(cov);
	Vector3 axis = eig.eigenvectors().col(2).normalized();

	return std::make_pair(origin, axis);
}
相关推荐
是main不是漫5 小时前
【快慢指针 | LeetCode202 快乐数】鸽巢原理佐证环特性,快慢指针巧解快乐数
c语言·开发语言·c++·算法
Felven5 小时前
A. Iskander and Drawings
算法
qq_348231859 小时前
大模型算法: 反向传播
算法
电科_银尘9 小时前
【大模型算法与工程 27】训练配方:批量、学习率、精度与梯度裁剪
学习·算法·机器学习
专注仿真10 小时前
智能体训练之模仿学习:GAIL 算法详解与实战
python·算法·仿真·多智能体强化学习·afsim智能体训练
leobertlan12 小时前
痛苦系列 | DSP-04 截断时空的“窗”:FIR 滤波器设计与线性相位
算法
秋天的怀念_David13 小时前
算法一千零一夜 · 第三十五夜 · 恢复风暴
java·程序人生·算法
泯泷15 小时前
别再“裸聊”大模型了!你家的 AI 为什么总胡说八道?根子在 RAG
人工智能·算法·架构
2601_9679801116 小时前
Python中heapq模块的各种用法实例
数据结构·python·算法·应用实例·heapq模块