using namespace Eigen

2D&3D融合 截止目前为止，我们学习了机器人学，学习了2D和3D视觉算法。我们也学习了2D相机(图像数据的来源)和3D相机(点云数据的来源)工作原理。 实际上，我们最终要做的，是一个手眼机器人系统。在这个系统里，相机与机器人构成了两个非常关键的部分，它们之间需要密切配合，因此，它们之间的关系，也就非常重要。确定相机与机器人之间的关系，这是手眼标定要解决的问题。 另一方面，在很多场合，为了增强算法的鲁棒性，我们通常同时使用图像数据与点云数据，这又涉及到2D与3D配准的问题。 方式一： 通过双重循环遍历 /** * 将彩色图和深度图合并成点云 * @param matrix 相机内参矩阵3x3 * @param rgb 彩色图 * @param depth 深度图 * @param cloud 输出点云 */ static void convert(Mat &matrix, Mat &rgb, Mat &depth, PointCloud::Ptr &cloud) { double camera_fx = matrix.at<double>(0, 0); double...

前言 在项目中用到Eigen相关运算，并在此纪录一下Eigen的学习和使用情况。 一、Eigen库常用函数 #include <Eigen/Eigen> #include <Eigen/Dense> x.size() C.rows() C.cols() x(i) C(i,j) A << 1, 2, 3, 4, 5, 6, 7, 8, 9; B << A, A, A; A.fill(10); MatrixXd::Identity(rows,cols) C.setIdentity(rows,cols) MatrixXd::Zero(rows,cols) C.setZero(rows,cols) MatrixXd::Ones(rows,cols) C.setOnes(rows,cols) MatrixXd::Random(rows,cols) C.setRandom(rows,cols) VectorXd::LinSpaced(size,low,high) v.setLinSpaced(size,low,high)...

ICP（Iterative Closest Point）细化是一种点云配准算法，用于将两个或多个点云数据集对齐，以便进行后续的三维重建、拓扑分析等操作。在ICP细化中，通过迭代计算最小化两个点云之间的距离，来优化一个点云到另一个点云的转换矩阵（旋转矩阵和平移向量）。通过反复迭代，ICP细化算法可以逐步地将两个点云对齐，使它们的误差越来越小，最终达到一个较好的配准效果。ICP细化算法常用于三维重建、医学图像处理、机器人视觉、自动驾驶等领域。 ICP细化算法的具体步骤如下： 初始化：设定目标点云$P$和源点云$Q$，以及初步的变换矩阵$T_{0}$。 配准迭代：对于每一次迭代$i=1,2,3,...$，都执行以下步骤： a. 对于每个点$p$在目标点云中，找到其在源点云中最近的点$q$。 b. 计算每对匹配点之间的偏移量（即源点云中的点$q$到目标点云中的点$p$之间的向量$T_{i-1}q-p$）。 c. 通过最小二乘法估计一个变换矩阵$T_{i}$，将源点云中的点移动和旋转，以最小化偏移量的平方和。 d.