From 20361400e1a43d2085ae0717b742083737f6d1ec Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 7 Sep 2021 15:57:23 -0400 Subject: [PATCH] export: output intensity channel with RGB when --cam_projection and --scan options are set (PDAL required) --- corelib/include/rtabmap/core/PDALWriter.h | 4 +- corelib/src/PDALWriter.cpp | 74 +++++++++++++++++++++-- tools/Export/main.cpp | 41 +++++++++++-- 3 files changed, 109 insertions(+), 10 deletions(-) diff --git a/corelib/include/rtabmap/core/PDALWriter.h b/corelib/include/rtabmap/core/PDALWriter.h index a4ed246e..b7cc2d5b 100644 --- a/corelib/include/rtabmap/core/PDALWriter.h +++ b/corelib/include/rtabmap/core/PDALWriter.h @@ -36,8 +36,8 @@ namespace rtabmap { std::string getPDALSupportedWriters(); int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector(), bool binary = false); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector(), bool binary = false); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector(), bool binary = false); +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector(), bool binary = false, const std::vector & intensities = std::vector()); +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector(), bool binary = false, const std::vector & intensities = std::vector()); int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector(), bool binary = false); int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector(), bool binary = false); diff --git a/corelib/src/PDALWriter.cpp b/corelib/src/PDALWriter.cpp index de938136..6310c089 100644 --- a/corelib/src/PDALWriter.cpp +++ b/corelib/src/PDALWriter.cpp @@ -143,14 +143,40 @@ int savePDALFile(const std::string & filePath, int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds, - bool binary) + bool binary, + const std::vector & intensities) { UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); + UASSERT_MSG(intensities.empty() || intensities.size() == cloud.size(), + uFormat("intensities=%d cloud=%d", (int)intensities.size(), (int)cloud.size()).c_str()); pdal::PointTable table; - if(!cameraIds.empty()) + if(!intensities.empty() && !cameraIds.empty()) + { + table.layout()->registerDims({ + pdal::Dimension::Id::X, + pdal::Dimension::Id::Y, + pdal::Dimension::Id::Z, + pdal::Dimension::Id::Red, + pdal::Dimension::Id::Green, + pdal::Dimension::Id::Blue, + pdal::Dimension::Id::PointSourceId, + pdal::Dimension::Id::Intensity}); + } + else if(!intensities.empty()) + { + table.layout()->registerDims({ + pdal::Dimension::Id::X, + pdal::Dimension::Id::Y, + pdal::Dimension::Id::Z, + pdal::Dimension::Id::Red, + pdal::Dimension::Id::Green, + pdal::Dimension::Id::Blue, + pdal::Dimension::Id::Intensity}); + } + else if(!cameraIds.empty()) { table.layout()->registerDims({ pdal::Dimension::Id::X, @@ -186,6 +212,10 @@ int savePDALFile(const std::string & filePath, { view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i)); } + if(!intensities.empty()) + { + view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)intensities.at(i)); + } } bufferReader.addView(view); @@ -219,14 +249,46 @@ int savePDALFile(const std::string & filePath, int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds, - bool binary) + bool binary, + const std::vector & intensities) { UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); + UASSERT_MSG(intensities.empty() || intensities.size() == cloud.size(), + uFormat("intensities=%d cloud=%d", (int)intensities.size(), (int)cloud.size()).c_str()); pdal::PointTable table; - if(!cameraIds.empty()) + if(!intensities.empty() && !cameraIds.empty()) + { + table.layout()->registerDims({ + pdal::Dimension::Id::X, + pdal::Dimension::Id::Y, + pdal::Dimension::Id::Z, + pdal::Dimension::Id::Red, + pdal::Dimension::Id::Green, + pdal::Dimension::Id::Blue, + pdal::Dimension::Id::NormalX, + pdal::Dimension::Id::NormalY, + pdal::Dimension::Id::NormalZ, + pdal::Dimension::Id::PointSourceId, + pdal::Dimension::Id::Intensity}); + } + else if(!intensities.empty()) + { + table.layout()->registerDims({ + pdal::Dimension::Id::X, + pdal::Dimension::Id::Y, + pdal::Dimension::Id::Z, + pdal::Dimension::Id::Red, + pdal::Dimension::Id::Green, + pdal::Dimension::Id::Blue, + pdal::Dimension::Id::NormalX, + pdal::Dimension::Id::NormalY, + pdal::Dimension::Id::NormalZ, + pdal::Dimension::Id::Intensity}); + } + else if(!cameraIds.empty()) { table.layout()->registerDims({ pdal::Dimension::Id::X, @@ -271,6 +333,10 @@ int savePDALFile(const std::string & filePath, { view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i)); } + if(!intensities.empty()) + { + view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)intensities.at(i)); + } } bufferReader.addView(view); diff --git a/tools/Export/main.cpp b/tools/Export/main.cpp index 274694d8..8c536fce 100644 --- a/tools/Export/main.cpp +++ b/tools/Export/main.cpp @@ -1057,6 +1057,7 @@ int main(int argc, char * argv[]) } std::vector pointToCamId; + std::vector pointToCamIntensity; if(camProjection && !robotPoses.empty()) { printf("Camera projection...\n"); @@ -1083,6 +1084,7 @@ int main(int argc, char * argv[]) 0, std::vector(), distanceToCamPolicy); + pointToCamIntensity.resize(pointToPixel.size()); } // color the cloud @@ -1095,6 +1097,7 @@ int main(int argc, char * argv[]) for(size_t i=0; iempty()) { pt = cloudToExport->at(i); @@ -1107,6 +1110,7 @@ int main(int argc, char * argv[]) pt.normal_x = cloudIToExport->at(i).normal_x; pt.normal_y = cloudIToExport->at(i).normal_y; pt.normal_z = cloudIToExport->at(i).normal_z; + intensity = cloudIToExport->at(i).intensity; } int nodeID = pointToPixel[i].first.first; int cameraIndex = pointToPixel[i].first.second; @@ -1150,6 +1154,10 @@ int main(int argc, char * argv[]) int exportedId = nodeID; pointToCamId[oi] = exportedId; + if(!pointToCamIntensity.empty()) + { + pointToCamIntensity[oi] = intensity; + } assembledCloudValidPoints->at(oi++) = pt; } else if(camProjectionKeepAll) @@ -1158,6 +1166,10 @@ int main(int argc, char * argv[]) pt.b = 0; pt.g = 0; pt.r = 255; + if(!pointToCamIntensity.empty()) + { + pointToCamIntensity[oi] = intensity; + } assembledCloudValidPoints->at(oi++) = pt; // red } } @@ -1166,6 +1178,10 @@ int main(int argc, char * argv[]) cloudToExport = assembledCloudValidPoints; cloudIToExport->clear(); pointToCamId.resize(oi); + if(!pointToCamIntensity.empty()) + { + pointToCamIntensity.resize(oi); + } printf("Camera projection... done! (%fs)\n", timer.ticks()); } @@ -1187,10 +1203,19 @@ int main(int argc, char * argv[]) std::string outputPath=outputDirectory+"/"+baseName+"_cloud."+ext; printf("Saving %s... (%d points)\n", outputPath.c_str(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size()); #ifdef RTABMAP_PDAL - if(las || !pointToCamId.empty()) + if(las || !pointToCamId.empty() || !pointToCamIntensity.empty()) { if(!cloudToExport->empty()) - savePDALFile(outputPath, *cloudToExport, pointToCamId, binary); + { + if(!pointToCamIntensity.empty()) + { + savePDALFile(outputPath, *cloudToExport, pointToCamId, binary, pointToCamIntensity); + } + else + { + savePDALFile(outputPath, *cloudToExport, pointToCamId, binary); + } + } else if(!cloudIToExport->empty()) savePDALFile(outputPath, *cloudIToExport, pointToCamId, binary); } @@ -1199,8 +1224,16 @@ int main(int argc, char * argv[]) { if(!pointToCamId.empty()) { - printf("Option --cam_projection is enabled but rtabmap is not built " - "with PDAL support, so camera IDs won't be exported in the output cloud.\n"); + if(!pointToCamIntensity.empty()) + { + printf("Option --cam_projection is enabled but rtabmap is not built " + "with PDAL support, so camera IDs and lidar intensities won't be exported in the output cloud.\n"); + } + else + { + printf("Option --cam_projection is enabled but rtabmap is not built " + "with PDAL support, so camera IDs won't be exported in the output cloud.\n"); + } } if(!cloudToExport->empty()) pcl::io::savePLYFile(outputPath, *cloudToExport, binary);