diff --git a/corelib/include/rtabmap/core/PDALWriter.h b/corelib/include/rtabmap/core/PDALWriter.h index acbddd5414..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<pcl::PointXYZ> & cloud); -int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud); -int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud); -int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud); -int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud); +int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false); +int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false); +int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false); +int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false); +int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false); } diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 5d8b5f13bf..6b9309223c 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 <rtabmap/core/SensorData.h> #include <rtabmap/core/Parameters.h> #include <opencv2/core/core.hpp> +#include <rtabmap/core/ProgressState.h> #include <map> #include <list> @@ -300,6 +301,33 @@ 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<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras ( + const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, + const std::map<int, Transform> & cameraPoses, + const std::map<int, std::vector<CameraModel> > & cameraModels, + float maxDistance = 0.0f, + float maxAngle = 0.0f, + const std::vector<float> & roiRatios = std::vector<float>(), + 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<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras ( + const pcl::PointCloud<pcl::PointXYZINormal> & cloud, + const std::map<int, Transform> & cameraPoses, + const std::map<int, std::vector<CameraModel> > & cameraModels, + float maxDistance = 0.0f, + float maxAngle = 0.0f, + const std::vector<float> & roiRatios = std::vector<float>(), + bool distanceToCamPolicy = false, + const ProgressState * state = 0); + bool RTABMAP_EXP isFinite(const cv::Point3f & pt); pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds( diff --git a/corelib/src/PDALWriter.cpp b/corelib/src/PDALWriter.cpp index 4cecbd6a49..de9381363c 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 <rtabmap/utilite/UFile.h> #include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/UStl.h> +#include <rtabmap/utilite/UConversion.h> #include <pdal/io/BufferReader.hpp> #include <pdal/StageFactory.hpp> #include <pdal/PluginManager.hpp> @@ -72,14 +73,31 @@ std::string getPDALSupportedWriters() return output; } -int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud) +int savePDALFile(const std::string & filePath, + const pcl::PointCloud<pcl::PointXYZ> & cloud, + const std::vector<int> & cameraIds, + bool binary) { + 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,15 +106,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX view->setField(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); pdal::StageFactory factory; - pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath)); + std::string ext = UFile::getExtension(filePath); + pdal::Stage *writer = factory.createStage("writers." + ext); if(writer) { pdal::Options writerOps; writerOps.add("filename", filePath); + if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY + if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD writer->setOptions(writerOps); writer->setInput(bufferReader); @@ -115,17 +140,37 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX return 0; //success } -int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud) +int savePDALFile(const std::string & filePath, + const pcl::PointCloud<pcl::PointXYZRGB> & cloud, + const std::vector<int> & cameraIds, + bool binary) { + 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,15 +182,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX view->setField(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); pdal::StageFactory factory; - pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath)); + std::string ext = UFile::getExtension(filePath); + pdal::Stage *writer = factory.createStage("writers." + ext); if(writer) { pdal::Options writerOps; writerOps.add("filename", filePath); + if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY + if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD writer->setOptions(writerOps); writer->setInput(bufferReader); @@ -164,20 +216,43 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX return 0; //success } -int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud) +int savePDALFile(const std::string & filePath, + const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, + const std::vector<int> & cameraIds, + bool binary) { + 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,15 +267,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX view->setField(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); pdal::StageFactory factory; - pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath)); + std::string ext = UFile::getExtension(filePath); + pdal::Stage *writer = factory.createStage("writers." + ext); if(writer) { pdal::Options writerOps; writerOps.add("filename", filePath); + if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY + if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD writer->setOptions(writerOps); writer->setInput(bufferReader); @@ -219,15 +301,33 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX return 0; //success } -int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud) +int savePDALFile(const std::string & filePath, + const pcl::PointCloud<pcl::PointXYZI> & cloud, + const std::vector<int> & cameraIds, + bool binary) { + 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,15 +337,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX view->setField(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); pdal::StageFactory factory; - pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath)); + std::string ext = UFile::getExtension(filePath); + pdal::Stage *writer = factory.createStage("writers." + ext); if(writer) { pdal::Options writerOps; writerOps.add("filename", filePath); + if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY + if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD writer->setOptions(writerOps); writer->setInput(bufferReader); @@ -264,18 +371,39 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX return 0; //success } -int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud) +int savePDALFile(const std::string & filePath, + const pcl::PointCloud<pcl::PointXYZINormal> & cloud, + const std::vector<int> & cameraIds, + bool binary) { + 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,15 +416,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX view->setField(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); pdal::StageFactory factory; - pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath)); + std::string ext = UFile::getExtension(filePath); + pdal::Stage *writer = factory.createStage("writers." + ext); if(writer) { pdal::Options writerOps; writerOps.add("filename", filePath); + if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY + if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD writer->setOptions(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<int> signaturesRemoved; + bool neighborLinkRefined = false; if(_rgbdSlamMode) { statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(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 38fe9c2cb5..632eb10483 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 <pcl/io/pcd_io.h> #include <pcl/io/ply_io.h> #include <pcl/common/transforms.h> +#include <pcl/common/common.h> #include <opencv2/imgproc/imgproc.hpp> #include <opencv2/imgproc/types_c.h> @@ -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<float>(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<float>(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,275 @@ 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 + */ +template<class PointT> +std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamerasImpl ( + const typename pcl::PointCloud<PointT> & cloud, + const std::map<int, Transform> & cameraPoses, + const std::map<int, std::vector<CameraModel> > & cameraModels, + float maxDistance, + float maxAngle, + const std::vector<float> & roiRatios, + bool distanceToCamPolicy, + const ProgressState * state) +{ + std::vector<std::pair< std::pair<int, int>, 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<std::vector<ProjectionInfo> > invertedIndex(cloud.size()); // For each point: list of cameras + int cameraProcessed = 0; + for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter) + { + std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.begin(); + if(iter!=cameraModels.end() && !iter->second.empty()) + { + for(size_t i=0; i<iter->second.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<double>(0,0); + float fy = cameraMatrixK.at<double>(1,1); + float cx = cameraMatrixK.at<double>(0,2); + float cy = cameraMatrixK.at<double>(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<cloud.size(); ++i) + { + // Get 3D from laser scan + PointT ptScan = cloud.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; + 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<cv::Vec2i>(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<cv::Vec2i>(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<registered.cols; ++u) + { + for(int v=0; v<registered.rows; ++v) + { + cv::Vec2i &zReg = registered.at<cv::Vec2i>(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; i<invertedIndex.size(); ++i) + { + if((i+1)%10000 == 0) + { + UDEBUG("Point %d/%d", i+1, (int)cloud.size()); + if(state && !state->callback(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 PointT & pt = cloud.at(i); + int nodeID = -1; + int cameraIndex = -1; + float smallestWeight = std::numeric_limits<float>::max(); + pcl::PointXY uv_coords; + for (size_t j = 0; j<invertedIndex[i].size(); ++j) + { + const Transform & cam = cameraPoses.at(invertedIndex[i][j].nodeID); + Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0); + Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0); + float angleToCam = pcl::getAngle3D(normal, camDir); + float distanceToCam = invertedIndex[i][j].distance; + if(camDir.dot(normal) > 0 && // is facing camera? + (maxAngle<=0 || angleToCam < maxAngle) && // is point normal perpendicular to camera? + (maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera? + { + float vx = invertedIndex[i][j].uv.x-0.5f; + float vy = invertedIndex[i][j].uv.y-0.5f; + + float distanceToCenter = vx*vx+vy*vy; + float distance = distanceToCenter; + if(distanceToCamPolicy) + { + distance = distanceToCam; + } + if(distance <= smallestWeight) + { + nodeID = invertedIndex[i][j].nodeID; + cameraIndex = invertedIndex[i][j].cameraIndex; + smallestWeight = distance; + uv_coords = invertedIndex[i][j].uv; + } + } + } + + if(nodeID>-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; +} + +std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCameras ( + const typename pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, + const std::map<int, Transform> & cameraPoses, + const std::map<int, std::vector<CameraModel> > & cameraModels, + float maxDistance, + float maxAngle, + const std::vector<float> & roiRatios, + bool distanceToCamPolicy, + const ProgressState * state) +{ + return projectCloudToCamerasImpl(cloud, + cameraPoses, + cameraModels, + maxDistance, + maxAngle, + roiRatios, + distanceToCamPolicy, + state); +} + +std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCameras ( + const typename pcl::PointCloud<pcl::PointXYZINormal> & cloud, + const std::map<int, Transform> & cameraPoses, + const std::map<int, std::vector<CameraModel> > & cameraModels, + float maxDistance, + float maxAngle, + const std::vector<float> & 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/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<int, LaserScan> & cachedScans, const ParametersMap & parameters, bool & has2dScans) const; - void saveClouds(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true); + void saveClouds(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true, const std::vector<std::map<int, pcl::PointXY> > & pointToPixels = std::vector<std::map<int, pcl::PointXY> >()); void saveMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool binaryMode = true); void saveTextureMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, std::map<int, pcl::TextureMesh::Ptr> & textureMeshes, const QMap<int, Signature> & cachedSignatures, const std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels); diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index b56fb035e9..6771f7bc74 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())); @@ -173,9 +175,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())); @@ -313,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()); @@ -356,6 +374,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()); @@ -451,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()); @@ -497,6 +526,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()); @@ -592,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); @@ -635,6 +675,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 +827,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 +1005,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 +1287,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 @@ -1709,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<pcl::PointXYZ>::Ptr cloudWithoutNormals(new pcl::PointCloud<pcl::PointXYZ>); pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals); @@ -2444,6 +2499,217 @@ 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<pcl::PointXYZRGBNormal>::Ptr assembledCloud = cloudsWithNormals.begin()->second; + + std::map<int, Transform> cameraPoses; + std::map<int, std::vector<CameraModel> > cameraModels; + for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(0); iter!=poses.end(); ++iter) + { + std::vector<CameraModel> 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<float> 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<std::pair< std::pair<int, int>, 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<int, cv::Mat> cachedImages; + pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloudValidPoints; + if(!_ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked()) + { + assembledCloudValidPoints.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>()); + 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; i<pointToPixel.size(); ++i) + { + pcl::PointXYZRGBNormal & pt = assembledCloud->at(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<image.cols); + UASSERT(y>=0 && y<image.rows); + + if(image.type()==CV_8UC3) + { + cv::Vec3b bgr = image.at<cv::Vec3b>(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<unsigned char>(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)); + } + } + } + + if(assembledCloudValidPoints.get()) + { + assembledCloudValidPoints->resize(oi); + cloudsWithNormals.begin()->second = assembledCloudValidPoints; + + if(!textureVertexToPixels.empty()) + { + textureVertexToPixels.resize(oi); + } + } + + if(_canceled) + { + return false; + } + } + } UDEBUG(""); #ifdef RTABMAP_CPUTSDF @@ -2933,7 +3199,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 +3284,8 @@ bool ExportCloudsDialog::getExportedClouds( textureMeshes.clear(); textureMeshes.insert(std::make_pair(0, assembledMesh)); } - } + UDEBUG(""); return true; } @@ -3401,7 +3670,8 @@ void ExportCloudsDialog::saveClouds( const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, - bool binaryMode) + bool binaryMode, + const std::vector<std::map<int, pcl::PointXY> > & pointToPixels) { if(clouds.size() == 1) { @@ -3435,9 +3705,13 @@ void ExportCloudsDialog::saveClouds( pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBWithoutNormals; pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIWithoutNormals; pcl::PointCloud<pcl::PointXYZINormal>::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<pcl::PointXYZINormal>); @@ -3505,8 +3779,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 +3804,29 @@ void ExportCloudsDialog::saveClouds( #ifdef RTABMAP_PDAL else if(!QFileInfo(path).suffix().isEmpty()) { + std::vector<int> cameraIds(pointToPixels.size(), 0); + for(size_t i=0;i<pointToPixels.size(); ++i) + { + if(!pointToPixels[i].empty()) + { + cameraIds[i] = pointToPixels[i].begin()->first; + } + } if(cloudIWithNormals.get()) { - success = savePDALFile(path.toStdString(), *cloudIWithNormals) == 0; + success = savePDALFile(path.toStdString(), *cloudIWithNormals, cameraIds, binaryMode) == 0; } else if(cloudIWithoutNormals.get()) { - success = savePDALFile(path.toStdString(), *cloudIWithoutNormals) == 0; + success = savePDALFile(path.toStdString(), *cloudIWithoutNormals, cameraIds, binaryMode) == 0; } else if(cloudRGBWithoutNormals.get()) { - success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals) == 0; + success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals, cameraIds, binaryMode) == 0; } else { - success = savePDALFile(path.toStdString(), *clouds.begin()->second) == 0; + success = savePDALFile(path.toStdString(), *clouds.begin()->second, cameraIds, binaryMode) == 0; } } #endif diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index fc4968372a..6caff34e55 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -23,9 +23,9 @@ <property name="geometry"> <rect> <x>0</x> - <y>0</y> + <y>-584</y> <width>780</width> - <height>4975</height> + <height>5347</height> </rect> </property> <layout class="QVBoxLayout" name="verticalLayout_13"> @@ -76,13 +76,6 @@ </property> </widget> </item> - <item row="13" column="0"> - <widget class="QCheckBox" name="checkBox_gainCompensation"> - <property name="text"> - <string/> - </property> - </widget> - </item> <item row="8" column="1"> <widget class="QLabel" name="label_voxel"> <property name="text"> @@ -103,16 +96,6 @@ </property> </widget> </item> - <item row="13" column="1"> - <widget class="QLabel" name="label_gainCompensation"> - <property name="text"> - <string>Gain compensation. Normalize brightness of images.</string> - </property> - <property name="wordWrap"> - <bool>true</bool> - </property> - </widget> - </item> <item row="5" column="1"> <widget class="QLabel" name="label_frame"> <property name="text"> @@ -133,16 +116,6 @@ </property> </widget> </item> - <item row="14" column="1"> - <widget class="QLabel" name="label_binaryFile_12"> - <property name="text"> - <string>Meshing.</string> - </property> - <property name="wordWrap"> - <bool>true</bool> - </property> - </widget> - </item> <item row="12" column="0"> <widget class="QCheckBox" name="checkBox_smoothing"> <property name="text"> @@ -160,13 +133,6 @@ </property> </widget> </item> - <item row="14" column="0"> - <widget class="QCheckBox" name="checkBox_meshing"> - <property name="text"> - <string/> - </property> - </widget> - </item> <item row="3" column="1"> <widget class="QLabel" name="label_binaryFile"> <property name="text"> @@ -323,6 +289,57 @@ </item> </widget> </item> + <item row="13" column="1"> + <widget class="QLabel" name="label_gainCompensation"> + <property name="text"> + <string>Gain compensation. Normalize brightness of images.</string> + </property> + <property name="wordWrap"> + <bool>true</bool> + </property> + </widget> + </item> + <item row="14" column="1"> + <widget class="QLabel" name="label_cameraProjection"> + <property name="text"> + <string>Camera projection. This can be used to colorize point cloud created from scans and/or export camera IDs for each point of the cloud.</string> + </property> + <property name="wordWrap"> + <bool>true</bool> + </property> + </widget> + </item> + <item row="13" column="0"> + <widget class="QCheckBox" name="checkBox_gainCompensation"> + <property name="text"> + <string/> + </property> + </widget> + </item> + <item row="14" column="0"> + <widget class="QCheckBox" name="checkBox_cameraProjection"> + <property name="text"> + <string/> + </property> + </widget> + </item> + <item row="15" column="1"> + <widget class="QLabel" name="label_binaryFile_12"> + <property name="text"> + <string>Meshing.</string> + </property> + <property name="wordWrap"> + <bool>true</bool> + </property> + </widget> + </item> + <item row="15" column="0"> + <widget class="QCheckBox" name="checkBox_meshing"> + <property name="text"> + <string/> + </property> + </widget> + </item> </layout> </item> <item> @@ -642,8 +659,11 @@ <property name="decimals"> <number>2</number> </property> + <property name="minimum"> + <double>-1000.000000000000000</double> + </property> <property name="maximum"> - <double>100.000000000000000</double> + <double>1000.000000000000000</double> </property> <property name="singleStep"> <double>0.100000000000000</double> @@ -661,8 +681,11 @@ <property name="decimals"> <number>2</number> </property> + <property name="minimum"> + <double>-1000.000000000000000</double> + </property> <property name="maximum"> - <double>100.000000000000000</double> + <double>1000.000000000000000</double> </property> <property name="singleStep"> <double>0.100000000000000</double> @@ -1475,6 +1498,185 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string> </layout> </widget> </item> + <item> + <widget class="QGroupBox" name="groupBox_cameraProjection"> + <property name="title"> + <string>Camera Projection</string> + </property> + <property name="checkable"> + <bool>false</bool> + </property> + <layout class="QGridLayout" name="gridLayout_20" columnstretch="0,0,0,1"> + <item row="3" column="1"> + <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxAngle"> + <property name="suffix"> + <string> deg</string> + </property> + <property name="decimals"> + <number>0</number> + </property> + <property name="minimum"> + <double>0.000000000000000</double> + </property> + <property name="maximum"> + <double>180.000000000000000</double> + </property> + <property name="singleStep"> + <double>1.000000000000000</double> + </property> + <property name="value"> + <double>0.000000000000000</double> + </property> + </widget> + </item> + <item row="5" column="2" colspan="2"> + <widget class="QLabel" name="label_meshingTextureSize_15"> + <property name="text"> + <string>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.</string> + </property> + <property name="wordWrap"> + <bool>true</bool> + </property> + </widget> + </item> + <item row="4" column="1"> + <widget class="QCheckBox" name="checkBox_camProjDistanceToCamPolicy"> + <property name="text"> + <string/> + </property> + </widget> + </item> + <item row="4" column="2" colspan="2"> + <widget class="QLabel" name="label_meshingTextureSize_14"> + <property name="text"> + <string>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.</string> + </property> + <property name="wordWrap"> + <bool>true</bool> + </property> + </widget> + </item> + <item row="2" column="2" colspan="2"> + <widget class="QLabel" name="label_meshingTextureSize_13"> + <property name="text"> + <string>Maximum distance from the camera for points to be colorized by this camera (0 means infinite).</string> + </property> + <property name="wordWrap"> + <bool>true</bool> + </property> + </widget> + </item> + <item row="3" column="2" colspan="2"> + <widget class="QLabel" name="label_meshingTextureSize_12"> + <property name="text"> + <string>Maximum angle between camera and point's normal to apply color (0 means disabled).</string> + </property> + <property name="wordWrap"> + <bool>true</bool> + </property> + </widget> + </item> + <item row="2" column="1"> + <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxDistance"> + <property name="suffix"> + <string> m</string> + </property> + <property name="decimals"> + <number>1</number> + </property> + <property name="minimum"> + <double>0.000000000000000</double> + </property> + <property name="maximum"> + <double>99.000000000000000</double> + </property> + <property name="singleStep"> + <double>1.000000000000000</double> + </property> + <property name="value"> + <double>0.000000000000000</double> + </property> + </widget> + </item> + <item row="5" column="1"> + <widget class="QCheckBox" name="checkBox_camProjKeepPointsNotSeenByCameras"> + <property name="text"> + <string/> + </property> + </widget> + </item> + <item row="7" column="2" colspan="2"> + <widget class="QLabel" name="label_camProjExportCamera"> + <property name="text"> + <string>ID format of the camera selected for each point.</string> + </property> + <property name="wordWrap"> + <bool>true</bool> + </property> + </widget> + </item> + <item row="7" column="1"> + <widget class="QComboBox" name="comboBox_camProjExportCamera"> + <property name="toolTip"> + <string>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</string> + </property> + <item> + <property name="text"> + <string>Disabled</string> + </property> + </item> + <item> + <property name="text"> + <string>By Node ID</string> + </property> + </item> + <item> + <property name="text"> + <string>By Camera Index</string> + </property> + </item> + <item> + <property name="text"> + <string>By Node ID and Camera Index</string> + </property> + </item> + </widget> + </item> + <item row="1" column="1"> + <widget class="QLineEdit" name="lineEdit_camProjRoiRatios"/> + </item> + <item row="1" column="2" colspan="2"> + <widget class="QLabel" name="label_meshingTextureSize_17"> + <property name="text"> + <string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string> + </property> + <property name="wordWrap"> + <bool>true</bool> + </property> + </widget> + </item> + <item row="6" column="2"> + <widget class="QLabel" name="label_meshingTextureSize_18"> + <property name="text"> + <string>Recolor points from camera projection. This would be used to color laser scans with the cameras.</string> + </property> + <property name="wordWrap"> + <bool>true</bool> + </property> + </widget> + </item> + <item row="6" column="1"> + <widget class="QCheckBox" name="checkBox_camProjRecolorPoints"> + <property name="text"> + <string/> + </property> + </widget> + </item> + </layout> + </widget> + </item> <item> <widget class="QGroupBox" name="groupBox_meshing"> <property name="title"> 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 <rtabmap/core/util3d_filtering.h> #include <rtabmap/core/util3d_transforms.h> #include <rtabmap/core/util3d_surface.h> +#include <rtabmap/core/util2d.h> #include <rtabmap/core/optimizer/OptimizerG2O.h> +#include <rtabmap/core/Graph.h> #include <rtabmap/utilite/UMath.h> #include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UFile.h> @@ -43,19 +45,28 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <pcl/surface/poisson.h> #include <stdio.h> +#ifdef RTABMAP_PDAL +#include <rtabmap/core/PDALWriter.h> +#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<argc; ++i) { if(std::strcmp(argv[i], "--help") == 0) { showUsage(); } + else if(std::strcmp(argv[i], "--bin") == 0) + { + binary = true; + } + else if(std::strcmp(argv[i], "--las") == 0) + { +#ifdef RTABMAP_PDAL + las = true; +#else + printf("\"--las\" option cannot be used because RTAB-Map is not built with PDAL support. Will export in PLY...\n"); +#endif + + } else if(std::strcmp(argv[i], "--mesh") == 0) { mesh = true; @@ -164,6 +193,18 @@ int main(int argc, char * argv[]) { distanceToCamPolicy = true; } + else if(std::strcmp(argv[i], "--cam_projection") == 0) + { + camProjection = true; + } + else if(std::strcmp(argv[i], "--poses") == 0) + { + exportPoses = true; + } + else if(std::strcmp(argv[i], "--images") == 0) + { + exportImages = true; + } else if(std::strcmp(argv[i], "--ba") == 0) { ba = true; @@ -409,35 +450,45 @@ int main(int argc, char * argv[]) // Construct the cloud printf("Create and assemble the clouds...\n"); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>); + pcl::PointCloud<pcl::PointXYZINormal>::Ptr mergedCloudsI(new pcl::PointCloud<pcl::PointXYZINormal>); std::map<int, rtabmap::Transform> cameraPoses; + std::map<int, double> cameraStamps; std::map<int, std::vector<rtabmap::CameraModel> > cameraModels; std::map<int, cv::Mat> cameraDepths; - + int imagesExported = 0; for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter) { Signature node = nodes.find(iter->first)->second; // uncompress data std::vector<CameraModel> models = node.sensorData().cameraModels(); + cv::Mat rgb; cv::Mat depth; pcl::IndicesPtr indices(new std::vector<int>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; + pcl::PointCloud<pcl::PointXYZI>::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<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint); - - pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>); - pcl::concatenateFields(*cloud, *normals, *cloudWithNormals); - - if(mergedClouds->size() == 0) + if(cloud.get() && !cloud->empty()) { - *mergedClouds = *cloudWithNormals; + pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint); + pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>); + 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<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloudI, 20, 0.0f, viewpoint); + pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals(new pcl::PointCloud<pcl::PointXYZINormal>); + 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<int, Link>(), cameraStamps); + printf("Poses exported to \"%s\".\n", outputPath.c_str()); + } - if(!(mesh || texture)) + pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport = mergedClouds; + pcl::PointCloud<pcl::PointXYZINormal>::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<int> pointToCamId; + if(camProjection && !cameraPoses.empty()) + { + printf("Camera projection...\n"); + pointToCamId.resize(!cloudToExport->empty()?cloudToExport->size():cloudIToExport->size()); + std::vector<std::pair< std::pair<int, int>, 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<float>(), + 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<float>(), + distanceToCamPolicy); + } + + // color the cloud + UASSERT(pointToPixel.empty() || pointToPixel.size() == pointToCamId.size()); + std::map<int, cv::Mat> cachedImages; + pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloudValidPoints(new pcl::PointCloud<pcl::PointXYZRGBNormal>()); + assembledCloudValidPoints->resize(pointToCamId.size()); + + int oi=0; + for(size_t i=0; i<pointToPixel.size(); ++i) + { + pcl::PointXYZRGBNormal pt; + if(!cloudToExport->empty()) + { + 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<image.cols); + UASSERT(y>=0 && y<image.rows); + + if(image.type()==CV_8UC3) + { + cv::Vec3b bgr = image.at<cv::Vec3b>(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<unsigned char>(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()); } }