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

下述是拟合平面:

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);
}
相关推荐
yi.Ist13 分钟前
数据结构 —— 键值对 map
数据结构·算法
s1533516 分钟前
数据结构-顺序表-猜数字
数据结构·算法·leetcode
Coding小公仔19 分钟前
LeetCode 8. 字符串转换整数 (atoi)
算法·leetcode·职场和发展
GEEK零零七24 分钟前
Leetcode 393. UTF-8 编码验证
算法·leetcode·职场和发展·二进制运算
DoraBigHead1 小时前
小哆啦解题记——异位词界的社交网络
算法
木头左3 小时前
逻辑回归的Python实现与优化
python·算法·逻辑回归
lifallen6 小时前
Paimon LSM Tree Compaction 策略
java·大数据·数据结构·数据库·算法·lsm-tree
web_Hsir9 小时前
vue3.2 前端动态分页算法
前端·算法
地平线开发者11 小时前
征程 6M 部署 Omnidet 感知模型
算法·自动驾驶
秋说12 小时前
【PTA数据结构 | C语言版】线性表循环右移
c语言·数据结构·算法