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

下述是拟合平面:

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);
}
相关推荐
张人玉1 小时前
C# 常量与变量
java·算法·c#
weixin_446122462 小时前
LinkedList剖析
算法
百年孤独_3 小时前
LeetCode 算法题解:链表与二叉树相关问题 打打卡
算法·leetcode·链表
我爱C编程3 小时前
基于拓扑结构检测的LDPC稀疏校验矩阵高阶环检测算法matlab仿真
算法·matlab·矩阵·ldpc·环检测
算法_小学生3 小时前
LeetCode 75. 颜色分类(荷兰国旗问题)
算法·leetcode·职场和发展
运器1233 小时前
【一起来学AI大模型】算法核心:数组/哈希表/树/排序/动态规划(LeetCode精练)
开发语言·人工智能·python·算法·ai·散列表·ai编程
算法_小学生3 小时前
LeetCode 287. 寻找重复数(不修改数组 + O(1) 空间)
数据结构·算法·leetcode
岁忧3 小时前
(LeetCode 每日一题) 1865. 找出和为指定值的下标对 (哈希表)
java·c++·算法·leetcode·go·散列表
alphaTao3 小时前
LeetCode 每日一题 2025/6/30-2025/7/6
算法·leetcode·职场和发展
ゞ 正在缓冲99%…3 小时前
leetcode67.二进制求和
算法·leetcode·位运算