From 1a64c3f2dc17e9566f2d586e405c77895d769d52 Mon Sep 17 00:00:00 2001 From: wmayer Date: Sun, 14 Feb 2016 22:46:27 +0100 Subject: [PATCH] + fixes #0001592: Import colored PCL point clouds --- src/Mod/Points/App/AppPointsPy.cpp | 206 ++++++++++++++------------- src/Mod/Points/App/PointsAlgos.cpp | 215 ++++++++++++++++++++++++----- src/Mod/Points/App/PointsAlgos.h | 37 ++++- src/Mod/Points/Init.py | 1 + 4 files changed, 311 insertions(+), 148 deletions(-) diff --git a/src/Mod/Points/App/AppPointsPy.cpp b/src/Mod/Points/App/AppPointsPy.cpp index 5a1ef28eb..fd0e11c40 100644 --- a/src/Mod/Points/App/AppPointsPy.cpp +++ b/src/Mod/Points/App/AppPointsPy.cpp @@ -23,6 +23,7 @@ #include "PreCompiled.h" #ifndef _PreComp_ +# include #endif // PCL test @@ -84,64 +85,63 @@ private: if (file.extension().empty()) throw Py::RuntimeError("No file extension"); + std::auto_ptr 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 - (pcFeature->addDynamicProperty("Points::PropertyGreyValueList", "Intensity")); - if (prop) { - prop->setValues(reader.getIntensities()); - } - } - // add colors - if (reader.hasColors()) { - App::PropertyColorList* prop = static_cast - (pcFeature->addDynamicProperty("App::PropertyColorList", "Color")); - if (prop) { - prop->setValues(reader.getColors()); - } - } - // add normals - if (reader.hasNormals()) { - Points::PropertyNormalList* prop = static_cast - (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 - (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 + (pcFeature->addDynamicProperty("Points::PropertyGreyValueList", "Intensity")); + if (prop) { + prop->setValues(reader->getIntensities()); + } + } + // add colors + if (reader->hasColors()) { + App::PropertyColorList* prop = static_cast + (pcFeature->addDynamicProperty("App::PropertyColorList", "Color")); + if (prop) { + prop->setValues(reader->getColors()); + } + } + // add normals + if (reader->hasNormals()) { + Points::PropertyNormalList* prop = static_cast + (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 + (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; 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 - (pcFeature->addDynamicProperty("Points::PropertyGreyValueList", "Intensity")); - if (prop) { - prop->setValues(reader.getIntensities()); - } - } - // add colors - if (reader.hasColors()) { - App::PropertyColorList* prop = static_cast - (pcFeature->addDynamicProperty("App::PropertyColorList", "Color")); - if (prop) { - prop->setValues(reader.getColors()); - } - } - // add normals - if (reader.hasNormals()) { - Points::PropertyNormalList* prop = static_cast - (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 - (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 + (pcFeature->addDynamicProperty("Points::PropertyGreyValueList", "Intensity")); + if (prop) { + prop->setValues(reader->getIntensities()); + } + } + // add colors + if (reader->hasColors()) { + App::PropertyColorList* prop = static_cast + (pcFeature->addDynamicProperty("App::PropertyColorList", "Color")); + if (prop) { + prop->setValues(reader->getColors()); + } + } + // add normals + if (reader->hasNormals()) { + Points::PropertyNormalList* prop = static_cast + (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 + (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()); diff --git a/src/Mod/Points/App/PointsAlgos.cpp b/src/Mod/Points/App/PointsAlgos.cpp index 33b7e87bc..4e534e506 100644 --- a/src/Mod/Points/App/PointsAlgos.cpp +++ b/src/Mod/Points/App/PointsAlgos.cpp @@ -31,6 +31,7 @@ #ifdef HAVE_PCL_IO # include +# include # include #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& Reader::getIntensities() const +{ + return intensity; +} + +bool Reader::hasIntensities() const +{ + return (!intensity.empty()); +} + +const std::vector& Reader::getColors() const +{ + return colors; +} + +bool Reader::hasColors() const +{ + return (!colors.empty()); +} + +const std::vector& 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& 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 cloud_in; + pcl::io::loadPCDFile(filename, cloud_in); + points.reserve(cloud_in.size()); + colors.reserve(cloud_in.size()); + normals.reserve(cloud_in.size()); + for (pcl::PointCloud::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 cloud_in; + pcl::io::loadPCDFile(filename, cloud_in); + points.reserve(cloud_in.size()); + intensity.reserve(cloud_in.size()); + normals.reserve(cloud_in.size()); + for (pcl::PointCloud::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 cloud_in; + pcl::io::loadPCDFile(filename, cloud_in); + points.reserve(cloud_in.size()); + colors.reserve(cloud_in.size()); + for (pcl::PointCloud::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 cloud_in; + pcl::io::loadPCDFile(filename, cloud_in); + points.reserve(cloud_in.size()); + intensity.reserve(cloud_in.size()); + for (pcl::PointCloud::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 cloud_in; + pcl::io::loadPCDFile(filename, cloud_in); + points.reserve(cloud_in.size()); + normals.reserve(cloud_in.size()); + for (pcl::PointCloud::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 cloud_in; + pcl::io::loadPCDFile(filename, cloud_in); + points.reserve(cloud_in.size()); + for (pcl::PointCloud::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& PlyReader::getColors() const -{ - return colors; -} - -bool PlyReader::hasColors() const -{ - return (!colors.empty()); -} - -const std::vector& PlyReader::getNormals() const -{ - return normals; -} - -bool PlyReader::hasNormals() const -{ - return (!normals.empty()); -} #endif diff --git a/src/Mod/Points/App/PointsAlgos.h b/src/Mod/Points/App/PointsAlgos.h index 42976e18e..b24d9d576 100644 --- a/src/Mod/Points/App/PointsAlgos.h +++ b/src/Mod/Points/App/PointsAlgos.h @@ -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& getIntensities() const; @@ -60,12 +60,37 @@ public: const std::vector& getNormals() const; bool hasNormals() const; -private: +protected: PointKernel points; std::vector intensity; std::vector colors; std::vector 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 diff --git a/src/Mod/Points/Init.py b/src/Mod/Points/Init.py index 853e90e58..8eeb50226 100644 --- a/src/Mod/Points/Init.py +++ b/src/Mod/Points/Init.py @@ -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")