将 pcl::PointCloud 转换为 Eigen::MatrixBase答案

【问题标题】：Converting pcl::PointCloud to Eigen::MatrixBase将 pcl::PointCloud 转换为 Eigen::MatrixBase
【发布时间】：2015-01-24 19:48:27
【问题描述】：

我正在尝试从 this 标头运行 ICP 算法。 line:260 处的函数声明为

template <typename Derived1, typename Derived2>
void point_to_point(Eigen::MatrixBase<Derived1>& X,
                    Eigen::MatrixBase<Derived2>& Y,
                    Parameters par = Parameters()) {

我想在我的两个 pcl::PointClouds 上应用此算法，但它接受 Eigen::MatrixBase。我尝试如下调用这个函数

Eigen::MatrixBase<Eigen::Matrix<double,3,1,Eigen::RowMajor,3,100>> X;
Eigen::MatrixBase<Eigen::Matrix<double,3,1,Eigen::RowMajor,3,100>> Y;
Parameters par = Parameters()
point_to_point(X,Y,Parameters)

它给了我这个错误

error: ‘Eigen::MatrixBase<Derived>::MatrixBase() [with Derived = Eigen::Matrix<double, 3, 1, 1, 3, 100>]’ is protected
     MatrixBase() : Base() {}

我也尝试过Eigen::MatrixBase<Eigen::Matrix4f> X;，但似乎每种类型都受到保护。具体是什么意思？

我搜索了很多，但没有找到MatrixBase 的任何示例。

如何在 Eigen::MatrixBase 中插入/删除值 - 未找到任何此类函数 here？
如何将我的 pcl::PointCloud 转换为 Eigen::MatrixBase 并反转？
如何调用这个函数？

【问题讨论】：

有一个函数PointCloud::getMatrixXfMap() 允许访问点云“后面”的特征矩阵。我还没有测试过，但我认为这个函数返回的内容应该可以隐式转换为MatrixBase。但一个问题是，在 PCL 中，点是行优先的，即点坐标在行上。您要运行的 ICP 算法要求它们按列排列。
PointCloud::getMatrixXfMap() 返回Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >。我需要将它投入什么？它不允许我采用类型并声明 MatrixBase 变量。
如何将云中的所有点一一添加到MatrixBase<xxx>，然后反向？
正如我所说，我认为编译器会隐式进行适当的强制转换。只需将 getMatrixXfMap() 返回的任何内容传递给函数即可。
嗯，它不接受映射，因为引用不是const（函数要修改其内容）。另一个问题是，在函数内部，矩阵标量类型假定为double（因为使用了VectorXd、Vector3d 等类），而PCL 云由floats 组成。考虑到这一点，我建议创建一个新的MatrixXd，用点云中的数据填充它，然后将其传递给函数。不是最优的，但似乎没有办法避免数据复制。

标签： c++ matrix eigen point-cloud-library

【解决方案1】：

您可以使用 for 循环构造矩阵。例如

Eigen::Matrix3Xd X, Y;
    for (size_t idx = 0; idx != pc_X->size(); ++idx) {
        X.col(idx) = Eigen::Vector3d{pc_X->at(idx).x, 
                    pc_X->at(idx).y, 
                    pc_X->at(idx).z};
    }
    for (size_t idx = 0; idx != pc_Y->size(); ++idx) {
        Y.col(idx) = Eigen::Vector3d{pc_Y->at(idx).x, pc_Y->at(idx).y, pc_Y->at(idx).z};
    }

【讨论】：