suzanne.soy/FreeCAD
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

+ fixes #0001592: Import colored PCL point clouds

Browse Source
This commit is contained in:
wmayer 2016-02-14 22:46:27 +01:00
parent 237c74c212
commit 1a64c3f2dc
4 changed files with 311 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

206
src/Mod/Points/App/AppPointsPy.cpp
View File

@ -23,6 +23,7 @@
#include "PreCompiled.h"
#ifndef _PreComp_
# include <memory>
#endif
// PCL test
@ -84,64 +85,63 @@ private:
if (file.extension().empty())
throw Py::RuntimeError("No file extension");
std::auto_ptr<Reader> reader;
if (file.hasExtension("asc")) {
// create new document and add Import feature
App::Document *pcDoc = App::GetApplication().newDocument("Unnamed");
Points::Feature *pcFeature = (Points::Feature *)pcDoc->addObject("Points::Feature", file.fileNamePure().c_str());
Points::PointKernel pkTemp;
pkTemp.load(EncodedName.c_str());
pcFeature->Points.setValue( pkTemp );
reader.reset(new AscReader);
}
#ifdef HAVE_PCL_IO
else if (file.hasExtension("ply")) {
PlyReader reader;
reader.read(EncodedName);
App::Document *pcDoc = App::GetApplication().newDocument("Unnamed");
if (reader.hasProperties()) {
Points::FeatureCustom *pcFeature = new Points::FeatureCustom();
pcFeature->Points.setValue(reader.getPoints());
// add gray values
if (reader.hasIntensities()) {
Points::PropertyGreyValueList* prop = static_cast<Points::PropertyGreyValueList*>
(pcFeature->addDynamicProperty("Points::PropertyGreyValueList", "Intensity"));
if (prop) {
prop->setValues(reader.getIntensities());
}
}
// add colors
if (reader.hasColors()) {
App::PropertyColorList* prop = static_cast<App::PropertyColorList*>
(pcFeature->addDynamicProperty("App::PropertyColorList", "Color"));
if (prop) {
prop->setValues(reader.getColors());
}
}
// add normals
if (reader.hasNormals()) {
Points::PropertyNormalList* prop = static_cast<Points::PropertyNormalList*>
(pcFeature->addDynamicProperty("Points::PropertyNormalList", "Normal"));
if (prop) {
prop->setValues(reader.getNormals());
}
}
// delayed adding of the points feature
pcDoc->addObject(pcFeature, file.fileNamePure().c_str());
pcDoc->recomputeFeature(pcFeature);
}
else {
Points::Feature *pcFeature = static_cast<Points::Feature*>
(pcDoc->addObject("Points::Feature", file.fileNamePure().c_str()));
pcFeature->Points.setValue(reader.getPoints());
pcDoc->recomputeFeature(pcFeature);
}
reader.reset(new PlyReader);
}
else if (file.hasExtension("pcd")) {
reader.reset(new PcdReader);
}
#endif
else {
throw Py::RuntimeError("Unsupported file extension");
}
reader->read(EncodedName);
App::Document *pcDoc = App::GetApplication().newDocument("Unnamed");
if (reader->hasProperties()) {
Points::FeatureCustom *pcFeature = new Points::FeatureCustom();
pcFeature->Points.setValue(reader->getPoints());
// add gray values
if (reader->hasIntensities()) {
Points::PropertyGreyValueList* prop = static_cast<Points::PropertyGreyValueList*>
(pcFeature->addDynamicProperty("Points::PropertyGreyValueList", "Intensity"));
if (prop) {
prop->setValues(reader->getIntensities());
}
}
// add colors
if (reader->hasColors()) {
App::PropertyColorList* prop = static_cast<App::PropertyColorList*>
(pcFeature->addDynamicProperty("App::PropertyColorList", "Color"));
if (prop) {
prop->setValues(reader->getColors());
}
}
// add normals
if (reader->hasNormals()) {
Points::PropertyNormalList* prop = static_cast<Points::PropertyNormalList*>
(pcFeature->addDynamicProperty("Points::PropertyNormalList", "Normal"));
if (prop) {
prop->setValues(reader->getNormals());
}
}
// delayed adding of the points feature
pcDoc->addObject(pcFeature, file.fileNamePure().c_str());
pcDoc->recomputeFeature(pcFeature);
}
else {
Points::Feature *pcFeature = static_cast<Points::Feature*>
(pcDoc->addObject("Points::Feature", file.fileNamePure().c_str()));
pcFeature->Points.setValue(reader->getPoints());
pcDoc->recomputeFeature(pcFeature);
}
}
catch (const Base::Exception& e) {
throw Py::RuntimeError(e.what());
@ -167,71 +167,67 @@ private:
if (file.extension().empty())
throw Py::RuntimeError("No file extension");
std::auto_ptr<Reader> reader;
if (file.hasExtension("asc")) {
// add Import feature
App::Document *pcDoc = App::GetApplication().getDocument(DocName);
if (!pcDoc) {
pcDoc = App::GetApplication().newDocument(DocName);
}
Points::Feature *pcFeature = (Points::Feature *)pcDoc->addObject("Points::Feature", file.fileNamePure().c_str());
Points::PointKernel pkTemp;
pkTemp.load(EncodedName.c_str());
pcFeature->Points.setValue( pkTemp );
reader.reset(new AscReader);
}
#ifdef HAVE_PCL_IO
else if (file.hasExtension("ply")) {
App::Document *pcDoc = App::GetApplication().getDocument(DocName);
if (!pcDoc) {
pcDoc = App::GetApplication().newDocument(DocName);
}
PlyReader reader;
reader.read(EncodedName);
if (reader.hasProperties()) {
Points::FeatureCustom *pcFeature = new Points::FeatureCustom();
pcFeature->Points.setValue(reader.getPoints());
// add gray values
if (reader.hasIntensities()) {
Points::PropertyGreyValueList* prop = static_cast<Points::PropertyGreyValueList*>
(pcFeature->addDynamicProperty("Points::PropertyGreyValueList", "Intensity"));
if (prop) {
prop->setValues(reader.getIntensities());
}
}
// add colors
if (reader.hasColors()) {
App::PropertyColorList* prop = static_cast<App::PropertyColorList*>
(pcFeature->addDynamicProperty("App::PropertyColorList", "Color"));
if (prop) {
prop->setValues(reader.getColors());
}
}
// add normals
if (reader.hasNormals()) {
Points::PropertyNormalList* prop = static_cast<Points::PropertyNormalList*>
(pcFeature->addDynamicProperty("Points::PropertyNormalList", "Normal"));
if (prop) {
prop->setValues(reader.getNormals());
}
}
// delayed adding of the points feature
pcDoc->addObject(pcFeature, file.fileNamePure().c_str());
pcDoc->recomputeFeature(pcFeature);
}
else {
Points::Feature *pcFeature = static_cast<Points::Feature*>
(pcDoc->addObject("Points::Feature", file.fileNamePure().c_str()));
pcFeature->Points.setValue(reader.getPoints());
pcDoc->recomputeFeature(pcFeature);
}
reader.reset(new PlyReader);
}
else if (file.hasExtension("pcd")) {
reader.reset(new PcdReader);
}
#endif
else {
throw Py::RuntimeError("Unsupported file extension");
}
reader->read(EncodedName);
App::Document *pcDoc = App::GetApplication().getDocument(DocName);
if (!pcDoc) {
pcDoc = App::GetApplication().newDocument(DocName);
}
if (reader->hasProperties()) {
Points::FeatureCustom *pcFeature = new Points::FeatureCustom();
pcFeature->Points.setValue(reader->getPoints());
// add gray values
if (reader->hasIntensities()) {
Points::PropertyGreyValueList* prop = static_cast<Points::PropertyGreyValueList*>
(pcFeature->addDynamicProperty("Points::PropertyGreyValueList", "Intensity"));
if (prop) {
prop->setValues(reader->getIntensities());
}
}
// add colors
if (reader->hasColors()) {
App::PropertyColorList* prop = static_cast<App::PropertyColorList*>
(pcFeature->addDynamicProperty("App::PropertyColorList", "Color"));
if (prop) {
prop->setValues(reader->getColors());
}
}
// add normals
if (reader->hasNormals()) {
Points::PropertyNormalList* prop = static_cast<Points::PropertyNormalList*>
(pcFeature->addDynamicProperty("Points::PropertyNormalList", "Normal"));
if (prop) {
prop->setValues(reader->getNormals());
}
}
// delayed adding of the points feature
pcDoc->addObject(pcFeature, file.fileNamePure().c_str());
pcDoc->recomputeFeature(pcFeature);
}
else {
Points::Feature *pcFeature = static_cast<Points::Feature*>
(pcDoc->addObject("Points::Feature", file.fileNamePure().c_str()));
pcFeature->Points.setValue(reader->getPoints());
pcDoc->recomputeFeature(pcFeature);
}
}
catch (const Base::Exception& e) {
throw Py::RuntimeError(e.what());

215
src/Mod/Points/App/PointsAlgos.cpp
View File

@ -31,6 +31,7 @@
#ifdef HAVE_PCL_IO
# include <pcl/io/ply_io.h>
# include <pcl/io/pcd_io.h>
# include <pcl/point_types.h>
#endif
@ -117,6 +118,80 @@ void PointsAlgos::LoadAscii(PointKernel &points, const char *FileName)
points.erase(LineCnt, points.size());
}
// ----------------------------------------------------------------------------
Reader::Reader()
{
}
Reader::~Reader()
{
}
void Reader::clear()
{
intensity.clear();
colors.clear();
normals.clear();
}
const PointKernel& Reader::getPoints() const
{
return points;
}
bool Reader::hasProperties() const
{
return (hasIntensities() || hasColors() || hasNormals());
}
const std::vector<float>& Reader::getIntensities() const
{
return intensity;
}
bool Reader::hasIntensities() const
{
return (!intensity.empty());
}
const std::vector<App::Color>& Reader::getColors() const
{
return colors;
}
bool Reader::hasColors() const
{
return (!colors.empty());
}
const std::vector<Base::Vector3f>& Reader::getNormals() const
{
return normals;
}
bool Reader::hasNormals() const
{
return (!normals.empty());
}
// ----------------------------------------------------------------------------
AscReader::AscReader()
{
}
AscReader::~AscReader()
{
}
void AscReader::read(const std::string& filename)
{
points.load(filename.c_str());
}
// ----------------------------------------------------------------------------
#ifdef HAVE_PCL_IO
PlyReader::PlyReader()
{
@ -126,13 +201,6 @@ PlyReader::~PlyReader()
{
}
void PlyReader::clear()
{
intensity.clear();
colors.clear();
normals.clear();
}
void PlyReader::read(const std::string& filename)
{
clear();
@ -235,43 +303,116 @@ void PlyReader::read(const std::string& filename)
}
}
const PointKernel& PlyReader::getPoints() const
// ----------------------------------------------------------------------------
PcdReader::PcdReader()
{
return points;
}
bool PlyReader::hasProperties() const
PcdReader::~PcdReader()
{
return (hasIntensities() || hasColors() || hasNormals());
}
const std::vector<float>& PlyReader::getIntensities() const
void PcdReader::read(const std::string& filename)
{
return intensity;
clear();
// pcl test
pcl::PCLPointCloud2 cloud2;
Eigen::Vector4f origin;
Eigen::Quaternionf orientation;
int ply_version;
int data_type;
unsigned int data_idx;
pcl::PCDReader pcd;
pcd.readHeader(filename, cloud2, origin, orientation, ply_version, data_type, data_idx);
bool hasIntensity = false;
bool hasColors = false;
bool hasNormals = false;
for (size_t i = 0; i < cloud2.fields.size (); ++i) {
if (cloud2.fields[i].name == "intensity")
hasIntensity = true;
if (cloud2.fields[i].name == "normal_x" || cloud2.fields[i].name == "nx")
hasNormals = true;
if (cloud2.fields[i].name == "normal_y" || cloud2.fields[i].name == "ny")
hasNormals = true;
if (cloud2.fields[i].name == "normal_z" || cloud2.fields[i].name == "nz")
hasNormals = true;
if (cloud2.fields[i].name == "red")
hasColors = true;
if (cloud2.fields[i].name == "green")
hasColors = true;
if (cloud2.fields[i].name == "blue")
hasColors = true;
if (cloud2.fields[i].name == "rgb")
hasColors = true;
if (cloud2.fields[i].name == "rgba")
hasColors = true;
}
if (hasNormals && hasColors) {
pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_in;
pcl::io::loadPCDFile<pcl::PointXYZRGBNormal>(filename, cloud_in);
points.reserve(cloud_in.size());
colors.reserve(cloud_in.size());
normals.reserve(cloud_in.size());
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
points.push_back(Base::Vector3d(it->x,it->y,it->z));
colors.push_back(App::Color(it->r/255.0f,it->g/255.0f,it->b/255.0f));
normals.push_back(Base::Vector3f(it->normal_x,it->normal_y,it->normal_z));
}
}
else if (hasNormals && hasIntensity) {
pcl::PointCloud<pcl::PointXYZINormal> cloud_in;
pcl::io::loadPCDFile<pcl::PointXYZINormal>(filename, cloud_in);
points.reserve(cloud_in.size());
intensity.reserve(cloud_in.size());
normals.reserve(cloud_in.size());
for (pcl::PointCloud<pcl::PointXYZINormal>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
points.push_back(Base::Vector3d(it->x,it->y,it->z));
intensity.push_back(it->intensity);
normals.push_back(Base::Vector3f(it->normal_x,it->normal_y,it->normal_z));
}
}
else if (hasColors) {
pcl::PointCloud<pcl::PointXYZRGBA> cloud_in;
pcl::io::loadPCDFile<pcl::PointXYZRGBA>(filename, cloud_in);
points.reserve(cloud_in.size());
colors.reserve(cloud_in.size());
for (pcl::PointCloud<pcl::PointXYZRGBA>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
points.push_back(Base::Vector3d(it->x,it->y,it->z));
colors.push_back(App::Color(it->r/255.0f,it->g/255.0f,it->b/255.0f,it->a/255.0f));
}
}
else if (hasIntensity) {
pcl::PointCloud<pcl::PointXYZI> cloud_in;
pcl::io::loadPCDFile<pcl::PointXYZI>(filename, cloud_in);
points.reserve(cloud_in.size());
intensity.reserve(cloud_in.size());
for (pcl::PointCloud<pcl::PointXYZI>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
points.push_back(Base::Vector3d(it->x,it->y,it->z));
intensity.push_back(it->intensity);
}
}
else if (hasNormals) {
pcl::PointCloud<pcl::PointNormal> cloud_in;
pcl::io::loadPCDFile<pcl::PointNormal>(filename, cloud_in);
points.reserve(cloud_in.size());
normals.reserve(cloud_in.size());
for (pcl::PointCloud<pcl::PointNormal>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
points.push_back(Base::Vector3d(it->x,it->y,it->z));
normals.push_back(Base::Vector3f(it->normal_x,it->normal_y,it->normal_z));
}
}
else {
pcl::PointCloud<pcl::PointXYZ> cloud_in;
pcl::io::loadPCDFile<pcl::PointXYZ>(filename, cloud_in);
points.reserve(cloud_in.size());
for (pcl::PointCloud<pcl::PointXYZ>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
points.push_back(Base::Vector3d(it->x,it->y,it->z));
}
}
}
bool PlyReader::hasIntensities() const
{
return (!intensity.empty());
}
const std::vector<App::Color>& PlyReader::getColors() const
{
return colors;
}
bool PlyReader::hasColors() const
{
return (!colors.empty());
}
const std::vector<Base::Vector3f>& PlyReader::getNormals() const
{
return normals;
}
bool PlyReader::hasNormals() const
{
return (!normals.empty());
}
#endif

37
src/Mod/Points/App/PointsAlgos.h
View File

@ -43,14 +43,14 @@ public:
static void LoadAscii(PointKernel&, const char *FileName);
};
#ifdef HAVE_PCL_IO
class PlyReader
class Reader
{
public:
PlyReader();
~PlyReader();
Reader();
virtual ~Reader();
virtual void read(const std::string& filename) = 0;
void clear();
void read(const std::string& filename);
const PointKernel& getPoints() const;
bool hasProperties() const;
const std::vector<float>& getIntensities() const;
@ -60,12 +60,37 @@ public:
const std::vector<Base::Vector3f>& getNormals() const;
bool hasNormals() const;
private:
protected:
PointKernel points;
std::vector<float> intensity;
std::vector<App::Color> colors;
std::vector<Base::Vector3f> normals;
};
class AscReader : public Reader
{
public:
AscReader();
~AscReader();
void read(const std::string& filename);
};
#ifdef HAVE_PCL_IO
class PlyReader : public Reader
{
public:
PlyReader();
~PlyReader();
void read(const std::string& filename);
};
class PcdReader : public Reader
{
public:
PcdReader();
~PcdReader();
void read(const std::string& filename);
};
#endif
} // namespace Points

1
src/Mod/Points/Init.py
View File

@ -30,3 +30,4 @@
# Append the open handler
FreeCAD.addImportType("Point formats (*.asc)","Points")
FreeCAD.addImportType("PLY points (*.ply)","Points")
FreeCAD.addImportType("PCD points (*.pcd)","Points")