pcl与cloudcompare点云库CCCoreLib的点云互转

fighton

于 2024-04-19 17:42:12 发布

阅读量147

点赞数 1

文章标签： pcl cloudcompare

本文链接：https://blog.csdn.net/fighton/article/details/137972588

版权

本文详细介绍了如何在C++中使用PCL（PointCloudLibrary）处理PointXYZE类型的点云数据，并提供了从CCCoreLib的PointCloud和ReferenceCloud转换到PCL以及反之的操作方法。

摘要由CSDN通过智能技术生成

#include <pcl/pcl_base.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <CCCoreLib/PointCloud.h>

struct EIGEN_ALIGN16 PointXYZE {
    PCL_ADD_POINT4D;
    float e1, e2, e3;

    inline constexpr PointXYZE(const PointXYZE &p)
        : PointXYZE(p.x, p.y, p.z, p.e1, p.e2, p.e3)
    {
    }

    inline constexpr PointXYZE() : PointXYZE(0.f, 0.f, 0.f, 0.f, 0.f, 0.f) {}

    inline constexpr PointXYZE(float _x, float _y, float _z, float _e1 = 0.f,
                               float _e2 = 0.f, float _e3 = 0.f)
        : x(_x), y(_y), z(_z), e1(_e1), e2(_e2), e3(_e3)
    {
    }

    PCL_MAKE_ALIGNED_OPERATOR_NEW
};

// 注册点类型宏
POINT_CLOUD_REGISTER_POINT_STRUCT(
    PointXYZE,
    (float, x, x)(float, y, y)(float, z, z)(float, e1, e1)(float, e2,
                                                           e2)(float, e3, e3))

using PointT             = PointXYZE;
using PointCloudT        = pcl::PointCloud<PointT>;
using PointCloudPtr      = PointCloudT::Ptr;
using PointCloudConstPtr = PointCloudT::ConstPtr;

// 转换到pcl的点云
void Foo::fromCCPointCloud(CCCoreLib::PointCloud& cccloud,
                                   PointCloudT& cloud)
{
    uint count = cccloud.size();
    if (count != 0) {
        cloud.reserve(count);

        for (uint i = 0; i < count; i++) {
            auto thePoint = cccloud.getPoint(i);
            auto& pt      = cloud.points.emplace_back(thePoint->x, thePoint->y,
                                                      thePoint->z);
            cccloud.setCurrentOutScalarField(0);
            pt.e1 = cccloud.getPointScalarValue(i);
            cccloud.setCurrentOutScalarField(1);
            pt.e2 = cccloud.getPointScalarValue(i);
            cccloud.setCurrentOutScalarField(2);
            pt.e3 = cccloud.getPointScalarValue(i);
        }
    }
}

// 从CCCoreLib的索引点云转换到pcl点云
void Foo::fromCCCoreReferenceCloud(CCCoreLib::ReferenceCloud& refCloud,
                                           PointCloudT& cloud)
{
    uint refSize = refCloud.size();
    if (refSize != 0) {
        cloud.reserve(refSize);

        auto theCloud =
            dynamic_cast<CCCoreLib::PointCloud*>(refCloud.getAssociatedCloud());

        if (theCloud == nullptr) {
            return;
        }

        for (uint i = 0; i < refSize; i++) {
            auto index    = refCloud.getPointGlobalIndex(i);
            auto thePoint = theCloud->getPoint(index);
            auto& pt      = cloud.points.emplace_back(thePoint->x, thePoint->y,
                                                      thePoint->z);
            theCloud->setCurrentOutScalarField(0);
            pt.e1 = theCloud->getPointScalarValue(index);
            theCloud->setCurrentOutScalarField(1);
            pt.e2 = theCloud->getPointScalarValue(index);
            theCloud->setCurrentOutScalarField(2);
            pt.e3 = theCloud->getPointScalarValue(index);
        }
    }
}

// CCCoreLib从点云索引提取部分点云
std::shared_ptr<CCCoreLib::PointCloud> Foo::partialClone(
    CCCoreLib::ReferenceCloud* selection)
{
    uint refSize = selection->size();
    std::shared_ptr<CCCoreLib::PointCloud> cloud;
    if (refSize != 0) {
        cloud.reset(new CCCoreLib::PointCloud);
        cloud->reserve(refSize);

        auto theCloud = dynamic_cast<CCCoreLib::PointCloud*>(
            selection->getAssociatedCloud());

        if (theCloud == nullptr) {
            return nullptr;
        }

        cloud->addScalarField("e1");
        cloud->addScalarField("e2");
        cloud->addScalarField("e3");

        for (uint i = 0; i < refSize; i++) {
            auto index    = selection->getPointGlobalIndex(i);
            auto thePoint = theCloud->getPoint(index);
            cloud->addPoint(CCVector3(thePoint->x, thePoint->y, thePoint->z));
            theCloud->setCurrentOutScalarField(0);
            cloud->setCurrentInScalarField(0);
            cloud->addPointScalarValue(theCloud->getPointScalarValue(index));
            theCloud->setCurrentOutScalarField(1);
            cloud->setCurrentInScalarField(1);
            cloud->addPointScalarValue(theCloud->getPointScalarValue(index));
            theCloud->setCurrentOutScalarField(2);
            cloud->setCurrentInScalarField(2);
            cloud->addPointScalarValue(theCloud->getPointScalarValue(index));
        }

        if (selection->normalsAvailable()) {
            cloud->reserveNormals(refSize);

            for (int i = 0; i < refSize; i++) {
                auto n = theCloud->getNormal(selection->getPointGlobalIndex(i));
                cloud->addNormal(*n);
            }
        }
    }

    return cloud;
}

// 从pcl点云转换到CCCoreLib点云
void Foo::toCCCorePointCloud(const PointCloudT& cloud,
                                     CCCoreLib::PointCloud& cccloud)
{
    cccloud.enableScalarField();
    cccloud.reserve(cloud.size());
    cccloud.addScalarField("e1");
    cccloud.addScalarField("e2");
    cccloud.addScalarField("e3");

    int index = 0;
    for (auto pt : cloud) {
        cccloud.addPoint(CCVector3f{pt.x, pt.y, pt.z});
        cccloud.setCurrentInScalarField(0);
        cccloud.addPointScalarValue(pt.e1);
        cccloud.setCurrentInScalarField(1);
        cccloud.addPointScalarValue(pt.e2);
        cccloud.setCurrentInScalarField(2);
        cccloud.addPointScalarValue(pt.e3);
    }
}