From 8cb3ca401e929584a05880bf649b4fad61982e23 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 21 Feb 2021 14:02:12 -0500 Subject: [PATCH 1/4] ExportClouds: Added camera projection options --- corelib/include/rtabmap/core/PDALWriter.h | 10 +- corelib/include/rtabmap/core/util3d.h | 15 + corelib/src/PDALWriter.cpp | 183 +++++++--- corelib/src/util3d.cpp | 275 ++++++++++++--- .../include/rtabmap/gui/ExportCloudsDialog.h | 2 +- guilib/src/ExportCloudsDialog.cpp | 321 ++++++++++++++++-- guilib/src/ui/exportCloudsDialog.ui | 268 +++++++++++++-- 7 files changed, 925 insertions(+), 149 deletions(-) diff --git a/corelib/include/rtabmap/core/PDALWriter.h b/corelib/include/rtabmap/core/PDALWriter.h index acbddd5414..c13cde27bf 100644 --- a/corelib/include/rtabmap/core/PDALWriter.h +++ b/corelib/include/rtabmap/core/PDALWriter.h @@ -35,11 +35,11 @@ namespace rtabmap { std::string getPDALSupportedWriters(); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud); +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector()); +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector()); +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector()); +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector()); +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector()); } diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 5d8b5f13bf..e155e7fab1 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -300,6 +301,20 @@ void RTABMAP_EXP fillProjectedCloudHoles( bool verticalDirection, bool fillToBorder); +/** + * For each point, return pixel of the best camera (NodeID->CameraIndex) + * looking at it based on the policy and parameters + */ +std::vector, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras ( + const pcl::PointCloud & cloud, + const std::map & cameraPoses, + const std::map > & cameraModels, + float maxDistance, + float maxAngle, + const std::vector & roiRatios, + bool distanceToCamPolicy, + const ProgressState * state); + bool RTABMAP_EXP isFinite(const cv::Point3f & pt); pcl::PointCloud::Ptr RTABMAP_EXP concatenateClouds( diff --git a/corelib/src/PDALWriter.cpp b/corelib/src/PDALWriter.cpp index 4cecbd6a49..d9b568ba2e 100644 --- a/corelib/src/PDALWriter.cpp +++ b/corelib/src/PDALWriter.cpp @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -72,14 +73,28 @@ std::string getPDALSupportedWriters() return output; } -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud) +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds) { + UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), + uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); + pdal::PointTable table; - table.layout()->registerDims({ - pdal::Dimension::Id::X, - pdal::Dimension::Id::Y, - pdal::Dimension::Id::Z}); + if(!cameraIds.empty()) + { + table.layout()->registerDims({ + pdal::Dimension::Id::X, + pdal::Dimension::Id::Y, + pdal::Dimension::Id::Z, + pdal::Dimension::Id::PointSourceId}); + } + else + { + table.layout()->registerDims({ + pdal::Dimension::Id::X, + pdal::Dimension::Id::Y, + pdal::Dimension::Id::Z}); + } pdal::BufferReader bufferReader; pdal::PointViewPtr view(new pdal::PointView(table)); @@ -88,6 +103,10 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloudsetField(pdal::Dimension::Id::X, i, cloud.at(i).x); view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y); view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z); + if(!cameraIds.empty()) + { + view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i)); + } } bufferReader.addView(view); @@ -115,17 +134,34 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud) +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds) { + UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), + uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); + pdal::PointTable table; - 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}); + if(!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}); + } + else + { + 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::BufferReader bufferReader; pdal::PointViewPtr view(new pdal::PointView(table)); @@ -137,6 +173,10 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloudsetField(pdal::Dimension::Id::Red, i, cloud.at(i).r); view->setField(pdal::Dimension::Id::Green, i, cloud.at(i).g); view->setField(pdal::Dimension::Id::Blue, i, cloud.at(i).b); + if(!cameraIds.empty()) + { + view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i)); + } } bufferReader.addView(view); @@ -164,20 +204,40 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud) +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds) { + UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), + uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); + pdal::PointTable table; - 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}); + if(!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}); + } + else + { + 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::BufferReader bufferReader; pdal::PointViewPtr view(new pdal::PointView(table)); @@ -192,6 +252,10 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloudsetField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x); view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y); view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z); + if(!cameraIds.empty()) + { + view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i)); + } } bufferReader.addView(view); @@ -219,15 +283,30 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud) +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds) { + UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), + uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); + pdal::PointTable table; - table.layout()->registerDims({ - pdal::Dimension::Id::X, - pdal::Dimension::Id::Y, - pdal::Dimension::Id::Z, - pdal::Dimension::Id::Intensity}); + if(!cameraIds.empty()) + { + table.layout()->registerDims({ + pdal::Dimension::Id::X, + pdal::Dimension::Id::Y, + pdal::Dimension::Id::Z, + pdal::Dimension::Id::Intensity, + pdal::Dimension::Id::PointSourceId}); + } + else + { + table.layout()->registerDims({ + pdal::Dimension::Id::X, + pdal::Dimension::Id::Y, + pdal::Dimension::Id::Z, + pdal::Dimension::Id::Intensity}); + } pdal::BufferReader bufferReader; pdal::PointViewPtr view(new pdal::PointView(table)); @@ -237,6 +316,10 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloudsetField(pdal::Dimension::Id::Y, i, cloud.at(i).y); view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z); view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)cloud.at(i).intensity); + if(!cameraIds.empty()) + { + view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i)); + } } bufferReader.addView(view); @@ -264,18 +347,36 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud) +int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds) { + UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), + uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); + pdal::PointTable table; - table.layout()->registerDims({ - pdal::Dimension::Id::X, - pdal::Dimension::Id::Y, - pdal::Dimension::Id::Z, - pdal::Dimension::Id::Intensity, - pdal::Dimension::Id::NormalX, - pdal::Dimension::Id::NormalY, - pdal::Dimension::Id::NormalZ}); + if(!cameraIds.empty()) + { + table.layout()->registerDims({ + pdal::Dimension::Id::X, + pdal::Dimension::Id::Y, + pdal::Dimension::Id::Z, + pdal::Dimension::Id::Intensity, + pdal::Dimension::Id::NormalX, + pdal::Dimension::Id::NormalY, + pdal::Dimension::Id::NormalZ, + pdal::Dimension::Id::PointSourceId}); + } + else + { + table.layout()->registerDims({ + pdal::Dimension::Id::X, + pdal::Dimension::Id::Y, + pdal::Dimension::Id::Z, + pdal::Dimension::Id::Intensity, + pdal::Dimension::Id::NormalX, + pdal::Dimension::Id::NormalY, + pdal::Dimension::Id::NormalZ}); + } pdal::BufferReader bufferReader; pdal::PointViewPtr view(new pdal::PointView(table)); @@ -288,6 +389,10 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloudsetField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x); view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y); view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z); + if(!cameraIds.empty()) + { + view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i)); + } } bufferReader.addView(view); diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 38fe9c2cb5..eed1931b96 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -3015,52 +3016,6 @@ cv::Mat projectCloudToCamera( { UERROR("field map pcl::pointXYZ not found!"); } -/* - int count = 0; - for(int i=0; i<(int)laserScan->size(); ++i) - { - // Get 3D from laser scan - pcl::PointXYZ ptScan = laserScan->at(i); - ptScan = util3d::transformPoint(ptScan, t); - - // re-project in camera frame - float z = ptScan.z; - bool set = false; - if(z > 0.0f) - { - float invZ = 1.0f/z; - float dx = (fx*ptScan.x)*invZ + cx; - float dy = (fy*ptScan.y)*invZ + cy; - int dx_low = dx; - int dy_low = dy; - int dx_high = dx + 0.5f; - int dy_high = dy + 0.5f; - if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows)) - { - set = true; - float &zReg = registered.at(dy_low, dx_low); - if(zReg == 0 || z < zReg) - { - zReg = z; - } - } - if((dx_low != dx_high || dy_low != dy_high) && - uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows)) - { - set = true; - float &zReg = registered.at(dy_high, dx_high); - if(zReg == 0 || z < zReg) - { - zReg = z; - } - } - } - if(set) - { - count++; - } - } - */ UDEBUG("Points in camera=%d/%d", count, (int)laserScan->data.size()); return registered; @@ -3149,6 +3104,234 @@ void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection, } } +struct ProjectionInfo { + int nodeID; + int cameraIndex; + pcl::PointXY uv; + float distance; +}; + +/** + * For each point, return pixel of the best camera (NodeID->CameraIndex) + * looking at it based on the policy and parameters + */ +std::vector, pcl::PointXY> > projectCloudToCameras ( + const pcl::PointCloud & cloud, + const std::map & cameraPoses, + const std::map > & cameraModels, + float maxDistance, + float maxAngle, + const std::vector & roiRatios, + bool distanceToCamPolicy, + const ProgressState * state) +{ + std::vector, pcl::PointXY> > pointToPixel; + + if (cloud.empty() || cameraPoses.empty() || cameraModels.empty()) + return pointToPixel; + + std::string msg = uFormat("Computing visible points per cam (%d points, %d cams)", (int)cloud.size(), (int)cameraPoses.size()); + UINFO(msg.c_str()); + if(state && !state->callback(msg)) + { + //cancelled! + UWARN("Projecting to cameras cancelled!"); + return pointToPixel; + } + + std::vector > invertedIndex(cloud.size()); // For each point: list of cameras + int cameraProcessed = 0; + for(std::map::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter) + { + std::map >::const_iterator iter=cameraModels.begin(); + if(iter!=cameraModels.end() && !iter->second.empty()) + { + for(size_t i=0; isecond.size(); ++i) + { + Transform cameraTransform = (pter->second * iter->second[i].localTransform()); + UASSERT(!cameraTransform.isNull()); + cv::Mat cameraMatrixK = iter->second[i].K(); + UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3); + const cv::Size & imageSize = iter->second[i].imageSize(); + + float fx = cameraMatrixK.at(0,0); + float fy = cameraMatrixK.at(1,1); + float cx = cameraMatrixK.at(0,2); + float cy = cameraMatrixK.at(1,2); + + // depth: 2 channels UINT: [depthMM, indexPt] + cv::Mat registered = cv::Mat::zeros(imageSize, CV_32SC2); + Transform t = cameraTransform.inverse(); + + cv::Rect roi(0,0,imageSize.width, imageSize.height); + if(roiRatios.size()==4) + { + roi = util2d::computeRoi(imageSize, roiRatios); + } + + int count = 0; + for(size_t i=0; i 0.0f) + { + float invZ = 1.0f/z; + float dx = (fx*ptScan.x)*invZ + cx; + float dy = (fy*ptScan.y)*invZ + cy; + int dx_low = dx; + int dy_low = dy; + int dx_high = dx + 0.5f; + int dy_high = dy + 0.5f; + int zMM = z * 1000; + if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height)) + { + set = true; + cv::Vec2i &zReg = registered.at(dy_low, dx_low); + if(zReg[0] == 0 || zMM < zReg[0]) + { + zReg[0] = zMM; + zReg[1] = i; + } + } + if((dx_low != dx_high || dy_low != dy_high) && + uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height)) + { + set = true; + cv::Vec2i &zReg = registered.at(dy_high, dx_high); + if(zReg[0] == 0 || zMM < zReg[0]) + { + zReg[0] = zMM; + zReg[1] = i; + } + } + } + if(set) + { + count++; + } + } + if(count == 0) + { + registered = cv::Mat(); + UINFO("No points projected in camera %d/%d", pter->first, i); + } + else + { + UDEBUG("%d points projected in camera %d/%d", count, pter->first, i); + } + for(int u=0; u(v, u); + if(zReg[0] > 0) + { + ProjectionInfo info; + info.nodeID = pter->first; + info.cameraIndex = i; + info.uv.x = float(u)/float(imageSize.width); + info.uv.y = float(v)/float(imageSize.height); + info.distance = zReg[0]/1000.0f; + invertedIndex[zReg[1]].push_back(info); + } + } + } + } + } + + msg = uFormat("Processed camera %d/%d", (int)cameraProcessed+1, (int)cameraPoses.size()); + UINFO(msg.c_str()); + if(state && !state->callback(msg)) + { + //cancelled! + UWARN("Projecting to cameras cancelled!"); + return pointToPixel; + } + ++cameraProcessed; + } + + msg = uFormat("Select best camera for %d points...", (int)cloud.size()); + UINFO(msg.c_str()); + if(state && !state->callback(msg)) + { + //cancelled! + UWARN("Projecting to cameras cancelled!"); + return pointToPixel; + } + + pointToPixel.resize(invertedIndex.size()); + int colorized = 0; + + // For each point + for(size_t i=0; icallback(uFormat("%d/%d points projected to cameras (out of %d points)", colorized, i+1, (int)cloud.size()))) + { + //cancelled! + UWARN("Projecting to camera cancelled!"); + pointToPixel.clear(); + return pointToPixel; + } + } + + const pcl::PointXYZRGBNormal & pt = cloud.at(i); + int nodeID = -1; + int cameraIndex = -1; + float smallestWeight = std::numeric_limits::max(); + pcl::PointXY uv_coords; + for (size_t j = 0; j 0 && // is facing camera? + (maxAngle<=0 || angleToCam < maxAngle) && // is point normal perpendicular to camera? + (maxDistance<=0 || distanceToCam-1 && cameraIndex> -1) + { + pointToPixel[i].first.first = nodeID; + pointToPixel[i].first.second = cameraIndex; + pointToPixel[i].second = uv_coords; + ++colorized; + } + } + + UINFO("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size()); + + return pointToPixel; +} + bool isFinite(const cv::Point3f & pt) { return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z); diff --git a/guilib/include/rtabmap/gui/ExportCloudsDialog.h b/guilib/include/rtabmap/gui/ExportCloudsDialog.h index 7236de26bf..8faf088b9f 100644 --- a/guilib/include/rtabmap/gui/ExportCloudsDialog.h +++ b/guilib/include/rtabmap/gui/ExportCloudsDialog.h @@ -138,7 +138,7 @@ private Q_SLOTS: const std::map & cachedScans, const ParametersMap & parameters, bool & has2dScans) const; - void saveClouds(const QString & workingDirectory, const std::map & poses, const std::map::Ptr> & clouds, bool binaryMode = true); + void saveClouds(const QString & workingDirectory, const std::map & poses, const std::map::Ptr> & clouds, bool binaryMode = true, const std::vector > & pointToPixels = std::vector >()); void saveMeshes(const QString & workingDirectory, const std::map & poses, const std::map & meshes, bool binaryMode = true); void saveTextureMeshes(const QString & workingDirectory, const std::map & poses, std::map & textureMeshes, const QMap & cachedSignatures, const std::vector > & textureVertexToPixels); diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index b56fb035e9..0c03b679a7 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -173,9 +173,23 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->checkBox_blending, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->comboBox_blendingDecimation, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor())); + connect(_ui->lineEdit_camProjRoiRatios, SIGNAL(textChanged(const QString &)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_camProjMaxDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_camProjMaxAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_camProjDistanceToCamPolicy, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_camProjKeepPointsNotSeenByCameras, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_camProjRecolorPoints, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->comboBox_camProjExportCamera, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged())); +#ifndef RTABMAP_PDAL + _ui->comboBox_camProjExportCamera->setEnabled(false); + _ui->label_camProjExportCamera->setEnabled(false); + _ui->label_camProjExportCamera->setText(_ui->label_camProjExportCamera->text() + " (PDAL dependency required)"); +#endif + connect(_ui->checkBox_meshing, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->checkBox_meshing, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor())); - connect(_ui->checkBox_meshing, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor())); connect(_ui->doubleSpinBox_gp3Radius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_gp3Mu, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_meshDecimationFactor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); @@ -356,6 +370,15 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou settings.setValue("gain_rgb", _ui->checkBox_gainRGB->isChecked()); settings.setValue("gain_full", _ui->checkBox_gainFull->isChecked()); + settings.setValue("cam_proj", _ui->checkBox_cameraProjection->isChecked()); + settings.setValue("cam_proj_roi_ratios", _ui->lineEdit_camProjRoiRatios->text()); + settings.setValue("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value()); + settings.setValue("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value()); + settings.setValue("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked()); + settings.setValue("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked()); + settings.setValue("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked()); + settings.setValue("cam_proj_export_format", _ui->comboBox_camProjExportCamera->currentIndex()); + settings.setValue("mesh", _ui->checkBox_meshing->isChecked()); settings.setValue("mesh_radius", _ui->doubleSpinBox_gp3Radius->value()); settings.setValue("mesh_mu", _ui->doubleSpinBox_gp3Mu->value()); @@ -497,6 +520,15 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou _ui->checkBox_gainRGB->setChecked(settings.value("gain_rgb", _ui->checkBox_gainRGB->isChecked()).toBool()); _ui->checkBox_gainFull->setChecked(settings.value("gain_full", _ui->checkBox_gainFull->isChecked()).toBool()); + _ui->checkBox_cameraProjection->setChecked(settings.value("cam_proj", _ui->checkBox_cameraProjection->isChecked()).toBool()); + _ui->lineEdit_camProjRoiRatios->setText(settings.value("cam_proj_roi_ratios", _ui->lineEdit_camProjRoiRatios->text()).toString()); + _ui->doubleSpinBox_camProjMaxDistance->setValue(settings.value("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value()).toDouble()); + _ui->doubleSpinBox_camProjMaxAngle->setValue(settings.value("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value()).toDouble()); + _ui->checkBox_camProjDistanceToCamPolicy->setChecked(settings.value("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked()).toBool()); + _ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(settings.value("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked()).toBool()); + _ui->checkBox_camProjRecolorPoints->setChecked(settings.value("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked()).toBool()); + _ui->comboBox_camProjExportCamera->setCurrentIndex(settings.value("cam_proj_export_format", _ui->comboBox_camProjExportCamera->currentIndex()).toInt()); + _ui->checkBox_meshing->setChecked(settings.value("mesh", _ui->checkBox_meshing->isChecked()).toBool()); _ui->doubleSpinBox_gp3Radius->setValue(settings.value("mesh_radius", _ui->doubleSpinBox_gp3Radius->value()).toDouble()); _ui->doubleSpinBox_gp3Mu->setValue(settings.value("mesh_mu", _ui->doubleSpinBox_gp3Mu->value()).toDouble()); @@ -635,6 +667,15 @@ void ExportCloudsDialog::restoreDefaults() _ui->checkBox_gainRGB->setChecked(true); _ui->checkBox_gainFull->setChecked(false); + _ui->checkBox_cameraProjection->setChecked(false); + _ui->lineEdit_camProjRoiRatios->setText("0.0 0.0 0.0 0.0"); + _ui->doubleSpinBox_camProjMaxDistance->setValue(0); + _ui->doubleSpinBox_camProjMaxAngle->setValue(0); + _ui->checkBox_camProjDistanceToCamPolicy->setChecked(true); + _ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(false); + _ui->checkBox_camProjRecolorPoints->setChecked(true); + _ui->comboBox_camProjExportCamera->setCurrentIndex(0); + _ui->checkBox_meshing->setChecked(false); _ui->doubleSpinBox_gp3Radius->setValue(0.2); _ui->doubleSpinBox_gp3Mu->setValue(2.5); @@ -778,11 +819,15 @@ void ExportCloudsDialog::updateReconstructionFlavor() _ui->checkBox_gainCompensation->setVisible(_ui->checkBox_gainCompensation->isEnabled()); _ui->label_gainCompensation->setVisible(_ui->checkBox_gainCompensation->isEnabled()); + _ui->checkBox_cameraProjection->setEnabled(_ui->checkBox_assemble->isChecked() && !_ui->checkBox_meshing->isChecked()); + _ui->label_cameraProjection->setEnabled(_ui->checkBox_cameraProjection->isEnabled()); + _ui->groupBox_regenerate->setVisible(_ui->checkBox_regenerate->isChecked() && _ui->checkBox_fromDepth->isChecked()); _ui->groupBox_regenerateScans->setVisible(_ui->checkBox_regenerate->isChecked() && !_ui->checkBox_fromDepth->isChecked()); _ui->groupBox_bilateral->setVisible(_ui->checkBox_bilateral->isChecked()); _ui->groupBox_filtering->setVisible(_ui->checkBox_filtering->isChecked()); _ui->groupBox_gain->setVisible(_ui->checkBox_gainCompensation->isEnabled() && _ui->checkBox_gainCompensation->isChecked()); + _ui->groupBox_cameraProjection->setVisible(_ui->checkBox_cameraProjection->isEnabled() && _ui->checkBox_cameraProjection->isChecked()); _ui->groupBox_mls->setVisible(_ui->checkBox_smoothing->isEnabled() && _ui->checkBox_smoothing->isChecked()); _ui->groupBox_meshing->setVisible(_ui->checkBox_meshing->isChecked()); _ui->groupBox_subtraction->setVisible(_ui->checkBox_subtraction->isChecked()); @@ -952,7 +997,7 @@ void ExportCloudsDialog::exportClouds( } else { - saveClouds(workingDirectory, poses, clouds, _ui->checkBox_binary->isChecked()); + saveClouds(workingDirectory, poses, clouds, _ui->checkBox_binary->isChecked(), textureVertexToPixels); } } else if(!_canceled) @@ -1234,16 +1279,13 @@ void ExportCloudsDialog::viewClouds( _progressDialog->appendText(tr("Viewing the cloud %1 (%2 points)...").arg(iter->first).arg(iter->second->size())); _progressDialog->incrementStep(); - QColor color = Qt::gray; - int mapId = uValue(mapIds, iter->first, -1); - if(mapId >= 0) + if(!_ui->checkBox_fromDepth->isChecked() && + !(_ui->checkBox_cameraProjection->isEnabled() && + _ui->checkBox_cameraProjection->isChecked() && + _ui->checkBox_camProjRecolorPoints->isChecked() && + clouds.size()==1 && clouds.begin()->first==0)) { - color = (Qt::GlobalColor)(mapId % 12 + 7 ); - } - - if(!_ui->checkBox_fromDepth->isChecked()) - { - // When laser scans are exported, convert RGB to Intensity + // When laser scans are exported (and camera RGB was not applied), convert RGB to Intensity if(_ui->spinBox_normalKSearch->value()<=0 && _ui->doubleSpinBox_normalRadiusSearch->value()<=0.0) { // remove normals if not used @@ -2444,6 +2486,222 @@ bool ExportCloudsDialog::getExportedClouds( _progressDialog->appendText(msg.c_str(), Qt::darkYellow); UWARN(msg.c_str()); } + else if(_ui->checkBox_cameraProjection->isEnabled() && + _ui->checkBox_cameraProjection->isChecked() && + cloudsWithNormals.size()==1 && cloudsWithNormals.begin()->first==0) // only for assembled point cloud + { + _progressDialog->appendText(tr("Camera projection...")); + QApplication::processEvents(); + uSleep(100); + QApplication::processEvents(); + + pcl::PointCloud::Ptr assembledCloud = cloudsWithNormals.begin()->second; + + std::map cameraPoses; + std::map > cameraModels; + for(std::map::const_iterator iter=poses.lower_bound(0); iter!=poses.end(); ++iter) + { + std::vector models; + StereoCameraModel stereoModel; + if(cachedSignatures.contains(iter->first)) + { + const SensorData & data = cachedSignatures.find(iter->first)->sensorData(); + models = data.cameraModels(); + stereoModel = data.stereoCameraModel(); + } + else if(_dbDriver) + { + _dbDriver->getCalibration(iter->first, models, stereoModel); + } + + if(stereoModel.isValidForProjection()) + { + models.clear(); + models.push_back(stereoModel.left()); + } + else if(models.size() == 0 || !models[0].isValidForProjection()) + { + models.clear(); + } + + if(!models.empty() && models[0].imageWidth() != 0 && models[0].imageHeight() != 0) + { + cameraPoses.insert(std::make_pair(iter->first, iter->second)); + cameraModels.insert(std::make_pair(iter->first, models)); + } + else + { + UWARN("%d node has invalid camera models, ignoring it.", iter->first); + } + } + if(!cameraPoses.empty()) + { + TexturingState texturingState(_progressDialog, true); + _progressDialog->setMaximumSteps(_progressDialog->maximumSteps() + assembledCloud->size()/10000+1 + cameraPoses.size()); + std::vector roiRatios; + QStringList strings = _ui->lineEdit_camProjRoiRatios->text().split(' '); + if(!_ui->lineEdit_meshingTextureRoiRatios->text().isEmpty()) + { + if(strings.size()==4) + { + roiRatios.resize(4); + roiRatios[0]=strings[0].toDouble(); + roiRatios[1]=strings[1].toDouble(); + roiRatios[2]=strings[2].toDouble(); + roiRatios[3]=strings[3].toDouble(); + if(!(roiRatios[0]>=0.0f && roiRatios[0]<=1.0f && + roiRatios[1]>=0.0f && roiRatios[1]<=1.0f && + roiRatios[2]>=0.0f && roiRatios[2]<=1.0f && + roiRatios[3]>=0.0f && roiRatios[3]<=1.0f)) + { + roiRatios.clear(); + } + } + if(roiRatios.empty()) + { + QString msg = tr("Wrong ROI format. Region of Interest (ROI) must " + "have 4 values [left right top bottom] between 0 and 1 " + "separated by space (%1), ignoring it for projecting cameras...") + .arg(_ui->lineEdit_camProjRoiRatios->text()); + UWARN(msg.toStdString().c_str()); + _progressDialog->appendText(msg, Qt::darkYellow); + _progressDialog->setAutoClose(false); + } + } + std::vector, pcl::PointXY> > pointToPixel; + pointToPixel = util3d::projectCloudToCameras( + *assembledCloud, + cameraPoses, + cameraModels, + _ui->doubleSpinBox_camProjMaxDistance->value(), + _ui->doubleSpinBox_camProjMaxAngle->value()*M_PI/180.0, + roiRatios, + _ui->checkBox_camProjDistanceToCamPolicy->isChecked(), + &texturingState); + + // color the cloud + UASSERT(pointToPixel.empty() || pointToPixel.size() == assembledCloud->size()); + QMap cachedImages; + pcl::PointCloud::Ptr assembledCloudValidPoints; + if(!_ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked()) + { + assembledCloudValidPoints.reset(new pcl::PointCloud()); + assembledCloudValidPoints->resize(assembledCloud->size()); + } + if(_ui->comboBox_camProjExportCamera->isEnabled() && + _ui->comboBox_camProjExportCamera->currentIndex()>0) + { + textureVertexToPixels.resize(assembledCloud->size()); + } + int oi=0; + for(size_t i=0; iat(i); + if(_ui->checkBox_camProjRecolorPoints->isChecked() && !_ui->checkBox_fromDepth->isChecked()) + { + pt.r = 255; + pt.g = 0; + pt.b = 0; + pt.a = 255; + } + int nodeID = pointToPixel[i].first.first; + int cameraIndex = pointToPixel[i].first.second; + if(nodeID>0 && cameraIndex>=0) + { + if(_ui->checkBox_camProjRecolorPoints->isChecked()) + { + cv::Mat image; + if(cachedImages.contains(nodeID)) + { + image = cachedImages.value(nodeID); + } + else if(cachedSignatures.contains(nodeID) && !cachedSignatures.value(nodeID).sensorData().imageCompressed().empty()) + { + cachedSignatures.value(nodeID).sensorData().uncompressDataConst(&image, 0); + cachedImages.insert(nodeID, image); + } + else if(_dbDriver) + { + SensorData data; + _dbDriver->getNodeData(nodeID, data, true, false, false, false); + data.uncompressDataConst(&image, 0); + cachedImages.insert(nodeID, image); + } + + if(!image.empty()) + { + int subImageWidth = image.cols / cameraModels.at(nodeID).size(); + image = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth)); + + + int x = pointToPixel[i].second.x * (float)image.cols; + int y = pointToPixel[i].second.y * (float)image.rows; + UASSERT(x>=0 && x=0 && y(y, x); + pt.b = bgr[0]; + pt.g = bgr[1]; + pt.r = bgr[2]; + } + else + { + UASSERT(image.type()==CV_8UC1); + pt.r = pt.g = pt.b = image.at(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols); + } + } + } + + int exportedId = nodeID; + if(_ui->comboBox_camProjExportCamera->currentIndex() == 2) + { + exportedId = cameraIndex+1; + } + else if(_ui->comboBox_camProjExportCamera->currentIndex() == 3) + { + UASSERT_MSG(cameraIndex<10, "Exporting cam id as NodeId+CamIndex is only supported when the number of cameras per node < 10."); + exportedId = nodeID*10 + cameraIndex; + } + + if(assembledCloudValidPoints.get()) + { + if(!textureVertexToPixels.empty()) + { + textureVertexToPixels[oi].insert(std::make_pair(exportedId, pointToPixel[i].second)); + } + assembledCloudValidPoints->at(oi++) = pt; + } + else if(!textureVertexToPixels.empty()) + { + textureVertexToPixels[i].insert(std::make_pair(exportedId, pointToPixel[i].second)); + } + } + } + + //[FATAL] (2021-02-17 14:47:28.891) PDALWriter.cpp:140::savePDALFile() Condition (cameraIds.empty() || cameraIds.size() == cloud.size()) not met! [cameraIds=254890 cloud=262366] + //terminate called after throwing an instance of 'UException' + // what(): [FATAL] (2021-02-17 14:47:28.891) PDALWriter.cpp:140::savePDALFile() Condition (cameraIds.empty() || cameraIds.size() == cloud.size()) not met! [cameraIds=254890 cloud=262366] + + + if(assembledCloudValidPoints.get()) + { + assembledCloudValidPoints->resize(oi); + assembledCloud = assembledCloudValidPoints; + + if(!textureVertexToPixels.empty()) + { + textureVertexToPixels.resize(oi); + } + } + + if(_canceled) + { + return false; + } + } + } UDEBUG(""); #ifdef RTABMAP_CPUTSDF @@ -2933,7 +3191,10 @@ bool ExportCloudsDialog::getExportedClouds( } if(roiRatios.empty()) { - QString msg = tr("Wrong ROI format. Region of Interest (ROI) must have 4 values [left right top bottom] between 0 and 1 separated by space (%1), ignoring it for texturing...").arg(_ui->lineEdit_meshingTextureRoiRatios->text()); + QString msg = tr("Wrong ROI format. Region of Interest (ROI) must have 4 " + "values [left right top bottom] between 0 and 1 " + "separated by space (%1), ignoring it for texturing...") + .arg(_ui->lineEdit_meshingTextureRoiRatios->text()); UWARN(msg.toStdString().c_str()); _progressDialog->appendText(msg, Qt::darkYellow); _progressDialog->setAutoClose(false); @@ -3015,8 +3276,8 @@ bool ExportCloudsDialog::getExportedClouds( textureMeshes.clear(); textureMeshes.insert(std::make_pair(0, assembledMesh)); } - } + UDEBUG(""); return true; } @@ -3401,7 +3662,8 @@ void ExportCloudsDialog::saveClouds( const QString & workingDirectory, const std::map & poses, const std::map::Ptr> & clouds, - bool binaryMode) + bool binaryMode, + const std::vector > & pointToPixels) { if(clouds.size() == 1) { @@ -3435,9 +3697,13 @@ void ExportCloudsDialog::saveClouds( pcl::PointCloud::Ptr cloudRGBWithoutNormals; pcl::PointCloud::Ptr cloudIWithoutNormals; pcl::PointCloud::Ptr cloudIWithNormals; - if(!_ui->checkBox_fromDepth->isChecked()) + if(!_ui->checkBox_fromDepth->isChecked() && + !(_ui->checkBox_cameraProjection->isEnabled() && + _ui->checkBox_cameraProjection->isChecked() && + _ui->checkBox_camProjRecolorPoints->isChecked() && + clouds.size()==1 && clouds.begin()->first==0)) { - // When laser scans are exported, convert RGB to Intensity + // When laser scans are exported (and camera RGB was not applied), convert RGB to Intensity if(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0) { cloudIWithNormals.reset(new pcl::PointCloud); @@ -3505,8 +3771,11 @@ void ExportCloudsDialog::saveClouds( success = pcl::io::savePCDFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0; } } - else if(QFileInfo(path).suffix() == "ply") - { +#ifdef RTABMAP_PDAL + else if(QFileInfo(path).suffix() == "ply" && pointToPixels.empty()) { +#else + else if(QFileInfo(path).suffix() == "ply") { +#endif if(cloudIWithNormals.get()) { success = pcl::io::savePLYFile(path.toStdString(), *cloudIWithNormals, binaryMode) == 0; @@ -3527,21 +3796,29 @@ void ExportCloudsDialog::saveClouds( #ifdef RTABMAP_PDAL else if(!QFileInfo(path).suffix().isEmpty()) { + std::vector cameraIds(pointToPixels.size(), 0); + for(size_t i=0;ifirst; + } + } if(cloudIWithNormals.get()) { - success = savePDALFile(path.toStdString(), *cloudIWithNormals) == 0; + success = savePDALFile(path.toStdString(), *cloudIWithNormals, cameraIds) == 0; } else if(cloudIWithoutNormals.get()) { - success = savePDALFile(path.toStdString(), *cloudIWithoutNormals) == 0; + success = savePDALFile(path.toStdString(), *cloudIWithoutNormals, cameraIds) == 0; } else if(cloudRGBWithoutNormals.get()) { - success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals) == 0; + success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals, cameraIds) == 0; } else { - success = savePDALFile(path.toStdString(), *clouds.begin()->second) == 0; + success = savePDALFile(path.toStdString(), *clouds.begin()->second, cameraIds) == 0; } } #endif diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index fc4968372a..5761fcd7b3 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -23,9 +23,9 @@ 0 - 0 + -1949 780 - 4975 + 5347 @@ -76,13 +76,6 @@ - - - - - - - @@ -103,16 +96,6 @@ - - - - Gain compensation. Normalize brightness of images. - - - true - - - @@ -133,16 +116,6 @@ - - - - Meshing. - - - true - - - @@ -160,13 +133,6 @@ - - - - - - - @@ -323,6 +289,57 @@ + + + + Gain compensation. Normalize brightness of images. + + + true + + + + + + + Camera projection. This can be used to colorize point cloud created from scans and/or export camera IDs for each point of the cloud. + + + true + + + + + + + + + + + + + + + + + + + + + Meshing. + + + true + + + + + + + + + + @@ -1475,6 +1492,185 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. + + + + Camera Projection + + + false + + + + + + deg + + + 0 + + + 0.000000000000000 + + + 180.000000000000000 + + + 1.000000000000000 + + + 0.000000000000000 + + + + + + + Keep points not seen by the cameras. These points will be set with a pure red color (255,0,0) if the cloud was created from laser scans. + + + true + + + + + + + + + + + + + + Distance to camera policy. The closest camera from a point is used to colorize the point. If disabled, the camera for which the point projection is the closest of the image center is used to colorize the point. + + + true + + + + + + + Maximum distance from the camera for points to be colorized by this camera (0 means infinite). + + + true + + + + + + + Maximum angle between camera and point's normal to apply color (0 means disabled). + + + true + + + + + + + m + + + 1 + + + 0.000000000000000 + + + 99.000000000000000 + + + 1.000000000000000 + + + 0.000000000000000 + + + + + + + + + + + + + + ID format of the camera selected for each point. + + + true + + + + + + + By Node ID: cameras of same node have same ID +By Camera Index: of a multi-cameras setup +By Node ID and Camera Index: NodeID*10+CameraIndex + + + + Disabled + + + + + By Node ID + + + + + By Camera Index + + + + + By Node ID and Camera Index + + + + + + + + + + + ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. + + + true + + + + + + + Recolor points from camera projection. This would be used to color laser scans with the cameras. + + + true + + + + + + + + + + + + + From 7a8b88417b9a4420cbd4e75232b339277af15d35 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 23 Feb 2021 18:39:51 -0500 Subject: [PATCH 2/4] ExportClouds: fixed ceiling/floor filtering options not saved in config --- guilib/src/ExportCloudsDialog.cpp | 13 +++++++++++++ guilib/src/ui/exportCloudsDialog.ui | 12 +++++++++--- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 0c03b679a7..52101a757f 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -115,6 +115,8 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_ceilingHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_floorHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->spinBox_decimation_scan, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_rangeMin, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_rangeMax, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); @@ -327,6 +329,8 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value()); settings.setValue("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value()); settings.setValue("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value()); + settings.setValue("regenerate_ceiling", _ui->doubleSpinBox_ceilingHeight->value()); + settings.setValue("regenerate_floor", _ui->doubleSpinBox_floorHeight->value()); settings.setValue("regenerate_scan_decimation", _ui->spinBox_decimation_scan->value()); settings.setValue("regenerate_scan_max_range", _ui->doubleSpinBox_rangeMax->value()); settings.setValue("regenerate_scan_min_range", _ui->doubleSpinBox_rangeMin->value()); @@ -474,6 +478,8 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou _ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt()); _ui->doubleSpinBox_maxDepth->setValue(settings.value("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble()); _ui->doubleSpinBox_minDepth->setValue(settings.value("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble()); + _ui->doubleSpinBox_ceilingHeight->setValue(settings.value("regenerate_ceiling", _ui->doubleSpinBox_ceilingHeight->value()).toDouble()); + _ui->doubleSpinBox_floorHeight->setValue(settings.value("regenerate_floor", _ui->doubleSpinBox_floorHeight->value()).toDouble()); _ui->spinBox_decimation_scan->setValue(settings.value("regenerate_scan_decimation", _ui->spinBox_decimation_scan->value()).toInt()); _ui->doubleSpinBox_rangeMax->setValue(settings.value("regenerate_scan_max_range", _ui->doubleSpinBox_rangeMax->value()).toDouble()); _ui->doubleSpinBox_rangeMin->setValue(settings.value("regenerate_scan_min_range", _ui->doubleSpinBox_rangeMin->value()).toDouble()); @@ -624,6 +630,8 @@ void ExportCloudsDialog::restoreDefaults() _ui->spinBox_decimation->setValue(1); _ui->doubleSpinBox_maxDepth->setValue(4); _ui->doubleSpinBox_minDepth->setValue(0); + _ui->doubleSpinBox_ceilingHeight->setValue(0); + _ui->doubleSpinBox_floorHeight->setValue(0); _ui->spinBox_decimation_scan->setValue(1); _ui->doubleSpinBox_rangeMax->setValue(0); _ui->doubleSpinBox_rangeMin->setValue(0); @@ -1751,6 +1759,11 @@ bool ExportCloudsDialog::getExportedClouds( if(!_ui->checkBox_fromDepth->isChecked() && !has2dScans && (_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0)) { + _progressDialog->appendText(tr("Computing normals (%1 points, K=%2, radius=%3 m)...") + .arg(assembledCloud->size()) + .arg(_ui->spinBox_normalKSearch->value()) + .arg(_ui->doubleSpinBox_normalRadiusSearch->value())); + // recompute normals pcl::PointCloud::Ptr cloudWithoutNormals(new pcl::PointCloud); pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals); diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index 5761fcd7b3..6caff34e55 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -23,7 +23,7 @@ 0 - -1949 + -584 780 5347 @@ -659,8 +659,11 @@ 2 + + -1000.000000000000000 + - 100.000000000000000 + 1000.000000000000000 0.100000000000000 @@ -678,8 +681,11 @@ 2 + + -1000.000000000000000 + - 100.000000000000000 + 1000.000000000000000 0.100000000000000 From 14cc7dabc7988972e79e0b33fa61a1110b5a164f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 24 Feb 2021 18:28:28 -0500 Subject: [PATCH 3/4] ExportClouds: fixed colorless scan points still exported when option is unchecked. --- guilib/src/ExportCloudsDialog.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 52101a757f..fd54764402 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -2693,15 +2693,10 @@ bool ExportCloudsDialog::getExportedClouds( } } - //[FATAL] (2021-02-17 14:47:28.891) PDALWriter.cpp:140::savePDALFile() Condition (cameraIds.empty() || cameraIds.size() == cloud.size()) not met! [cameraIds=254890 cloud=262366] - //terminate called after throwing an instance of 'UException' - // what(): [FATAL] (2021-02-17 14:47:28.891) PDALWriter.cpp:140::savePDALFile() Condition (cameraIds.empty() || cameraIds.size() == cloud.size()) not met! [cameraIds=254890 cloud=262366] - - if(assembledCloudValidPoints.get()) { assembledCloudValidPoints->resize(oi); - assembledCloud = assembledCloudValidPoints; + cloudsWithNormals.begin()->second = assembledCloudValidPoints; if(!textureVertexToPixels.empty()) { From b635a9cc58fc5b9d06eb5f6863453668c3b3f2ce Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 28 Feb 2021 16:08:30 -0500 Subject: [PATCH 4/4] Export tool: added --bin, --poses, --images, --las and --cam_projection options; export with intensity with --scan option. PDALWriter: added binary option (only used for PLY an PCD formats). Rtabmap: Do graph optimization if neighbor link refined and Mem/UseOdomGravity is used. --- corelib/include/rtabmap/core/PDALWriter.h | 10 +- corelib/include/rtabmap/core/util3d.h | 23 +- corelib/src/PDALWriter.cpp | 50 +++- corelib/src/Rtabmap.cpp | 6 +- corelib/src/util3d.cpp | 49 +++- guilib/src/ExportCloudsDialog.cpp | 8 +- tools/Export/main.cpp | 332 +++++++++++++++++++--- 7 files changed, 415 insertions(+), 63 deletions(-) diff --git a/corelib/include/rtabmap/core/PDALWriter.h b/corelib/include/rtabmap/core/PDALWriter.h index c13cde27bf..a4ed246e6f 100644 --- a/corelib/include/rtabmap/core/PDALWriter.h +++ b/corelib/include/rtabmap/core/PDALWriter.h @@ -35,11 +35,11 @@ namespace rtabmap { std::string getPDALSupportedWriters(); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector()); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector()); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector()); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector()); -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = 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); +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); } diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index e155e7fab1..6b9309223c 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -309,11 +309,24 @@ std::vector, pcl::PointXY> > RTABMAP_EXP projectC const pcl::PointCloud & cloud, const std::map & cameraPoses, const std::map > & cameraModels, - float maxDistance, - float maxAngle, - const std::vector & roiRatios, - bool distanceToCamPolicy, - const ProgressState * state); + float maxDistance = 0.0f, + float maxAngle = 0.0f, + const std::vector & roiRatios = std::vector(), + bool distanceToCamPolicy = false, + const ProgressState * state = 0); +/** + * For each point, return pixel of the best camera (NodeID->CameraIndex) + * looking at it based on the policy and parameters + */ +std::vector, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras ( + const pcl::PointCloud & cloud, + const std::map & cameraPoses, + const std::map > & cameraModels, + float maxDistance = 0.0f, + float maxAngle = 0.0f, + const std::vector & roiRatios = std::vector(), + bool distanceToCamPolicy = false, + const ProgressState * state = 0); bool RTABMAP_EXP isFinite(const cv::Point3f & pt); diff --git a/corelib/src/PDALWriter.cpp b/corelib/src/PDALWriter.cpp index d9b568ba2e..de9381363c 100644 --- a/corelib/src/PDALWriter.cpp +++ b/corelib/src/PDALWriter.cpp @@ -73,7 +73,10 @@ std::string getPDALSupportedWriters() return output; } -int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds) +int savePDALFile(const std::string & filePath, + const pcl::PointCloud & cloud, + const std::vector & cameraIds, + bool binary) { UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); @@ -111,11 +114,14 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloudsetOptions(writerOps); writer->setInput(bufferReader); @@ -134,7 +140,10 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds) +int savePDALFile(const std::string & filePath, + const pcl::PointCloud & cloud, + const std::vector & cameraIds, + bool binary) { UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); @@ -181,11 +190,14 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloudsetOptions(writerOps); writer->setInput(bufferReader); @@ -204,7 +216,10 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds) +int savePDALFile(const std::string & filePath, + const pcl::PointCloud & cloud, + const std::vector & cameraIds, + bool binary) { UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); @@ -260,11 +275,14 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloudsetOptions(writerOps); writer->setInput(bufferReader); @@ -283,7 +301,10 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds) +int savePDALFile(const std::string & filePath, + const pcl::PointCloud & cloud, + const std::vector & cameraIds, + bool binary) { UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); @@ -324,11 +345,14 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloudsetOptions(writerOps); writer->setInput(bufferReader); @@ -347,7 +371,10 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds) +int savePDALFile(const std::string & filePath, + const pcl::PointCloud & cloud, + const std::vector & cameraIds, + bool binary) { UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); @@ -397,11 +424,14 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloudsetOptions(writerOps); writer->setInput(bufferReader); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index a18db4aa90..922a749740 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1235,6 +1235,7 @@ bool Rtabmap::process( bool smallDisplacement = false; bool tooFastMovement = false; std::list signaturesRemoved; + bool neighborLinkRefined = false; if(_rgbdSlamMode) { statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at(0,0)); @@ -1363,7 +1364,8 @@ bool Rtabmap::process( _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv())); } } - statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0); + neighborLinkRefined = !t.isNull(); + statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(),neighborLinkRefined?1.0f:0); statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers); statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio); statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation); @@ -2686,7 +2688,7 @@ bool Rtabmap::process( lastProximitySpaceClosureId>0 || // can be different map of the current one statistics_.reducedIds().size() || (signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge - (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && !_memory->isOdomGravityUsed()) || // gravity edge + (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && (!_memory->isOdomGravityUsed() || neighborLinkRefined)) || // gravity edge proximityDetectionsInTimeFound>0 || landmarkDetected!=0 || signaturesRetrieved.size()) // can be different map of the current one diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index eed1931b96..632eb10483 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -3115,8 +3115,9 @@ struct ProjectionInfo { * For each point, return pixel of the best camera (NodeID->CameraIndex) * looking at it based on the policy and parameters */ -std::vector, pcl::PointXY> > projectCloudToCameras ( - const pcl::PointCloud & cloud, +template +std::vector, pcl::PointXY> > projectCloudToCamerasImpl ( + const typename pcl::PointCloud & cloud, const std::map & cameraPoses, const std::map > & cameraModels, float maxDistance, @@ -3173,7 +3174,7 @@ std::vector, pcl::PointXY> > projectCloudToCamera for(size_t i=0; i, pcl::PointXY> > projectCloudToCamera } } - const pcl::PointXYZRGBNormal & pt = cloud.at(i); + const PointT & pt = cloud.at(i); int nodeID = -1; int cameraIndex = -1; float smallestWeight = std::numeric_limits::max(); @@ -3332,6 +3333,46 @@ std::vector, pcl::PointXY> > projectCloudToCamera return pointToPixel; } +std::vector, pcl::PointXY> > projectCloudToCameras ( + const typename pcl::PointCloud & cloud, + const std::map & cameraPoses, + const std::map > & cameraModels, + float maxDistance, + float maxAngle, + const std::vector & roiRatios, + bool distanceToCamPolicy, + const ProgressState * state) +{ + return projectCloudToCamerasImpl(cloud, + cameraPoses, + cameraModels, + maxDistance, + maxAngle, + roiRatios, + distanceToCamPolicy, + state); +} + +std::vector, pcl::PointXY> > projectCloudToCameras ( + const typename pcl::PointCloud & cloud, + const std::map & cameraPoses, + const std::map > & cameraModels, + float maxDistance, + float maxAngle, + const std::vector & roiRatios, + bool distanceToCamPolicy, + const ProgressState * state) +{ + return projectCloudToCamerasImpl(cloud, + cameraPoses, + cameraModels, + maxDistance, + maxAngle, + roiRatios, + distanceToCamPolicy, + state); +} + bool isFinite(const cv::Point3f & pt) { return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z); diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index fd54764402..6771f7bc74 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -3814,19 +3814,19 @@ void ExportCloudsDialog::saveClouds( } if(cloudIWithNormals.get()) { - success = savePDALFile(path.toStdString(), *cloudIWithNormals, cameraIds) == 0; + success = savePDALFile(path.toStdString(), *cloudIWithNormals, cameraIds, binaryMode) == 0; } else if(cloudIWithoutNormals.get()) { - success = savePDALFile(path.toStdString(), *cloudIWithoutNormals, cameraIds) == 0; + success = savePDALFile(path.toStdString(), *cloudIWithoutNormals, cameraIds, binaryMode) == 0; } else if(cloudRGBWithoutNormals.get()) { - success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals, cameraIds) == 0; + success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals, cameraIds, binaryMode) == 0; } else { - success = savePDALFile(path.toStdString(), *clouds.begin()->second, cameraIds) == 0; + success = savePDALFile(path.toStdString(), *clouds.begin()->second, cameraIds, binaryMode) == 0; } } #endif diff --git a/tools/Export/main.cpp b/tools/Export/main.cpp index 1069432f30..27d730353e 100644 --- a/tools/Export/main.cpp +++ b/tools/Export/main.cpp @@ -31,7 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include +#include #include #include #include @@ -43,19 +45,28 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef RTABMAP_PDAL +#include +#endif + using namespace rtabmap; void showUsage() { printf("\nUsage:\n" - "rtabmap-exportCloud [options] database.db\n" + "rtabmap-export [options] database.db\n" "Options:\n" + " --bin Export PLY in binary format.\n" + " --las Export cloud in LAS instead of PLY (PDAL dependency required).\n" " --mesh Create a mesh.\n" " --texture Create a mesh with texture.\n" " --texture_size # Texture size (default 4096).\n" " --texture_count # Maximum textures generated (default 1).\n" " --texture_range # Maximum camera range for texturing a polygon (default 0 meters: no limit).\n" " --texture_d2c Distance to camera policy.\n" + " --cam_projection Camera projection on assembled cloud and export node ID on each point (in PointSourceId field).\n" + " --poses Export optimized poses in RGB-D SLAM dataset format (stamp x y z qx qy qz qw).\n" + " --images Export images.\n" " --ba Do global bundle adjustment before assembling the clouds.\n" " --gain # Gain compensation value (default 1, set 0 to disable).\n" " --gain_gray Do gain estimation compensation on gray channel only (default RGB channels).\n" @@ -87,6 +98,8 @@ int main(int argc, char * argv[]) showUsage(); } + bool binary = false; + bool las = false; bool mesh = false; bool texture = false; bool ba = false; @@ -109,12 +122,28 @@ int main(int argc, char * argv[]) bool saveInDb = false; int lowBrightnessGain = 0; int highBrightnessGain = 10; + bool camProjection = false; + bool exportPoses = false; + bool exportImages = false; for(int i=1; i::Ptr mergedClouds(new pcl::PointCloud); + pcl::PointCloud::Ptr mergedCloudsI(new pcl::PointCloud); std::map cameraPoses; + std::map cameraStamps; std::map > cameraModels; std::map cameraDepths; - + int imagesExported = 0; for(std::map::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter) { Signature node = nodes.find(iter->first)->second; // uncompress data std::vector models = node.sensorData().cameraModels(); + cv::Mat rgb; cv::Mat depth; pcl::IndicesPtr indices(new std::vector); pcl::PointCloud::Ptr cloud; + pcl::PointCloud::Ptr cloudI; if(cloudFromScan) { cv::Mat tmpDepth; LaserScan scan; - node.sensorData().uncompressData(0, texture&&!node.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0, &scan); + node.sensorData().uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!node.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0, &scan); if(decimation>1 || maxRange) { scan = util3d::commonFiltering(scan, decimation, 0, maxRange); } - cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform()); + if(scan.hasRGB()) + { + cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform()); + } + else + { + cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform()); + } } else { - cv::Mat tmpRGB; - node.sensorData().uncompressData(&tmpRGB, &depth); + node.sensorData().uncompressData(&rgb, &depth); cloud = util3d::cloudRGBFromSensorData( node.sensorData(), decimation, // image decimation before creating the clouds @@ -445,11 +496,55 @@ int main(int argc, char * argv[]) 0.0f, indices.get()); } + + if(exportImages && !rgb.empty()) + { + std::string dirSuffix = (depth.type() != CV_16UC1 && depth.type() != CV_32FC1)?"rgb":"left"; + std::string dir = outputDirectory+"/"+baseName+"_"+dirSuffix; + UDirectory::makeDir(dir); + std::string outputPath=dir+"/"+uNumber2Str(iter->first)+".jpg"; + cv::imwrite(outputPath, rgb); + ++imagesExported; + if(!depth.empty()) + { + std::string ext; + cv::Mat depthExported = depth; + if(depth.type() != CV_16UC1 && depth.type() != CV_32FC1) + { + ext = ".jpg"; + dir = outputDirectory+"/"+baseName+"_right"; + } + else + { + ext = ".png"; + dir = outputDirectory+"/"+baseName+"_depth"; + if(depth.type() == CV_32FC1) + { + depthExported = rtabmap::util2d::cvtDepthFromFloat(depth); + } + } + if(!UDirectory::exists(dir)) + { + UDirectory::makeDir(dir); + } + + outputPath=dir+"/"+uNumber2Str(iter->first)+ext; + cv::imwrite(outputPath, depthExported); + } + } + if(voxelSize>0.0f) { - cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize); + if(cloud.get() && !cloud->empty()) + cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize); + else if(cloudI.get() && !cloudI->empty()) + cloudI = rtabmap::util3d::voxelize(cloudI, indices, voxelSize); } - cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second); + if(cloud.get() && !cloud->empty()) + cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second); + else if(cloudI.get() && !cloudI->empty()) + cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second); + Eigen::Vector3f viewpoint(iter->second.x(), iter->second.y(), iter->second.z()); if(cloudFromScan) @@ -457,21 +552,37 @@ int main(int argc, char * argv[]) Transform lidarViewpoint = iter->second * node.sensorData().laserScanRaw().localTransform(); viewpoint = Eigen::Vector3f(iter->second.x(), iter->second.y(), iter->second.z()); } - pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint); - - pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); - pcl::concatenateFields(*cloud, *normals, *cloudWithNormals); - - if(mergedClouds->size() == 0) + if(cloud.get() && !cloud->empty()) { - *mergedClouds = *cloudWithNormals; + pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint); + pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); + pcl::concatenateFields(*cloud, *normals, *cloudWithNormals); + if(mergedClouds->size() == 0) + { + *mergedClouds = *cloudWithNormals; + } + else + { + *mergedClouds += *cloudWithNormals; + } } - else + else if(cloudI.get() && !cloudI->empty()) { - *mergedClouds += *cloudWithNormals; + pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(cloudI, 20, 0.0f, viewpoint); + pcl::PointCloud::Ptr cloudIWithNormals(new pcl::PointCloud); + pcl::concatenateFields(*cloudI, *normals, *cloudIWithNormals); + if(mergedCloudsI->size() == 0) + { + *mergedCloudsI = *cloudIWithNormals; + } + else + { + *mergedCloudsI += *cloudIWithNormals; + } } cameraPoses.insert(std::make_pair(iter->first, iter->second)); + cameraStamps.insert(std::make_pair(iter->first, node.getStamp())); if(!models.empty()) { cameraModels.insert(std::make_pair(iter->first, models)); @@ -481,9 +592,12 @@ int main(int argc, char * argv[]) cameraDepths.insert(std::make_pair(iter->first, depth)); } } - printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), (int)mergedClouds->size()); + printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !mergedClouds->empty()?(int)mergedClouds->size():(int)mergedCloudsI->size()); - if(mergedClouds->size()) + if(imagesExported>0) + printf("%d images exported!\n", imagesExported); + + if(!mergedClouds->empty() || !mergedCloudsI->empty()) { if(saveInDb) { @@ -495,31 +609,180 @@ int main(int argc, char * argv[]) driver->save2DMap(cv::Mat(), 0, 0, 0); driver->saveOptimizedPoses(optimizedPoses, lastlocalizationPose); } + else if(exportPoses) + { + std::string outputPath=outputDirectory+"/"+baseName+"_poses.txt"; + rtabmap::graph::exportPoses(outputPath, 10, cameraPoses, std::multimap(), cameraStamps); + printf("Poses exported to \"%s\".\n", outputPath.c_str()); + } - if(!(mesh || texture)) + pcl::PointCloud::Ptr cloudToExport = mergedClouds; + pcl::PointCloud::Ptr cloudIToExport = mergedCloudsI; + if(voxelSize>0.0f) { - if(voxelSize>0.0f) + printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size()); + if(!cloudToExport->empty()) + { + cloudToExport = util3d::voxelize(cloudToExport, voxelSize); + } + else if(!cloudIToExport->empty()) { - printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", voxelSize, (int)mergedClouds->size()); - mergedClouds = util3d::voxelize(mergedClouds, voxelSize); + cloudIToExport = util3d::voxelize(cloudIToExport, voxelSize); } + printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size()); + } - if(saveInDb) + std::vector pointToCamId; + if(camProjection && !cameraPoses.empty()) + { + printf("Camera projection...\n"); + pointToCamId.resize(!cloudToExport->empty()?cloudToExport->size():cloudIToExport->size()); + std::vector, pcl::PointXY> > pointToPixel; + if(!cloudToExport->empty()) { - printf("Saving in db... (%d points)\n", (int)mergedClouds->size()); - driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*mergedClouds, Transform(), false).data()); - printf("Saving in db... done!\n"); + pointToPixel = util3d::projectCloudToCameras( + *cloudToExport, + cameraPoses, + cameraModels, + textureRange, + 0, + std::vector(), + distanceToCamPolicy); } - else + else if(!cloudIToExport->empty()) { - std::string outputPath=outputDirectory+"/"+baseName+"_cloud.ply"; - printf("Saving %s... (%d points)\n", outputPath.c_str(), (int)mergedClouds->size()); - pcl::io::savePLYFile(outputPath, *mergedClouds); - printf("Saving %s... done!\n", outputPath.c_str()); + pointToPixel = util3d::projectCloudToCameras( + *cloudIToExport, + cameraPoses, + cameraModels, + textureRange, + 0, + std::vector(), + distanceToCamPolicy); + } + + // color the cloud + UASSERT(pointToPixel.empty() || pointToPixel.size() == pointToCamId.size()); + std::map cachedImages; + pcl::PointCloud::Ptr assembledCloudValidPoints(new pcl::PointCloud()); + assembledCloudValidPoints->resize(pointToCamId.size()); + + int oi=0; + for(size_t i=0; iempty()) + { + pt = cloudToExport->at(i); + } + else if(!cloudIToExport->empty()) + { + pt.x = cloudIToExport->at(i).x; + pt.y = cloudIToExport->at(i).y; + pt.z = cloudIToExport->at(i).z; + pt.normal_x = cloudIToExport->at(i).normal_x; + pt.normal_y = cloudIToExport->at(i).normal_y; + pt.normal_z = cloudIToExport->at(i).normal_z; + } + int nodeID = pointToPixel[i].first.first; + int cameraIndex = pointToPixel[i].first.second; + if(nodeID>0 && cameraIndex>=0) + { + cv::Mat image; + if(uContains(cachedImages, nodeID)) + { + image = cachedImages.at(nodeID); + } + else if(uContains(nodes,nodeID) && !nodes.at(nodeID).sensorData().imageCompressed().empty()) + { + nodes.at(nodeID).sensorData().uncompressDataConst(&image, 0); + cachedImages.insert(std::make_pair(nodeID, image)); + } + + if(!image.empty()) + { + int subImageWidth = image.cols / cameraModels.at(nodeID).size(); + image = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth)); + + + int x = pointToPixel[i].second.x * (float)image.cols; + int y = pointToPixel[i].second.y * (float)image.rows; + UASSERT(x>=0 && x=0 && y(y, x); + pt.b = bgr[0]; + pt.g = bgr[1]; + pt.r = bgr[2]; + } + else + { + UASSERT(image.type()==CV_8UC1); + pt.r = pt.g = pt.b = image.at(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols); + } + } + + int exportedId = nodeID; + pointToCamId[oi] = exportedId; + assembledCloudValidPoints->at(oi++) = pt; + } + } + + assembledCloudValidPoints->resize(oi); + cloudToExport = assembledCloudValidPoints; + cloudIToExport->clear(); + pointToCamId.resize(oi); + + printf("Camera projection... done! (%fs)\n", timer.ticks()); + } + + if(saveInDb) + { + if(!(mesh || texture)) + { + printf("Saving in db... (%d points)\n", !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size()); + if(!cloudToExport->empty()) + driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*cloudToExport, Transform(), false).data()); + else if(!cloudIToExport->empty()) + driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*cloudIToExport, Transform(), false).data()); + printf("Saving in db... done!\n"); } } else { + std::string ext = las?"las":"ply"; + 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(!cloudToExport->empty()) + savePDALFile(outputPath, *cloudToExport, pointToCamId, binary); + else if(!cloudIToExport->empty()) + savePDALFile(outputPath, *cloudIToExport, pointToCamId, binary); +#else + 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(!cloudToExport->empty()) + pcl::io::savePLYFile(outputPath, *cloudToExport, binary); + else if(!cloudIToExport->empty()) + pcl::io::savePLYFile(outputPath, *cloudIToExport, binary); +#endif + printf("Saving %s... done!\n", outputPath.c_str()); + } + + // Meshing... + if(mesh || texture) + { + if(!mergedCloudsI->empty()) + { + pcl::copyPointCloud(*mergedCloudsI, *mergedClouds); + mergedCloudsI->clear(); + } + Eigen::Vector4f min,max; pcl::getMinMax3D(*mergedClouds, min, max); float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]); @@ -574,7 +837,10 @@ int main(int argc, char * argv[]) { std::string outputPath=outputDirectory+"/"+baseName+"_mesh.ply"; printf("Saving %s...\n", outputPath.c_str()); - pcl::io::savePLYFile(outputPath, *mesh); + if(binary) + pcl::io::savePLYFileBinary(outputPath, *mesh); + else + pcl::io::savePLYFile(outputPath, *mesh); printf("Saving %s... done!\n", outputPath.c_str()); } }