#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;
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);
}
}
}
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);
}
}
}
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;
}
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);
}
}