diff --git a/CMakeLists.txt b/CMakeLists.txt index b42fbf41..ac2c2e24 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 21) -SET(RTABMAP_PATCH_VERSION 9) +SET(RTABMAP_PATCH_VERSION 10) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/corelib/include/rtabmap/core/Compression.h b/corelib/include/rtabmap/core/Compression.h index 9ff69a3a..6f9709a3 100644 --- a/corelib/include/rtabmap/core/Compression.h +++ b/corelib/include/rtabmap/core/Compression.h @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines +#include #include #include @@ -86,5 +87,9 @@ cv::Mat RTABMAP_CORE_EXPORT uncompressData(const unsigned char * bytes, unsigned cv::Mat RTABMAP_CORE_EXPORT compressString(const std::string & str); std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat & bytes); +std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const cv::Mat & bytes); +std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const std::vector & bytes); +std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const unsigned char * bytes, size_t size); + } /* namespace rtabmap */ #endif /* COMPRESSION_H_ */ diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index f18121f9..fdbfe285 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -100,7 +100,7 @@ public: int nodeId, const std::vector & models, const std::vector & stereoModels); - void updateDepthImage(int nodeId, const cv::Mat & image); + void updateDepthImage(int nodeId, const cv::Mat & image, const std::string & format); void updateLaserScan(int nodeId, const LaserScan & scan); public: @@ -178,6 +178,7 @@ public: void getWeight(int signatureId, int & weight) const; void getLastNodeIds(std::set & ids) const; void getAllNodeIds(std::set & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false, bool ignoreIntermediateNodes = false) const; + void getAllOdomPoses(std::map & poses, bool ignoreChildren = false, bool ignoreIntermediateNodes = false) const; void getAllLinks(std::multimap & links, bool ignoreNullLinks = true, bool withLandmarks = false) const; void getLastNodeId(int & id) const; void getLastMapId(int & mapId) const; @@ -242,7 +243,8 @@ protected: virtual void updateDepthImageQuery( int nodeId, - const cv::Mat & image) const = 0; + const cv::Mat & image, + const std::string & format) const = 0; virtual void updateLaserScanQuery( int nodeId, @@ -286,6 +288,7 @@ protected: virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector & velocity, GPS & gps, EnvSensors & sensors) const = 0; virtual void getLastNodeIdsQuery(std::set & ids) const = 0; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const = 0; + virtual void getAllOdomPosesQuery(std::map & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const = 0; virtual void getAllLinksQuery(std::multimap & links, bool ignoreNullLinks, bool withLandmarks) const = 0; virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const = 0; virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0; diff --git a/corelib/include/rtabmap/core/DBDriverSqlite3.h b/corelib/include/rtabmap/core/DBDriverSqlite3.h index 9b61dbc4..407ec7e0 100644 --- a/corelib/include/rtabmap/core/DBDriverSqlite3.h +++ b/corelib/include/rtabmap/core/DBDriverSqlite3.h @@ -103,7 +103,8 @@ protected: virtual void updateDepthImageQuery( int nodeId, - const cv::Mat & image) const; + const cv::Mat & image, + const std::string & format) const; void updateLaserScanQuery( int nodeId, @@ -147,6 +148,7 @@ protected: virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector & velocity, GPS & gps, EnvSensors & sensors) const; virtual void getLastNodeIdsQuery(std::set & ids) const; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const; + virtual void getAllOdomPosesQuery(std::map & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const; virtual void getAllLinksQuery(std::multimap & links, bool ignoreNullLinks, bool withLandmarks) const; virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const; virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const; @@ -172,7 +174,7 @@ private: void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const; void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const; void stepCalibrationUpdate(sqlite3_stmt * ppStmt, int nodeId, const std::vector & models, const std::vector & stereoModels) const; - void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const; + void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image, const std::string & format) const; void stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & image) const; void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const; void stepLink(sqlite3_stmt * ppStmt, const Link & link) const; diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index 324323d2..974830cd 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -115,7 +115,8 @@ Transform RTABMAP_CORE_EXPORT calcRMSE( float & rotational_median, float & rotational_std, float & rotational_min, - float & rotational_max); + float & rotational_max, + bool align2D = false); void RTABMAP_CORE_EXPORT computeMaxGraphErrors( const std::map & poses, diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 145aeb19..60f2a17d 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -303,6 +303,7 @@ private: bool _notLinkedNodesKeptInDb; bool _saveIntermediateNodeData; std::string _rgbCompressionFormat; + std::string _depthCompressionFormat; bool _incrementalMemory; bool _localizationDataSaved; bool _reduceGraph; @@ -314,6 +315,7 @@ private: bool _badSignaturesIgnored; bool _mapLabelsAdded; bool _depthAsMask; + float _maskFloorThreshold; bool _stereoFromMotion; unsigned int _imagePreDecimation; unsigned int _imagePostDecimation; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index ee4da294..ffd3abd3 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -209,6 +209,7 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Mem, NotLinkedNodesKept, bool, true, "Keep not linked nodes in db (rehearsed nodes and deleted nodes)."); RTABMAP_PARAM(Mem, IntermediateNodeDataKept, bool, false, "Keep intermediate node data in db."); RTABMAP_PARAM_STR(Mem, ImageCompressionFormat, ".jpg", "RGB image compression format. It should be \".jpg\" or \".png\"."); + RTABMAP_PARAM_STR(Mem, DepthCompressionFormat, ".rvl", "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used."); RTABMAP_PARAM(Mem, STMSize, unsigned int, 10, "Short-term memory size."); RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, otherwise it is Localization mode."); RTABMAP_PARAM(Mem, LocalizationDataSaved, bool, false, uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str()); @@ -221,6 +222,7 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored."); RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session."); RTABMAP_PARAM(Mem, DepthAsMask, bool, true, "Use depth image as mask when extracting features for vocabulary."); + RTABMAP_PARAM(Mem, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kMemDepthAsMask().c_str())); RTABMAP_PARAM(Mem, StereoFromMotion, bool, false, uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str())); RTABMAP_PARAM(Mem, ImagePreDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str())); RTABMAP_PARAM(Mem, ImagePostDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str())); @@ -703,6 +705,7 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit)."); RTABMAP_PARAM(Vis, MinDepth, float, 0, "Min depth of the features (0 means no limit)."); RTABMAP_PARAM(Vis, DepthAsMask, bool, true, "Use depth image as mask when extracting features."); + RTABMAP_PARAM(Vis, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str())); RTABMAP_PARAM_STR(Vis, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix()."); RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining."); diff --git a/corelib/include/rtabmap/core/RegistrationVis.h b/corelib/include/rtabmap/core/RegistrationVis.h index 520be30a..abcfa3f8 100644 --- a/corelib/include/rtabmap/core/RegistrationVis.h +++ b/corelib/include/rtabmap/core/RegistrationVis.h @@ -101,6 +101,7 @@ private: bool _guessMatchToProjection; int _bundleAdjustment; bool _depthAsMask; + float _maskFloorThreshold; float _minInliersDistributionThr; float _maxInliersMeanDistance; diff --git a/corelib/include/rtabmap/core/rvl_codec.h b/corelib/include/rtabmap/core/rvl_codec.h new file mode 100644 index 00000000..217e426c --- /dev/null +++ b/corelib/include/rtabmap/core/rvl_codec.h @@ -0,0 +1,37 @@ +// The following code is a C++ wrapper of the code presented by +// Andrew D. Wilson in "Fast Lossless Depth Image Compression" at SIGCHI'17. +// The original code is licensed under the MIT License. + +#ifndef RVL_CODEC_H_ +#define RVL_CODEC_H_ + +#include +#include "rtabmap/core/rtabmap_core_export.h" + +namespace rtabmap +{ + +class RTABMAP_CORE_EXPORT RvlCodec { +public: + RvlCodec(); + // Compress input data into output. The size of output can be equal to (1.5 * numPixels + 4) in the worst case. + int CompressRVL(const uint16_t * input, unsigned char * output, int numPixels); + // Decompress input data into output. The size of output must be equal to numPixels. + void DecompressRVL(const unsigned char * input, uint16_t * output, int numPixels); + +private: + RvlCodec(const RvlCodec &); + RvlCodec & operator=(const RvlCodec &); + + void EncodeVLE(int value); + int DecodeVLE(); + + int *buffer_; + int *pBuffer_; + int word_; + int nibblesWritten_; +}; + +} // namespace rtabmap + +#endif // RVL_CODEC_H_ diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 9cba8724..390fe14c 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -364,6 +364,22 @@ void RTABMAP_CORE_EXPORT fillProjectedCloudHoles( bool verticalDirection, bool fillToBorder); +/** + * @brief Remove values below a floor threshold in a depth image. + * + * @param depth the depth image to filter (can be a multi-camera depth image). + * @param cameraModels corresponding camera model(s) to depth image, with valid + * local transform between base frame to camera frame. + * @param threshold height from base frame at which pixels below it are set to 0. + * @param depthBelow depth image of the pixels below the floor theshold. + * @return cv::Mat depth image of the pixels above the floor theshold. + */ +cv::Mat RTABMAP_CORE_EXPORT filterFloor( + const cv::Mat & depth, + const std::vector & cameraModels, + float threshold, + cv::Mat * depthBelow = 0); + /** * For each point, return pixel of the best camera (NodeID->CameraIndex) * looking at it based on the policy and parameters diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index 080cd8b7..e07e5c3c 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -482,6 +482,22 @@ void RTABMAP_CORE_EXPORT adjustNormalsToViewPoint( const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0), float groundNormalsUp = 0.0f); +void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints( + const std::map & poses, + const std::vector & cameraIndices, + pcl::PointCloud::Ptr & cloud, + float groundNormalsUp = 0.0f); +void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints( + const std::map & poses, + const std::vector & cameraIndices, + pcl::PointCloud::Ptr & cloud, + float groundNormalsUp = 0.0f); +void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints( + const std::map & poses, + const std::vector & cameraIndices, + pcl::PointCloud::Ptr & cloud, + float groundNormalsUp = 0.0f); + void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints( const std::map & poses, const pcl::PointCloud::Ptr & rawCloud, diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 0033140b..d4361dae 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -64,6 +64,8 @@ SET(SRC_FILES util3d_correspondences.cpp util3d_motion_estimation.cpp + rvl_codec.cpp + SensorData.cpp Graph.cpp Compression.cpp @@ -643,10 +645,10 @@ IF(octomap_FOUND) ENDIF(octomap_FOUND) IF(grid_map_core_FOUND) - IF(TARGET grid_map_core) + IF(TARGET grid_map_core::grid_map_core) SET(LIBRARIES ${LIBRARIES} - grid_map_core + grid_map_core::grid_map_core ) ELSE() SET(INCLUDE_DIRS diff --git a/corelib/src/Compression.cpp b/corelib/src/Compression.cpp index 1c51478e..5f9fb46c 100644 --- a/corelib/src/Compression.cpp +++ b/corelib/src/Compression.cpp @@ -34,14 +34,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -// format : ".png" ".jpg" "" (empty is general) +// format : ".jpg" ".png" ".rvl" "" (empty is general) CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) : uncompressedData_(mat), format_(format), image_(!format.empty()), compressMode_(true) { - UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0); + UASSERT(format.empty() || format.compare(".jpg") == 0 || format.compare(".png") == 0 || format.compare(".rvl") == 0); } // assume image CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) : @@ -96,7 +96,7 @@ void CompressionThread::mainLoop() this->kill(); } -// ".png" or ".jpg" +// ".jpg" or ".png" or ".rvl" std::vector compressImage(const cv::Mat & image, const std::string & format) { std::vector bytes; @@ -106,7 +106,21 @@ std::vector compressImage(const cv::Mat & image, const std::strin { //save in 8bits-4channel cv::Mat bgra(image.size(), CV_8UC4, image.data); - cv::imencode(format, bgra, bytes); + cv::imencode(".png", bgra, bytes); + } + else if(format == ".rvl") + { + bytes = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L'}; + int numPixels = image.rows * image.cols; + // In the worst case, RVL compression results in ~1.5x larger data. + bytes.resize(3 * numPixels + 20); + uint32_t cols = image.cols; + uint32_t rows = image.rows; + memcpy(&bytes[8], &cols, 4); + memcpy(&bytes[12], &rows, 4); + RvlCodec rvl; + int compressedSize = rvl.CompressRVL(image.ptr(), &bytes[16], numPixels); + bytes.resize(16 + compressedSize); } else { @@ -116,7 +130,7 @@ std::vector compressImage(const cv::Mat & image, const std::strin return bytes; } -// ".png" or ".jpg" +// ".jpg" or ".png" or ".rvl" cv::Mat compressImage2(const cv::Mat & image, const std::string & format) { std::vector bytes = compressImage(image, format); @@ -129,21 +143,33 @@ cv::Mat compressImage2(const cv::Mat & image, const std::string & format) cv::Mat uncompressImage(const cv::Mat & bytes) { - cv::Mat image; + cv::Mat image; if(!bytes.empty()) { -#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4) - image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED); -#else - image = cv::imdecode(bytes, -1); -#endif - if(image.type() == CV_8UC4) + if (compressedDepthFormat(bytes) == ".rvl") { - // Using clone() or copyTo() caused a memory leak !?!? - // image = cv::Mat(image.size(), CV_32FC1, image.data).clone(); - cv::Mat depth(image.size(), CV_32FC1); - memcpy(depth.data, image.data, image.total()*image.elemSize()); - image = depth; + uint32_t cols, rows; + memcpy(&cols, &bytes.data[8], 4); + memcpy(&rows, &bytes.data[12], 4); + image = cv::Mat(rows, cols, CV_16UC1); + RvlCodec rvl; + rvl.DecompressRVL(&bytes.data[16], image.ptr(), cols * rows); + } + else + { +#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4) + image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED); +#else + image = cv::imdecode(bytes, -1); +#endif + if(image.type() == CV_8UC4) + { + // Using clone() or copyTo() caused a memory leak !?!? + // image = cv::Mat(image.size(), CV_32FC1, image.data).clone(); + cv::Mat depth(image.size(), CV_32FC1); + memcpy(depth.data, image.data, image.total()*image.elemSize()); + image = depth; + } } } return image; @@ -151,17 +177,29 @@ cv::Mat uncompressImage(const cv::Mat & bytes) cv::Mat uncompressImage(const std::vector & bytes) { - cv::Mat image; + cv::Mat image; if(bytes.size()) { -#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4) - image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED); -#else - image = cv::imdecode(bytes, -1); -#endif - if(image.type() == CV_8UC4) + if (compressedDepthFormat(bytes) == ".rvl") { - image = cv::Mat(image.size(), CV_32FC1, image.data).clone(); + uint32_t cols, rows; + memcpy(&cols, &bytes[8], 4); + memcpy(&rows, &bytes[12], 4); + image = cv::Mat(rows, cols, CV_16UC1); + RvlCodec rvl; + rvl.DecompressRVL(&bytes[16], image.ptr(), cols * rows); + } + else + { +#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4) + image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED); +#else + image = cv::imdecode(bytes, -1); +#endif + if(image.type() == CV_8UC4) + { + image = cv::Mat(image.size(), CV_32FC1, image.data).clone(); + } } } return image; @@ -291,4 +329,33 @@ std::string uncompressString(const cv::Mat & bytes) return ""; } +std::string compressedDepthFormat(const cv::Mat & bytes) +{ + return compressedDepthFormat(bytes.data, bytes.rows * bytes.cols * bytes.elemSize()); +} +std::string compressedDepthFormat(const std::vector & bytes) +{ + return compressedDepthFormat(bytes.data(), bytes.size()); +} +std::string compressedDepthFormat(const unsigned char * bytes, size_t size) +{ + std::string format; + if(bytes && size) + { + size_t maxlen = std::min(size, size_t(8)); + std::vector signature(maxlen); + memcpy(&signature[0], bytes, maxlen); + if (std::string(signature.begin(), signature.end()) == "DEPTHRVL") + { + format = ".rvl"; + } + else + { + // Assuming png by default + format = ".png"; + } + } + return format; +} + } /* namespace rtabmap */ diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index 4639bcba..2c381164 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -512,12 +512,13 @@ void DBDriver::updateCalibration(int nodeId, const std::vector & mo _dbSafeAccessMutex.unlock(); } -void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image) +void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image, const std::string & format) { _dbSafeAccessMutex.lock(); this->updateDepthImageQuery( nodeId, - image); + image, + format); _dbSafeAccessMutex.unlock(); } @@ -922,6 +923,45 @@ void DBDriver::getAllNodeIds(std::set & ids, bool ignoreChildren, bool igno _dbSafeAccessMutex.unlock(); } +void DBDriver::getAllOdomPoses(std::map & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const +{ + // look in the trash + _trashesMutex.lock(); + if(_trashSignatures.size()) + { + for(std::map::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter) + { + bool hasNeighbors = !ignoreChildren; + if(ignoreChildren) + { + for(std::map::const_iterator nIter = sIter->second->getLinks().begin(); + nIter!=sIter->second->getLinks().end(); + ++nIter) + { + if(nIter->second.type() == Link::kNeighbor || + nIter->second.type() == Link::kNeighborMerged) + { + hasNeighbors = true; + break; + } + } + } + if(hasNeighbors && (!ignoreIntermediateNodes || sIter->second->getWeight() != -1)) + { + poses.insert(std::make_pair(sIter->first, sIter->second->getPose())); + } + } + + std::vector keys = uKeys(_trashSignatures); + + } + _trashesMutex.unlock(); + + _dbSafeAccessMutex.lock(); + this->getAllOdomPosesQuery(poses, ignoreChildren, ignoreIntermediateNodes); + _dbSafeAccessMutex.unlock(); +} + void DBDriver::getAllLinks(std::multimap & links, bool ignoreNullLinks, bool withLandmarks) const { _dbSafeAccessMutex.lock(); @@ -1139,7 +1179,7 @@ void DBDriver::addInfoAfterRun( << processMemUsed << "," << databaseMemUsed << "," << dictionarySize << "," - "\"" << param.c_str() << "\");"; + "'" << param.c_str() << "');"; } else { @@ -1149,7 +1189,7 @@ void DBDriver::addInfoAfterRun( << processMemUsed << "," << databaseMemUsed << "," << dictionarySize << "," - "\"" << param.c_str() << "\");"; + "'" << param.c_str() << "');"; } } else diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 909aeca5..370cd2c3 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -2427,9 +2427,13 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set & ids, bool ignoreChildre << "FROM Node "; if(ignoreChildren) { - query << "INNER JOIN Link "; - query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them) - query << "WHERE from_id != to_id "; // ignore self referring links + // use to_id to ignore all children (which don't have link pointing on them) + // ignore self referring links + // keep nodes without link to other nodes (map has only a single node) + query << "WHERE "; + query << "(EXISTS (select 1 from Link where Node.id=to_id and from_id != to_id) OR "; + query << " NOT EXISTS (select 1 from Link where id=to_id and from_id != to_id)) "; + query << "AND weight>-9 "; //ignore invalid nodes if(ignoreIntermediateNodes) { @@ -2483,6 +2487,75 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set & ids, bool ignoreChildre } } +void DBDriverSqlite3::getAllOdomPosesQuery(std::map & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const +{ + if(_ppDb) + { + UTimer timer; + timer.start(); + int rc = SQLITE_OK; + sqlite3_stmt * ppStmt = 0; + std::stringstream query; + + query << "SELECT DISTINCT id, pose " + << "FROM Node "; + if(ignoreChildren) + { + query << "INNER JOIN Link "; + query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them) + query << "WHERE from_id != to_id "; // ignore self referring links + query << "AND weight>-9 "; //ignore invalid nodes + if(ignoreIntermediateNodes) + { + query << "AND weight!=-1 "; //ignore intermediate nodes + } + } + else if(ignoreIntermediateNodes) + { + query << "WHERE weight!=-1 "; //ignore intermediate nodes + } + + query << "ORDER BY id"; + + rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + const void * data = 0; + int dataSize = 0; + + // Process the result if one + rc = sqlite3_step(ppStmt); + while(rc == SQLITE_ROW) + { + int id = sqlite3_column_int(ppStmt, 0); // Signature Id + data = sqlite3_column_blob(ppStmt, 1); // Pose + dataSize = sqlite3_column_bytes(ppStmt, 1); + + Transform pose; + if((unsigned int)dataSize == pose.size()*sizeof(float) && data) + { + memcpy(pose.data(), data, dataSize); + if(uStrNumCmp(_version, "0.15.2") < 0) + { + pose.normalizeRotation(); + } + } + else if(dataSize) + { + UERROR("Error while loading pose for node %d! Setting to null...", id); + } + poses.insert(std::make_pair(id, pose)); + rc = sqlite3_step(ppStmt); + } + UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)poses.size()); + } +} + void DBDriverSqlite3::getAllLinksQuery(std::multimap & links, bool ignoreNullLinks, bool withLandmarks) const { links.clear(); @@ -4650,7 +4723,8 @@ void DBDriverSqlite3::updateCalibrationQuery( void DBDriverSqlite3::updateDepthImageQuery( int nodeId, - const cv::Mat & image) const + const cv::Mat & image, + const std::string & format) const { UDEBUG(""); if(_ppDb) @@ -4669,7 +4743,8 @@ void DBDriverSqlite3::updateDepthImageQuery( // Save depth stepDepthUpdate(ppStmt, nodeId, - image); + image, + format); // Finalize (delete) the statement rc = sqlite3_finalize(ppStmt); @@ -5940,7 +6015,7 @@ std::string DBDriverSqlite3::queryStepDepthUpdate() const return "UPDATE Data SET depth=? WHERE id=?;"; } } -void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image) const +void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image, const std::string & format) const { if(!ppStmt) { @@ -5954,7 +6029,7 @@ void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const c if(!image.empty() && (image.type()!=CV_8UC1 || image.rows > 1)) { // compress - imageCompressed = compressImage2(image, ".png"); + imageCompressed = compressImage2(image, format); } else { diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index 73e54471..5aa67230 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -1315,6 +1315,11 @@ std::vector SIFT::generateKeypointsImpl(const cv::Mat & image, con UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U); std::vector keypoints; cv::Mat imgRoi(image, roi); + cv::Mat maskRoi; + if(!mask.empty()) + { + maskRoi = cv::Mat(mask, roi); + } #ifdef RTABMAP_CUDASIFT if(gpu_) { @@ -1383,6 +1388,12 @@ std::vector SIFT::generateKeypointsImpl(const cv::Mat & image, con //std::cout << cv::Mat(1, 128*4, CV_8UC1, desc) << std::endl; continue; } + // Ignore keypoints not in the mask + if(!maskRoi.empty() && maskRoi.at(cudaSiftData_->h_data[i].ypos, cudaSiftData_->h_data[i].xpos) == 0) + { + continue; + } + //Keep track of the data, to be easier to manage the data in the next step hessianMap.insert(std::pair(cudaSiftData_->h_data[i].sharpness, i)); } @@ -1412,12 +1423,6 @@ std::vector SIFT::generateKeypointsImpl(const cv::Mat & image, con else #endif { - cv::Mat maskRoi; - if(!mask.empty()) - { - maskRoi = cv::Mat(mask, roi); - } - #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11))) #ifdef RTABMAP_NONFREE sift_->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 5fb69829..aed8e491 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -783,7 +783,8 @@ Transform calcRMSE ( float & rotational_median, float & rotational_std, float & rotational_min, - float & rotational_max) + float & rotational_max, + bool align2D) { translational_rmse = 0.0f; @@ -815,8 +816,8 @@ Transform calcRMSE ( { idFirst = iter->first; } - cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), jter->second.z()); - cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()); + cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), align2D?0:jter->second.z()); + cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), align2D?0:iter->second.z()); } } diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 0357fbba..590d4cb0 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -80,6 +80,7 @@ Memory::Memory(const ParametersMap & parameters) : _notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()), _saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()), _rgbCompressionFormat(Parameters::defaultMemImageCompressionFormat()), + _depthCompressionFormat(Parameters::defaultMemDepthCompressionFormat()), _incrementalMemory(Parameters::defaultMemIncrementalMemory()), _localizationDataSaved(Parameters::defaultMemLocalizationDataSaved()), _reduceGraph(Parameters::defaultMemReduceGraph()), @@ -91,6 +92,7 @@ Memory::Memory(const ParametersMap & parameters) : _badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()), _mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()), _depthAsMask(Parameters::defaultMemDepthAsMask()), + _maskFloorThreshold(Parameters::defaultMemDepthMaskFloorThr()), _stereoFromMotion(Parameters::defaultMemStereoFromMotion()), _imagePreDecimation(Parameters::defaultMemImagePreDecimation()), _imagePostDecimation(Parameters::defaultMemImagePostDecimation()), @@ -567,6 +569,7 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb); Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData); Parameters::parse(params, Parameters::kMemImageCompressionFormat(), _rgbCompressionFormat); + Parameters::parse(params, Parameters::kMemDepthCompressionFormat(), _depthCompressionFormat); Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal); Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds); Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored); @@ -576,6 +579,7 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(params, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId); Parameters::parse(params, Parameters::kMemSTMSize(), _maxStMemSize); Parameters::parse(params, Parameters::kMemDepthAsMask(), _depthAsMask); + Parameters::parse(params, Parameters::kMemDepthMaskFloorThr(), _maskFloorThreshold); Parameters::parse(params, Parameters::kMemStereoFromMotion(), _stereoFromMotion); Parameters::parse(params, Parameters::kMemImagePreDecimation(), _imagePreDecimation); Parameters::parse(params, Parameters::kMemImagePostDecimation(), _imagePostDecimation); @@ -4884,7 +4888,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor imageMono.cols % decimatedData.depthRaw().cols == 0 && imageMono.rows/decimatedData.depthRaw().rows == imageMono.cols/decimatedData.depthRaw().cols) { - depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f); + depthMask = decimatedData.depthRaw(); + + if(_maskFloorThreshold != 0.0f) + { + UASSERT(!decimatedData.cameraModels().empty()); + UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold); + if(_maskFloorThreshold<0.0f) + { + cv::Mat depthBelow; + util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow); + depthMask = depthBelow; + } + else + { + depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold); + } + UDEBUG("Masking floor done."); + } + + depthMask = util2d::interpolate(depthMask, imageMono.rows/depthMask.rows, 0.1f); } else { @@ -5787,10 +5810,42 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor std::vector imageBytes; std::vector depthBytes; - if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1) + if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1) { - UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion (or set parameter \"Mem/SaveDepth16Format\"=false to use 32bits format)."); - depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage); + if(_saveDepth16Format) + { + static bool warned = false; + if(!warned) + { + UWARN("Converting depth data to 16 bits format because depth type detected is 32FC1, " + "feed 16UC1 depth format directly to avoid this conversion (or set parameter %s=false " + "to save 32bits format). This warning is only printed once.", + Parameters::kMemSaveDepth16Format().c_str()); + warned = true; + } + depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage); + } + else if(_depthCompressionFormat == ".rvl") + { + static bool warned = false; + if(!warned) + { + UWARN("%s is set to false to use 32bits format but this is not " + "compatible with the compressed depth format chosen (%s=\"%s\"), depth " + "images will be compressed in \".png\" format instead. Explicitly " + "set %s to true to keep using \"%s\" format and images will be " + "converted to 16bits for convenience (warning: that would " + "remove all depth values over 65 meters). Explicitly set %s=\".png\" " + "to suppress this warning. This warning is only printed once.", + Parameters::kMemSaveDepth16Format().c_str(), + Parameters::kMemDepthCompressionFormat().c_str(), + _depthCompressionFormat.c_str(), + Parameters::kMemSaveDepth16Format().c_str(), + _depthCompressionFormat.c_str(), + Parameters::kMemDepthCompressionFormat().c_str()); + warned = true; + } + } } cv::Mat compressedImage; @@ -5800,7 +5855,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor if(_compressionParallelized) { rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat); - rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):_rgbCompressionFormat); + rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat); rtabmap::CompressionThread ctLaserScan(laserScan.data()); rtabmap::CompressionThread ctUserData(data.userDataRaw()); if(!image.empty()) @@ -5832,7 +5887,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor else { compressedImage = compressImage2(image, _rgbCompressionFormat); - compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):_rgbCompressionFormat); + compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat); compressedScan = compressData2(laserScan.data()); compressedUserData = compressData2(data.userDataRaw()); } diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp index c16ee766..fc4ce0ca 100644 --- a/corelib/src/RegistrationVis.cpp +++ b/corelib/src/RegistrationVis.cpp @@ -94,6 +94,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration _guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()), _bundleAdjustment(Parameters::defaultVisBundleAdjustment()), _depthAsMask(Parameters::defaultVisDepthAsMask()), + _maskFloorThreshold(Parameters::defaultVisDepthMaskFloorThr()), _minInliersDistributionThr(Parameters::defaultVisMinInliersDistribution()), _maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance()), _detectorFrom(0), @@ -155,6 +156,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection); Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment); Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask); + Parameters::parse(parameters, Parameters::kVisDepthMaskFloorThr(), _maskFloorThreshold); Parameters::parse(parameters, Parameters::kVisMinInliersDistribution(), _minInliersDistributionThr); Parameters::parse(parameters, Parameters::kVisMeanInliersDistance(), _maxInliersMeanDistance); uInsert(_bundleParameters, parameters); @@ -423,13 +425,32 @@ Transform RegistrationVis::computeTransformationImpl( imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 && imageFrom.rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols) { - depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f); + depthMask = fromSignature.sensorData().depthRaw(); + + if(_maskFloorThreshold != 0.0f) + { + UASSERT(!fromSignature.sensorData().cameraModels().empty()); + UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold); + if(_maskFloorThreshold<0.0f) + { + cv::Mat depthBelow; + util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow); + depthMask = depthBelow; + } + else + { + depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold); + } + UDEBUG("Masking floor done."); + } + + depthMask = util2d::interpolate(depthMask, imageFrom.rows/depthMask.rows, 0.1f); } else { UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.", Parameters::kVisDepthAsMask().c_str(), - fromSignature.sensorData().imageRaw().rows, fromSignature.sensorData().imageRaw().cols, + imageFrom.rows, imageFrom.cols, fromSignature.sensorData().depthRaw().rows, fromSignature.sensorData().depthRaw().cols); } } @@ -770,13 +791,32 @@ Transform RegistrationVis::computeTransformationImpl( imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 && imageTo.rows/toSignature.sensorData().depthRaw().rows == imageTo.cols/toSignature.sensorData().depthRaw().cols) { - depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().rows, 0.1f); + depthMask = toSignature.sensorData().depthRaw(); + + if(_maskFloorThreshold != 0.0f) + { + UASSERT(!toSignature.sensorData().cameraModels().empty()); + UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold); + if(_maskFloorThreshold<0.0f) + { + cv::Mat depthBelow; + util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow); + depthMask = depthBelow; + } + else + { + depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold); + } + UDEBUG("Masking floor done."); + } + + depthMask = util2d::interpolate(depthMask, imageTo.rows/depthMask.rows, 0.1f); } else { UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.", Parameters::kVisDepthAsMask().c_str(), - toSignature.sensorData().imageRaw().rows, toSignature.sensorData().imageRaw().cols, + imageTo.rows, imageTo.cols, toSignature.sensorData().depthRaw().rows, toSignature.sensorData().depthRaw().cols); } } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 41db9a4e..79eb5bfb 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -716,6 +716,36 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) if(_memory) { + bool isMemIncremental = _memory->isIncremental(); + if(Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), isMemIncremental) && + isMemIncremental != _memory->isIncremental()) + { + // Mode has changed from Mapping to Localization, cleanup the local graph + if(_memory->isGraphReduced() && _memory->isIncremental()) + { + // Force reducing graph, then remove filtered nodes from the optimized poses + std::map reducedIds; + _memory->incrementMapId(&reducedIds); + for(std::map::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter) + { + _optimizedPoses.erase(iter->first); + } + } + + // In both cases, we save the latest optimized graph and latest localization pose + _memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose); + + // Mode changed from Localization to Mapping, clear local graph + if(!_memory->isIncremental()) { + _optimizedPoses.clear(); + _lastLocalizationPose.setNull(); + _mapCorrection.setIdentity(); + _mapCorrectionBackup.setNull(); + _localizationCovariance = cv::Mat(); + _lastLocalizationNodeId = 0; + } + } + _memory->parseParameters(parameters); if(_memory->isIncremental() && !_globalScanMap.empty()) { @@ -1720,6 +1750,7 @@ bool Rtabmap::process( _constraints.erase(--_constraints.end()); } } + _constraints.insert(std::make_pair(tmp.from(), tmp)); } // Localization mode stuff diff --git a/corelib/src/camera/CameraStereoZed.cpp b/corelib/src/camera/CameraStereoZed.cpp index 70d636b1..c3ea3a6d 100644 --- a/corelib/src/camera/CameraStereoZed.cpp +++ b/corelib/src/camera/CameraStereoZed.cpp @@ -318,12 +318,13 @@ CameraStereoZed::CameraStereoZed( sl::RESOLUTION res = static_cast(resolution_); sl::DEPTH_MODE qual = static_cast(quality_); - UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST); UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST); #if ZED_SDK_MAJOR_VERSION < 4 + UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST); sl::SENSING_MODE sens = static_cast(sensingMode_); UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST); #else + UASSERT(res >= sl::RESOLUTION::HD4K && res < sl::RESOLUTION::LAST); UASSERT(sensingMode_ >= 0 && sensingMode_ < 2); #endif UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100); diff --git a/corelib/src/optimizer/OptimizerCeres.cpp b/corelib/src/optimizer/OptimizerCeres.cpp index 5a62283a..04de80b9 100644 --- a/corelib/src/optimizer/OptimizerCeres.cpp +++ b/corelib/src/optimizer/OptimizerCeres.cpp @@ -37,22 +37,47 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef RTABMAP_CERES #include + +#if CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1) +#include +#else #include +#endif + #include "ceres/pose_graph_2d/types.h" #include "ceres/pose_graph_2d/pose_graph_2d_error_term.h" -#include "ceres/pose_graph_2d/angle_local_parameterization.h" +#include "ceres/pose_graph_2d/angle_manifold.h" #include "ceres/pose_graph_3d/types.h" #include "ceres/pose_graph_3d/pose_graph_3d_error_term.h" #include "ceres/bundle/BAProblem.h" #include "ceres/bundle/snavely_reprojection_error.h" #if not(CERES_VERSION_MAJOR > 1 || (CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 12)) -#include "ceres/pose_graph_3d/eigen_quaternion_parameterization.h" +#include "ceres/pose_graph_3d/eigen_quaternion_manifold.h" #endif #endif namespace rtabmap { +namespace { + +#ifdef RTABMAP_CERES +#if CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1) +inline void SetCeresProblemManifold(ceres::Problem& problem, double* params, + ceres::Manifold* manifold) { + problem.SetManifold(params, manifold); +#else +inline void SetCeresProblemManifold( + ceres::Problem& problem, double* params, + ceres::LocalParameterization* parameterization) { + problem.SetParameterization(params, parameterization); +#endif +} +#endif + +} // namespace bool OptimizerCeres::available() { @@ -118,8 +143,14 @@ std::map OptimizerCeres::optimize( } ceres::LossFunction* loss_function = NULL; - ceres::LocalParameterization* angle_local_parameterization = NULL; - ceres::LocalParameterization* quaternion_local_parameterization = NULL; +#if CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1) + ceres::Manifold* angle_local_manifold = NULL; + ceres::Manifold* quaternion_local_manifold = NULL; +#else + ceres::LocalParameterization* angle_local_manifold = NULL; + ceres::LocalParameterization* quaternion_local_manifold = NULL; +#endif for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) { @@ -164,12 +195,12 @@ std::map OptimizerCeres::optimize( &pose_begin_iter->second.x, &pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians, &pose_end_iter->second.x, &pose_end_iter->second.y, &pose_end_iter->second.yaw_radians); - if(angle_local_parameterization == NULL) + if(angle_local_manifold == NULL) { - angle_local_parameterization = ceres::examples::AngleLocalParameterization::Create(); + angle_local_manifold = ceres::examples::AngleManfold::Create(); } - problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization); - problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization); + SetCeresProblemManifold(problem, &pose_begin_iter->second.yaw_radians, angle_local_manifold); + SetCeresProblemManifold(problem, &pose_end_iter->second.yaw_radians, angle_local_manifold); } else { @@ -194,12 +225,17 @@ std::map OptimizerCeres::optimize( problem.AddResidualBlock(cost_function, loss_function, pose_begin_iter->second.p.data(), pose_begin_iter->second.q.coeffs().data(), pose_end_iter->second.p.data(), pose_end_iter->second.q.coeffs().data()); - if(quaternion_local_parameterization == NULL) + if(quaternion_local_manifold == NULL) { - quaternion_local_parameterization = new ceres::EigenQuaternionParameterization; +#if CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1) + quaternion_local_manifold = new ceres::EigenQuaternionManifold; +#else + quaternion_local_manifold = new ceres::EigenQuaternionParameterization; +#endif } - problem.SetParameterization(pose_begin_iter->second.q.coeffs().data(), quaternion_local_parameterization); - problem.SetParameterization(pose_end_iter->second.q.coeffs().data(), quaternion_local_parameterization); + SetCeresProblemManifold(problem, pose_begin_iter->second.q.coeffs().data(), quaternion_local_manifold); + SetCeresProblemManifold(problem, pose_end_iter->second.q.coeffs().data(), quaternion_local_manifold); } } //else // not supporting pose prior and landmarks diff --git a/corelib/src/optimizer/OptimizerG2O.cpp b/corelib/src/optimizer/OptimizerG2O.cpp index c292b4d8..afa6b0c8 100644 --- a/corelib/src/optimizer/OptimizerG2O.cpp +++ b/corelib/src/optimizer/OptimizerG2O.cpp @@ -1037,6 +1037,12 @@ std::map OptimizerG2O::optimize( int it = 0; UTimer timer; double lastError = 0.0; + + if (!optimizer.solver()->init()) { + UERROR("g2o: Error while initializing solver"); + return optimizedPoses; + } + if(intermediateGraphes || this->epsilon() > 0.0) { for(int i=0; i OptimizerG2O::optimize( } } - it += optimizer.optimize(1); + g2o::OptimizationAlgorithm::SolverResult result = optimizer.solver()->solve(i); + ++it; // early stop condition optimizer.computeActiveErrors(); @@ -1163,6 +1170,12 @@ std::map OptimizerG2O::optimize( return optimizedPoses; } + if(result == g2o::OptimizationAlgorithm::Fail) + { + UERROR("g2o: Solver failed, aborting optimization!"); + return optimizedPoses; + } + double errorDelta = lastError - chi2; if(i>0 && errorDelta < this->epsilon()) { diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp index 571b8170..53c87edb 100644 --- a/corelib/src/optimizer/OptimizerGTSAM.cpp +++ b/corelib/src/optimizer/OptimizerGTSAM.cpp @@ -209,7 +209,7 @@ std::map OptimizerGTSAM::optimize( UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false"); if(isSlam2d()) { - gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits::min())); + gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:1e-9)); graph.add(gtsam::PriorFactor(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise)); addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1)); } @@ -217,7 +217,7 @@ std::map OptimizerGTSAM::optimize( { gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances( (gtsam::Vector(6) << - (hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:std::numeric_limits::min()), // roll, pitch, fixed yaw if there are no priors + (hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:1e-9), // roll, pitch, fixed yaw if there are no priors (hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz ).finished()); graph.add(gtsam::PriorFactor(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise)); @@ -924,22 +924,42 @@ std::map OptimizerGTSAM::optimize( // early stop condition UDEBUG("iteration %d error =%f", i+1, error); double errorDelta = lastError - error; - if((isam2_ || i>0) && errorDelta < this->epsilon()) + if(this->epsilon() > 0.0 && fabs(error) > 1000000000000.0) { - if(errorDelta < 0) - { - UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon()); - } - else - { - UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon()); - break; - } + UWARN("Error computed (%e) is very huge and/or diverging! Aborting! " + "Set %s to 0 to ignore that check and keep iterating up to %s (%d).", + error, + Parameters::kOptimizerEpsilon().c_str(), + Parameters::kOptimizerIterations().c_str(), + this->iterations()); + return optimizedPoses; } - else if(i==0 && error < this->epsilon()) + else { - UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon()); - break; + if((isam2_ || i>0) && errorDelta < this->epsilon()) + { + if(errorDelta < 0) + { + UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon()); + } + else + { + UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon()); + break; + } + } + else if(i==0) + { + if(error < 0) + { + UDEBUG("Negative error?! Ignore and continue optimizing... (%f)", error); + } + else if(error < this->epsilon()) + { + UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon()); + break; + } + } } lastError = error; } diff --git a/corelib/src/optimizer/ceres/pose_graph_2d/angle_local_parameterization.h b/corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h similarity index 63% rename from corelib/src/optimizer/ceres/pose_graph_2d/angle_local_parameterization.h rename to corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h index 428ccccd..2f551038 100644 --- a/corelib/src/optimizer/ceres/pose_graph_2d/angle_local_parameterization.h +++ b/corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h @@ -28,10 +28,16 @@ // // Author: vitus@google.com (Michael Vitus) -#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_ -#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_ +#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_ +#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_ -#include "ceres/local_parameterization.h" +#if CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1) +#include +#include +#else +#include +#endif #include "normalize_angle.h" namespace ceres { @@ -39,7 +45,39 @@ namespace examples { // Defines a local parameterization for updating the angle to be constrained in // [-pi to pi). -class AngleLocalParameterization { + +#if CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1) + +// Defines a manifold for updating the angle to be constrained in [-pi to pi). +class AngleManifold { + public: + template + bool Plus(const T* x_radians, + const T* delta_radians, + T* x_plus_delta_radians) const { + *x_plus_delta_radians = NormalizeAngle(*x_radians + *delta_radians); + return true; + } + + template + bool Minus(const T* y_radians, + const T* x_radians, + T* y_minus_x_radians) const { + *y_minus_x_radians = + NormalizeAngle(*y_radians) - NormalizeAngle(*x_radians); + + return true; + } + + static ceres::Manifold* Create() { + return new ceres::AutoDiffManifold; + } +}; + +#else + +class AngleManfold { public: template @@ -52,12 +90,13 @@ class AngleLocalParameterization { } static ceres::LocalParameterization* Create() { - return (new ceres::AutoDiffLocalParameterization); + return (new ceres::AutoDiffLocalParameterization); } }; +#endif + } // namespace examples } // namespace ceres -#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_ +#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_ diff --git a/corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h b/corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h index ae03ebda..ffab3042 100644 --- a/corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h +++ b/corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h @@ -31,7 +31,7 @@ #ifndef CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_ #define CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_ -#include "ceres/local_parameterization.h" +#include "ceres/manifold.h" namespace ceres { @@ -46,7 +46,7 @@ namespace ceres { // // Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x // with * being the quaternion multiplication operator. -class EigenQuaternionParameterization : public ceres::LocalParameterization { +class EigenQuaternionParameterization : public ceres::Manifold { public: virtual ~EigenQuaternionParameterization() {} virtual bool Plus(const double* x_ptr, diff --git a/corelib/src/rvl_codec.cpp b/corelib/src/rvl_codec.cpp new file mode 100644 index 00000000..8d2f303a --- /dev/null +++ b/corelib/src/rvl_codec.cpp @@ -0,0 +1,102 @@ +// The following code is a C++ wrapper of the code presented by +// Andrew D. Wilson in "Fast Lossless Depth Image Compression" at SIGCHI'17. +// The original code is licensed under the MIT License. + +#include + +namespace rtabmap +{ + +RvlCodec::RvlCodec() {} + +void RvlCodec::EncodeVLE(int value) +{ + do + { + int nibble = value & 0x7; // lower 3 bits + if (value >>= 3) + nibble |= 0x8; // more to come + word_ <<= 4; + word_ |= nibble; + if (++nibblesWritten_ == 8) // output word + { + *pBuffer_++ = word_; + nibblesWritten_ = 0; + word_ = 0; + } + } while (value); +} + +int RvlCodec::DecodeVLE() +{ + unsigned int nibble; + int value = 0, bits = 29; + do + { + if (!nibblesWritten_) + { + word_ = *pBuffer_++; // load word + nibblesWritten_ = 8; + } + nibble = word_ & 0xf0000000; + value |= (nibble << 1) >> bits; + word_ <<= 4; + nibblesWritten_--; + bits -= 3; + } while (nibble & 0x80000000); + return value; +} + +int RvlCodec::CompressRVL(const uint16_t * input, unsigned char * output, int numPixels) +{ + buffer_ = pBuffer_ = reinterpret_cast(output); + nibblesWritten_ = 0; + const uint16_t * end = input + numPixels; + uint16_t previous = 0; + while (input != end) + { + int zeros = 0, nonzeros = 0; + for (; (input != end) && !*input; input++, zeros++) {} + EncodeVLE(zeros); // number of zeros + for (const uint16_t * p = input; (p != end) && *p++; nonzeros++) {} + EncodeVLE(nonzeros); // number of nonzeros + for (int i = 0; i < nonzeros; i++) + { + uint16_t current = *input++; + int delta = current - previous; + int positive = (delta << 1) ^ (delta >> 31); + EncodeVLE(positive); // nonzero value + previous = current; + } + } + if (nibblesWritten_) // last few values + *pBuffer_++ = word_ << 4 * (8 - nibblesWritten_); + return static_cast((unsigned char *)pBuffer_ - (unsigned char *)buffer_); // num bytes +} + +void RvlCodec::DecompressRVL(const unsigned char * input, uint16_t * output, int numPixels) +{ + buffer_ = pBuffer_ = const_cast(reinterpret_cast(input)); + nibblesWritten_ = 0; + uint16_t current, previous = 0; + int numPixelsToDecode = numPixels; + while (numPixelsToDecode) + { + int zeros = DecodeVLE(); // number of zeros + numPixelsToDecode -= zeros; + for (; zeros; zeros--) + *output++ = 0; + int nonzeros = DecodeVLE(); // number of nonzeros + numPixelsToDecode -= nonzeros; + for (; nonzeros; nonzeros--) + { + int positive = DecodeVLE(); // nonzero value + int delta = (positive >> 1) ^ -(positive & 1); + current = previous + delta; + *output++ = current; + previous = current; + } + } +} + +} // namespace rtabmap diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 606dfa69..0fa21329 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -3007,6 +3007,115 @@ void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection, } } +cv::Mat filterFloor(const cv::Mat & depth, const std::vector & cameraModels, float threshold, cv::Mat * depthBelow) +{ + cv::Mat output = depth.clone(); + if(depth.empty()) + { + return output; + } + if(depthBelow) + { + *depthBelow = cv::Mat::zeros(output.size(), output.type()); + } + + UASSERT(!cameraModels.empty()); + UASSERT(cameraModels[0].isValidForReprojection()); + // Support camera model with different resolution than depth image + float rgbToDepthFactorX = float(cameraModels[0].imageWidth()) / float(output.cols/cameraModels.size()); + float rgbToDepthFactorY = float(cameraModels[0].imageHeight()) / float(output.rows); + int depthWidth = output.cols/cameraModels.size(); + UASSERT(depthWidth*(int)cameraModels.size() == output.cols); + + // for each camera + for(size_t i=0; i0) + { + // Make sure all models are the same resolution + UASSERT(cam.imageWidth() == cameraModels[i-1].imageWidth()); + UASSERT(cam.imageHeight() == cameraModels[i-1].imageHeight()); + } + + float depthFx = cam.fx() / rgbToDepthFactorX; + float depthFy = cam.fy() / rgbToDepthFactorY; + float depthCx = cam.cx() / rgbToDepthFactorX; + float depthCy = cam.cy() / rgbToDepthFactorY; + + cv::Mat subImage = output.colRange(cv::Range(i*depthWidth, (i+1)*depthWidth)); + cv::Mat subImageBelow; + if(depthBelow) + subImageBelow = depthBelow->colRange(cv::Range(i*depthWidth, (i+1)*depthWidth)); + + for(int y=0; y 0) + { + float d = float(ptr[x])/1000.0f; + cv::Point3f pt; + pt.x = (x - depthCx) * d / depthFx; + pt.y = (y - depthCy) * d / depthFy; + pt.z = d; + pt = util3d::transformPoint(pt, localTransform); + if(pt.z < threshold) + { + if(ptrBelow) + { + ptrBelow[x] = ptr[x]; + } + ptr[x] = 0; + } + } + } + } + else // CV_32FC1 + { + float * ptr = (float *)subImage.row(y).ptr(); + float * ptrBelow = 0; + if(depthBelow) + { + ptrBelow = (float *)subImageBelow.row(y).ptr(); + } + for(int x=0; x 0.0f) + { + float & d = ptr[x]; + cv::Point3f pt; + pt.x = (x - depthCx) * d / depthFx; + pt.y = (y - depthCy) * d / depthFy; + pt.z = d; + pt = util3d::transformPoint(pt, localTransform); + if(pt.z < threshold) + { + if(ptrBelow) + { + ptrBelow[x] = ptr[x]; + } + d = 0; + } + } + } + } + } + } + return output; +} + class ProjectionInfo { public: ProjectionInfo(): diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index dafe7dbe..5fb1cad6 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -694,7 +694,7 @@ typename pcl::PointCloud::Ptr voxelizeImpl( if ((dx*dy*dz) > static_cast(std::numeric_limits::max())) { - UWARN("Leaf size is too small for the input dataset. Integer indices would overflow. " + UDEBUG("Leaf size is too small for the input dataset. Integer indices would overflow. " "We will split space to be able to voxelize (lvl=%d cloud=%d min=[%f %f %f] max=[%f %f %f] voxel=%f).", level, (int)(indices->empty()?cloud->size():indices->size()), @@ -2155,7 +2155,7 @@ pcl::IndicesPtr normalFilteringImpl( for(unsigned int i=0; isize(); ++i) { Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f); - if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->size()!=0?indices->at(i):i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground + if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->size()!=0?indices->at(i):i).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground { //reverse normal v *= -1.0f; @@ -2226,7 +2226,7 @@ pcl::IndicesPtr normalFilteringImpl( for(unsigned int i=0; isize(); ++i) { Eigen::Vector4f v(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f); - if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->at(i)).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground + if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->at(i)).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground { //reverse normal v *= -1.0f; @@ -2244,7 +2244,7 @@ pcl::IndicesPtr normalFilteringImpl( for(unsigned int i=0; isize(); ++i) { Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f); - if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground + if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(i).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground { //reverse normal v *= -1.0f; diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index adfc1248..1a50258e 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -3530,7 +3530,7 @@ LaserScan adjustNormalsToViewPoint( float result = v.dot(n); if(result < 0 - || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground + || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[2])) // some far velodyne rays on road can have normals toward ground { //reverse normal ptr[nx] *= -1.0f; @@ -3569,7 +3569,7 @@ void adjustNormalsToViewPointImpl( float result = v.dot(n); if(result < 0 - || (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground + || (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint[2])) // some far velodyne rays on road can have normals toward ground { //reverse normal cloud->points[i].normal_x *= -1.0f; @@ -3625,6 +3625,67 @@ void adjustNormalsToViewPoint( adjustNormalsToViewPointImpl(cloud, viewpoint, groundNormalsUp); } +template +void adjustNormalsToViewPointsImpl( + const std::map & poses, + const std::vector & cameraIndices, + typename pcl::PointCloud::Ptr & cloud, + float groundNormalsUp) +{ + if(poses.size() && cloud->size() == cameraIndices.size() && cloud->size()) + { + #pragma omp parallel for + for(int i=0; i<(int)cloud->size(); ++i) + { + pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z); + if(pcl::isFinite(normal)) + { + const Transform & p = poses.at(cameraIndices[i]); + pcl::PointXYZ viewpoint(p.x(), p.y(), p.z()); + Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap(); + + Eigen::Vector3f n(normal.x, normal.y, normal.z); + + float result = v.dot(n); + if(result < 0 || + (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground) + { + //reverse normal + cloud->points[i].normal_x *= -1.0f; + cloud->points[i].normal_y *= -1.0f; + cloud->points[i].normal_z *= -1.0f; + } + } + } + } +} + +void adjustNormalsToViewPoints( + const std::map & poses, + const std::vector & cameraIndices, + pcl::PointCloud::Ptr & cloud, + float groundNormalsUp) +{ + adjustNormalsToViewPointsImpl(poses, cameraIndices, cloud, groundNormalsUp); +} + +void adjustNormalsToViewPoints( + const std::map & poses, + const std::vector & cameraIndices, + pcl::PointCloud::Ptr & cloud, + float groundNormalsUp) +{ + adjustNormalsToViewPointsImpl(poses, cameraIndices, cloud, groundNormalsUp); +} + +void adjustNormalsToViewPoints( + const std::map & poses, + const std::vector & cameraIndices, + pcl::PointCloud::Ptr & cloud, + float groundNormalsUp) +{ + adjustNormalsToViewPointsImpl(poses, cameraIndices, cloud, groundNormalsUp); +} template void adjustNormalsToViewPointsImpl( diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 640bdc96..efd5fbd4 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2423,7 +2423,8 @@ void DatabaseViewer::editDepthImage() UASSERT(data.depthRaw().type() == depth.type()); UASSERT(data.depthRaw().cols == depth.cols); UASSERT(data.depthRaw().rows == depth.rows); - dbDriver_->updateDepthImage(id, depth); + std::string depthFormat = compressedDepthFormat(data.depthOrRightCompressed()); + dbDriver_->updateDepthImage(id, depth, depthFormat); this->update3dView(); } } @@ -2501,115 +2502,9 @@ void DatabaseViewer::exportPoses(int format) return; } - if(format == 5) + if(format == 5 && (gpsValues_.empty() || gpsPoses_.empty())) { - if(gpsValues_.empty() || gpsPoses_.empty()) - { - QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!")); - } - else - { - std::map graph; - if(groundTruth) - { - graph = groundTruthPoses_; - } - else if(odometry) - { - graph = odomPoses_; - } - else - { - graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); - } - - - //align with ground truth for more meaningful results - pcl::PointCloud cloud1, cloud2; - cloud1.resize(graph.size()); - cloud2.resize(graph.size()); - int oi = 0; - int idFirst = 0; - for(std::map::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter) - { - std::map::iterator iter2 = graph.find(iter->first); - if(iter2!=graph.end()) - { - if(oi==0) - { - idFirst = iter->first; - } - cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()); - cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z()); - } - } - - Transform t = Transform::getIdentity(); - if(oi>5) - { - cloud1.resize(oi); - cloud2.resize(oi); - - t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1); - } - else if(idFirst) - { - t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse(); - } - - std::map values; - GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords(); - for(std::map::iterator iter=graph.begin(); iter!=graph.end(); ++iter) - { - iter->second = t * iter->second; - - GeodeticCoords coord; - coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin); - double bearing = -(iter->second.theta()*180.0/M_PI-90.0); - if(bearing < 0) - { - bearing += 360; - } - - Transform p, g; - int w; - std::string l; - double stamp=0.0; - int mapId; - std::vector v; - GPS gps; - EnvSensors sensors; - dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors); - values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0))); - } - - QString output = pathDatabase_ + QDir::separator() + "poses.kml"; - QString path = QFileDialog::getSaveFileName( - this, - tr("Save File"), - output, - tr("Google Earth file (*.kml)")); - - if(!path.isEmpty()) - { - bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba()); - - if(saved) - { - QMessageBox::information(this, - tr("Export poses..."), - tr("GPS coordinates saved to \"%1\".") - .arg(path)); - } - else - { - QMessageBox::information(this, - tr("Export poses..."), - tr("Failed to save GPS coordinates to \"%1\"!") - .arg(path)); - } - } - } + QMessageBox::warning(this, tr("Cannot export poses in KML format"), tr("No GPS in database?!")); return; } @@ -2618,70 +2513,132 @@ void DatabaseViewer::exportPoses(int format) { optimizedPoses = groundTruthPoses_; } + else if(odometry) + { + optimizedPoses = odomPoses_; + } else { - if(odometry) + optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + } + + bool alignToGPS = + (ui_->checkBox_alignPosesWithGPS->isEnabled() && + ui_->checkBox_alignPosesWithGPS->isChecked()) || + format == 5; + + if(alignToGPS || + (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) + { + std::map refPoses = groundTruthPoses_; + if(alignToGPS) { - optimizedPoses = odomPoses_; - } - else - { - optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + refPoses = gpsPoses_; } - if((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) || - (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) + // Log ground truth statistics (in TUM's RGBD-SLAM format) + if(refPoses.size()) { - std::map refPoses = groundTruthPoses_; - if(ui_->checkBox_alignPosesWithGPS->isEnabled() && - ui_->checkBox_alignPosesWithGPS->isChecked()) + float translational_rmse = 0.0f; + float translational_mean = 0.0f; + float translational_median = 0.0f; + float translational_std = 0.0f; + float translational_min = 0.0f; + float translational_max = 0.0f; + float rotational_rmse = 0.0f; + float rotational_mean = 0.0f; + float rotational_median = 0.0f; + float rotational_std = 0.0f; + float rotational_min = 0.0f; + float rotational_max = 0.0f; + + Transform gtToMap = graph::calcRMSE( + refPoses, + optimizedPoses, + translational_rmse, + translational_mean, + translational_median, + translational_std, + translational_min, + translational_max, + rotational_rmse, + rotational_mean, + rotational_median, + rotational_std, + rotational_min, + rotational_max, + alignToGPS); + + if(!gtToMap.isIdentity()) { - refPoses = gpsPoses_; - } - - // Log ground truth statistics (in TUM's RGBD-SLAM format) - if(refPoses.size()) - { - float translational_rmse = 0.0f; - float translational_mean = 0.0f; - float translational_median = 0.0f; - float translational_std = 0.0f; - float translational_min = 0.0f; - float translational_max = 0.0f; - float rotational_rmse = 0.0f; - float rotational_mean = 0.0f; - float rotational_median = 0.0f; - float rotational_std = 0.0f; - float rotational_min = 0.0f; - float rotational_max = 0.0f; - - Transform gtToMap = graph::calcRMSE( - refPoses, - optimizedPoses, - translational_rmse, - translational_mean, - translational_median, - translational_std, - translational_min, - translational_max, - rotational_rmse, - rotational_mean, - rotational_median, - rotational_std, - rotational_min, - rotational_max); - - if(!gtToMap.isIdentity()) + for(std::map::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) { + iter->second = gtToMap * iter->second; + } + if(alignToGPS && format != 5 && optimizedPoses.find(gpsValues_.begin()->first)!=optimizedPoses.end()) + { + // This will make the exported first pose the GPS origin. Don't do it for KML format as is it done implicitly below. + int originId = gpsValues_.begin()->first; + Transform offset = optimizedPoses.at(originId).translation().inverse(); for(std::map::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) { - iter->second = gtToMap * iter->second; + iter->second = offset * iter->second; } } } } } + if(format == 5) + { + std::map values; + GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords(); + for(std::map::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) + { + GeodeticCoords coord; + coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin); + + Transform p, g; + int w; + std::string l; + double stamp=0.0; + int mapId; + std::vector v; + GPS gps; + EnvSensors sensors; + dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors); + values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0))); + } + + QString output = pathDatabase_ + QDir::separator() + "poses.kml"; + QString path = QFileDialog::getSaveFileName( + this, + tr("Save File"), + output, + tr("Google Earth file (*.kml)")); + + if(!path.isEmpty()) + { + bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba()); + + if(saved) + { + QMessageBox::information(this, + tr("Export poses..."), + tr("GPS coordinates saved to \"%1\".") + .arg(path)); + } + else + { + QMessageBox::information(this, + tr("Export poses..."), + tr("Failed to save GPS coordinates to \"%1\"!") + .arg(path)); + } + } + return; + } + if(optimizedPoses.size()) { std::map localTransforms; @@ -4173,6 +4130,72 @@ void DatabaseViewer::generate3DMap() else { optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + + bool alignToGPS = + ui_->checkBox_alignPosesWithGPS->isEnabled() && + ui_->checkBox_alignPosesWithGPS->isChecked(); + + if(alignToGPS || + (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) + { + std::map refPoses = groundTruthPoses_; + if(alignToGPS) + { + refPoses = gpsPoses_; + } + + // Log ground truth statistics (in TUM's RGBD-SLAM format) + if(refPoses.size()) + { + float translational_rmse = 0.0f; + float translational_mean = 0.0f; + float translational_median = 0.0f; + float translational_std = 0.0f; + float translational_min = 0.0f; + float translational_max = 0.0f; + float rotational_rmse = 0.0f; + float rotational_mean = 0.0f; + float rotational_median = 0.0f; + float rotational_std = 0.0f; + float rotational_min = 0.0f; + float rotational_max = 0.0f; + + Transform gtToMap = graph::calcRMSE( + refPoses, + optimizedPoses, + translational_rmse, + translational_mean, + translational_median, + translational_std, + translational_min, + translational_max, + rotational_rmse, + rotational_mean, + rotational_median, + rotational_std, + rotational_min, + rotational_max, + alignToGPS); + + if(!gtToMap.isIdentity()) + { + for(std::map::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) + { + iter->second = gtToMap * iter->second; + } + if(alignToGPS && optimizedPoses.find(gpsValues_.begin()->first)!=optimizedPoses.end()) + { + // This will make the exported first pose the GPS origin. + int originId = gpsValues_.begin()->first; + Transform offset = optimizedPoses.at(originId).translation().inverse(); + for(std::map::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) + { + iter->second = offset * iter->second; + } + } + } + } + } } if(ui_->groupBox_posefiltering->isChecked()) { @@ -4870,9 +4893,7 @@ void DatabaseViewer::update(int value, dbDriver_->loadLinks(id, gravityLink, Link::kGravity); if(!gravityLink.empty()) { - float roll,pitch,yaw; - gravityLink.begin()->second.transform().getEulerAngles(roll, pitch, yaw); - Eigen::Vector3d v = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ(); + Eigen::Vector3f v = gravityLink.begin()->second.transform().inverse().toEigen3f() * -Eigen::Vector3f::UnitZ(); labelGravity->setText(QString("x=%1 y=%2 z=%3").arg(v[0]).arg(v[1]).arg(v[2])); labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw)); } @@ -5098,13 +5119,8 @@ void DatabaseViewer::update(int value, if(!gravityLink.empty() && ui_->checkBox_gravity_3dview->isChecked()) { Transform gravityT = gravityLink.begin()->second.transform(); - Eigen::Vector3f gravity(0,0,-1); - if(pose.isIdentity()) - { - gravityT = gravityT.inverse(); - } - gravity = (gravityT.rotation()*(pose).rotation().inverse()).toEigen3f()*gravity; - cloudViewer_->addOrUpdateLine("gravity", pose, (pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, false); + Eigen::Vector3f gravity = gravityT.inverse().toEigen3f()*-Eigen::Vector3f::UnitZ(); + cloudViewer_->addOrUpdateLine("gravity", pose, pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::yellow, true, false); } //add scan @@ -6960,7 +6976,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) std::map graph = uValueAt(graphes_, value); std::map refPoses = groundTruthPoses_; - if(ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) + bool alignToGPS = ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked(); + if(alignToGPS) { refPoses = gpsPoses_; } @@ -7006,7 +7023,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) rotational_median, rotational_std, rotational_min, - rotational_max); + rotational_max, + alignToGPS); // ground truth live statistics ui_->label_rmse->setNum(translational_rmse); @@ -7024,7 +7042,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) UINFO("rotational_min=%f", rotational_min); UINFO("rotational_max=%f", rotational_max); - if(((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) || + if((alignToGPS || (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) && !gtToMap.isIdentity()) { diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 6fd82ea7..a3fe346b 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -816,7 +816,7 @@ void ExportCloudsDialog::restoreDefaults() _ui->doubleSpinBox_gp3Mu->setValue(2.5); _ui->doubleSpinBox_meshDecimationFactor->setValue(0.0); _ui->spinBox_meshMaxPolygons->setValue(0); - _ui->doubleSpinBox_transferColorRadius->setValue(0.025); + _ui->doubleSpinBox_transferColorRadius->setValue(0.05); _ui->checkBox_cleanMesh->setChecked(true); _ui->spinBox_mesh_minClusterSize->setValue(0); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 03544dd7..2a1e6284 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -3094,8 +3094,8 @@ void MainWindow::updateMapCloud( { Transform gravityT = linkIter->second.transform(); Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(0)); - gravity = (gravityT.rotation()*(iter->second).rotation().inverse()).toEigen3f()*gravity; - _cloudViewer->addOrUpdateLine(gravityName, iter->second, (iter->second).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*iter->second.rotation().inverse(), Qt::yellow, false, false); + gravity = gravityT.inverse().toEigen3f()*gravity; + _cloudViewer->addOrUpdateLine(gravityName, iter->second, iter->second*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::yellow, false, false); } } else if(viewerLines.find(gravityName)!=viewerLines.end()) diff --git a/guilib/src/ParametersToolBox.cpp b/guilib/src/ParametersToolBox.cpp index 1bd583d2..47fa560a 100644 --- a/guilib/src/ParametersToolBox.cpp +++ b/guilib/src/ParametersToolBox.cpp @@ -405,7 +405,8 @@ void ParametersToolBox::addParameter(QVBoxLayout * layout, // set minimum for selected parameters if(key.compare(Parameters::kGridMinGroundHeight().c_str()) == 0 || key.compare(Parameters::kGridMaxGroundHeight().c_str()) == 0 || - key.compare(Parameters::kGridMaxObstacleHeight().c_str()) == 0) + key.compare(Parameters::kGridMaxObstacleHeight().c_str()) == 0 || + key.compare(Parameters::kVisDepthMaskFloorThr().c_str()) == 0) { widget->setMinimum(-1000000.0); } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 39e606cb..8e69a498 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -995,6 +995,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->general_checkBox_keepBinaryData->setObjectName(Parameters::kMemBinDataKept().c_str()); _ui->general_checkBox_saveIntermediateNodeData->setObjectName(Parameters::kMemIntermediateNodeDataKept().c_str()); _ui->lineEdit_rgbCompressionFormat->setObjectName(Parameters::kMemImageCompressionFormat().c_str()); + _ui->lineEdit_depthCompressionFormat->setObjectName(Parameters::kMemDepthCompressionFormat().c_str()); _ui->general_checkBox_keepDescriptors->setObjectName(Parameters::kMemRawDescriptorsKept().c_str()); _ui->general_checkBox_saveDepth16bits->setObjectName(Parameters::kMemSaveDepth16Format().c_str()); _ui->general_checkBox_compressionParallelized->setObjectName(Parameters::kMemCompressionParallelized().c_str()); @@ -1057,6 +1058,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str()); _ui->surf_doubleSpinBox_minDepth->setObjectName(Parameters::kKpMinDepth().c_str()); _ui->checkBox_memDepthAsMask->setObjectName(Parameters::kMemDepthAsMask().c_str()); + _ui->doubleSpinBox_memDepthMaskFloorThr->setObjectName(Parameters::kMemDepthMaskFloorThr().c_str()); _ui->checkBox_memStereoFromMotion->setObjectName(Parameters::kMemStereoFromMotion().c_str()); _ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpMaxFeatures().c_str()); _ui->checkBox_kp_ssc->setObjectName(Parameters::kKpSSC().c_str()); @@ -1289,6 +1291,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kVisMaxDepth().c_str()); _ui->loopClosure_bowMinDepth->setObjectName(Parameters::kVisMinDepth().c_str()); _ui->checkBox_visDepthAsMask->setObjectName(Parameters::kVisDepthAsMask().c_str()); + _ui->doubleSpinBox_visDepthMaskFloorThr->setObjectName(Parameters::kVisDepthMaskFloorThr().c_str()); _ui->loopClosure_roi->setObjectName(Parameters::kVisRoiRatios().c_str()); _ui->subpix_winSize->setObjectName(Parameters::kVisSubPixWinSize().c_str()); _ui->subpix_iterations->setObjectName(Parameters::kVisSubPixIterations().c_str()); @@ -4869,22 +4872,46 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string { if(valueInt==1 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0) { - UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built " - "with g2o. Keeping default combo value: %s.", - combo->objectName().toStdString().c_str(), - combo->currentText().toStdString().c_str()); - ok = false; + if(Optimizer::isAvailable(Optimizer::kTypeGTSAM)) { + UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built " + "with g2o. Falling back to GTSAM.", + combo->objectName().toStdString().c_str()); + valueInt = 2; + } + else + { + UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built " + "with g2o. Keeping default combo value: %s.", + combo->objectName().toStdString().c_str(), + combo->currentText().toStdString().c_str()); + ok = false; + } } } if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM)) { if(valueInt==2 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0) { - UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built " - "with GTSAM. Keeping default combo value: %s.", - combo->objectName().toStdString().c_str(), - combo->currentText().toStdString().c_str()); - ok = false; + if( +#ifndef RTABMAP_ORB_SLAM + Optimizer::isAvailable(Optimizer::kTypeG2O) +#else + true +#endif + ){ + UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built " + "with GTSAM. Falling back to g2o.", + combo->objectName().toStdString().c_str()); + valueInt = 1; + } + else + { + UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built " + "with GTSAM. Keeping default combo value: %s.", + combo->objectName().toStdString().c_str(), + combo->currentText().toStdString().c_str()); + ok = false; + } } } if(ok) diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index 4667fe68..a12695ba 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -23,9 +23,9 @@ 0 - -1328 + -2995 885 - 6169 + 6152 @@ -2353,7 +2353,7 @@ By Node ID and Camera Index: NodeID*10+CameraIndex m - 2 + 3 -1.000000000000000 diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 8bf2e9c0..91a45e44 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -269 + 0 713 4653 @@ -95,7 +95,7 @@ QFrame::Raised - 5 + 9 @@ -10315,7 +10315,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory. + Initialize the Working Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory. true @@ -10666,16 +10666,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 0 means that the response (hessian) threshold -used for the detector will not be adapted. -Otherwise, the threshold is modified to -generate the number of words requested. - + + - Maximum words per image (0=no maximum). Setting to -1 will disable features extraction, so disabling loop closure detection indirectly. + Maximum words depth (0 means inf). Only used when a depth image is provided. Applied before "Maximum words per image". true @@ -10685,61 +10679,26 @@ generate the number of words requested. - - - - Bad signature ratio (less than Ratio x AverageWordsPerImage = bad). + + + + % - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Top ROI ratio (0 = no change). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - 2 + 0 + + + + - 0.000000000000000 + -1 - 1.000000000000000 - - - 0.050000000000000 + 2000 - 0.250000000000000 - - - - - - - Minimum words depth. Only used when a depth image is provided. Applied before "Maximum words per image". - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + 500 @@ -10827,17 +10786,75 @@ generate the number of words requested. + + + + Number of columns of the grid used to extract uniformly "max words / grid cells" features from each cell. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Minimum words depth. Only used when a depth image is provided. Applied before "Maximum words per image". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + If true, SSC (Suppression via Square Covering) is applied to limit keypoints. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + 2 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.050000000000000 + + + 0.250000000000000 + + + + true - - + + - ROI ratios [left, right, top, bottom] between 0 and 1. + Triangulate features without depth using stereo from motion (odometry). It would be ignored if depth as mask is checked and the feature detector used supports masking. true @@ -10847,21 +10864,8 @@ generate the number of words requested. - - - - Maximum words depth (0 means inf). Only used when a depth image is provided. Applied before "Maximum words per image". - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - + + % @@ -10870,19 +10874,6 @@ generate the number of words requested. - - - - Visual word type. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -10905,30 +10896,10 @@ generate the number of words requested. - - - - -1 - - - 2000 - - - 500 - - - - - + + - - - - - - - - If true, SSC (Suppression via Square Covering) is applied to limit keypoints. + Use depth image as mask when extracting features. true @@ -10938,30 +10909,7 @@ generate the number of words requested. - - - - % - - - 0 - - - - - - - Left ROI ratio (0 = no change). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + Right ROI ratio (0 = no change). @@ -10974,30 +10922,10 @@ generate the number of words requested. - - - - % - - - 0 - - - - - - - % - - - 0 - - - - + - Bottom ROI ratio (0 = no change). + Top ROI ratio (0 = no change). true @@ -11007,6 +10935,131 @@ generate the number of words requested. + + + + 0 means that the response (hessian) threshold +used for the detector will not be adapted. +Otherwise, the threshold is modified to +generate the number of words requested. + + + Maximum words per image (0=no maximum). Setting to -1 will disable features extraction, so disabling loop closure detection indirectly. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Visual word type. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Left ROI ratio (0 = no change). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + % + + + 0 + + + + + + + 1 + + + 99 + + + 1 + + + + + + + + + + + + + + ROI ratios [left, right, top, bottom] between 0 and 1. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + % + + + 0 + + + + + + + + + + + + + + Bad signature ratio (less than Ratio x AverageWordsPerImage = bad). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + @@ -11029,7 +11082,7 @@ generate the number of words requested. - + Number of rows of the grid used to extract uniformly "max words / grid cells" features from each cell. @@ -11042,20 +11095,20 @@ generate the number of words requested. - - - - 1 + + + + Bottom ROI ratio (0 = no change). - - 99 + + true - - 1 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + 1 @@ -11068,43 +11121,10 @@ generate the number of words requested. - - - - Number of columns of the grid used to extract uniformly "max words / grid cells" features from each cell. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Use depth image as mask when extracting features. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - + - Triangulate features without depth using stereo from motion (odometry). It would be ignored if depth as mask is checked and the feature detector used supports masking. + Filter floor from depth mask. 0 means disabled, negative means keeping pixels below the floor theshold instead. true @@ -11115,9 +11135,24 @@ generate the number of words requested. - - - + + + m + + + 2 + + + -99.000000000000000 + + + 99.000000000000000 + + + 0.050000000000000 + + + 0.000000000000000 @@ -11554,7 +11589,7 @@ When set to false, no new words are added to dictionary, so no more updates are - + @@ -11596,7 +11631,7 @@ When set to false, no new words are added to dictionary, so no more updates are - + Sqlite3 temp store, @@ -11610,7 +11645,7 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + @@ -11639,7 +11674,7 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + 10 @@ -11655,7 +11690,7 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + Sqlite3 cache size, @@ -11669,7 +11704,7 @@ see Sqlite3 doc 'PRAGMA cache_size'. - + Sqlite3 journal mode, @@ -11683,7 +11718,7 @@ see Sqlite3 doc 'PRAGMA journal_mode'. - + Sqlite3 synchronous, @@ -11697,7 +11732,7 @@ see Sqlite3 doc 'PRAGMA synchronous'. - + 2 @@ -11749,6 +11784,22 @@ see Sqlite3 doc 'PRAGMA synchronous'. + + + Depth image compression format (should be ".png" or ".rvl"). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Target database version for backward compatibility purpose. Only Major and minor versions are used and should be set (e.g., "0.19" vs "0.20" or "1.0" vs "2.0"). Patch version is ignored (e.g., "0.20.1" and "0.20.3" will generate a "0.20" database). @@ -11761,7 +11812,7 @@ see Sqlite3 doc 'PRAGMA synchronous'. - + major.minor @@ -13887,11 +13938,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - true - - + @@ -22557,10 +22604,10 @@ Lower the ratio -> higher the precision. - - + + - Maximum feature depth. + Use depth image as mask when extracting features. true @@ -22570,49 +22617,6 @@ Lower the ratio -> higher the precision. - - - - ROI ratios [left right top bottom] between 0 and 1. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - 999999 - - - - - - - Max features extracted from the images (0 means inf). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - @@ -22626,6 +22630,32 @@ Lower the ratio -> higher the precision. + + + + Maximum feature depth. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 99 + + + 1 + + + @@ -22639,6 +22669,79 @@ Lower the ratio -> higher the precision. + + + + Number of rows of the grid used to extract uniformly "max features / grid cells" features from each cell. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 0.000000000000000 + + + 0.100000000000000 + + + + + + + + + + + + + + + + + + + + + Number of columns of the grid used to extract uniformly "max features / grid cells" features from each cell. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 999999 + + + + + + + Max features extracted from the images (0 means inf). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -22729,6 +22832,16 @@ Lower the ratio -> higher the precision. + + + + Feature detector + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -22742,30 +22855,10 @@ Lower the ratio -> higher the precision. - - - - Feature detector - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - m - - - 0.000000000000000 - - - 0.100000000000000 - - - + + + 1 @@ -22779,9 +22872,9 @@ Lower the ratio -> higher the precision. - + - Number of rows of the grid used to extract uniformly "max features / grid cells" features from each cell. + ROI ratios [left right top bottom] between 0 and 1. true @@ -22791,10 +22884,10 @@ Lower the ratio -> higher the precision. - - + + - Number of columns of the grid used to extract uniformly "max features / grid cells" features from each cell. + Filter floor from depth mask. 0 means disabled, negative means keeping pixels below the floor theshold instead. true @@ -22804,36 +22897,19 @@ Lower the ratio -> higher the precision. - - + + + + m + - 1 + -99.000000000000000 - - 99 + + 0.050000000000000 - 1 - - - - - - - Use depth image as mask when extracting features. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - + 0.000000000000000 diff --git a/package.xml b/package.xml index cd2205a3..a9835453 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.21.9 + 0.21.10 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/tools/Export/main.cpp b/tools/Export/main.cpp index c156fd8b..13016367 100644 --- a/tools/Export/main.cpp +++ b/tools/Export/main.cpp @@ -49,34 +49,64 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#ifdef RTABMAP_LIBLAS +#include +#endif + using namespace rtabmap; void showUsage() { printf("\nUsage:\n" "rtabmap-export [options] database.db\n" + " New version: we now have to explicitly set what we want to export to give a more fine-grained control.\n" + " At least one of these options is required:\n" + " --cloud (old version used this by default)\n" + " --mesh\n" + " --poses\n" + " --poses_camera\n" + " --poses_scan\n" + " --poses_gps\n" + " --poses_gt\n" + " --gps\n" + " --images\n" + " --images_id\n" "Options:\n" " --output \"\" Output name (default: name of the database is used).\n" " --output_dir \"\" Output directory (default: same directory than the database).\n" " --ascii Export PLY in ascii 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" + " --las Export cloud in LAS instead of PLY (PDAL or libLAS dependency required).\n" + " --cloud Export assembled cloud.\n" + " --mesh Export assembled mesh.\n" + " --texture Texture the mesh. Used with --mesh option.\n" " --texture_size # Texture size 1024, 2048, 4096, 8192, 16384 (default 8192).\n" " --texture_count # Maximum textures generated (default 1). Ignored by --multiband option (adjust --multiband_contrib instead).\n" " --texture_range # Maximum camera range for texturing a polygon (default 0 meters: no limit).\n" " --texture_angle # Maximum camera angle for texturing a polygon (default 0 deg: no limit).\n" - " --texture_depth_error # Maximum depth error between reprojected mesh and depth image to texture a face (-1=disabled, 0=edge length is used, default=0).\n" - " --texture_roi_ratios \"# # # #\" Region of interest from images to texture or to color scans. Format is \"left right top bottom\" (e.g. \"0 0 0 0.1\" means 10%% of the image bottom not used).\n" + " --texture_depth_error # Maximum depth error between reprojected mesh and depth image to texture a face\n" + " (-1=disabled, 0=edge length is used, default=0).\n" + " --texture_roi_ratios \"# # # #\" Region of interest from images to texture or to color scans. Format\n" + " is \"left right top bottom\" (e.g. \"0 0 0 0.1\" means 10%%\n" + " of the image bottom not used).\n" " --texture_d2c Distance to camera policy.\n" - " --texture_blur # Motion blur threshold (default 0: disabled). Below this threshold, the image is considered blurred. 0 means disabled. 50 can be good default.\n" + " --texture_blur # Motion blur threshold (default 0: disabled). Below this threshold, the image is\n" + " considered blurred. 0 means disabled. 50 can be good default.\n" " --cam_projection Camera projection on assembled cloud and export node ID on each point (in PointSourceId field).\n" " --cam_projection_keep_all Keep not colored points from cameras (node ID will be 0 and color will be red).\n" " --cam_projection_decimation Decimate images before projecting the points.\n" - " --cam_projection_mask \"\" File path for a mask. Format should be 8-bits grayscale. The mask should cover all cameras in case multi-camera is used and have the same resolution.\n" + " --cam_projection_mask \"\" File path for a mask. Format should be 8-bits grayscale. The mask should\n" + " cover all cameras in case multi-camera is used and have the same resolution.\n" + " --opt # Optimization approach:\n" + " 0=Full Global Optimization (default)\n" + " 1=Iterative Global Optimization\n" + " 2=Use optimized poses already computed in the database instead\n" + " of re-computing them (fallback to default if optimized poses don't exist).\n" + " 3=No optimization, use odometry poses directly.\n" " --poses Export optimized poses of the robot frame (e.g., base_link).\n" " --poses_camera Export optimized poses of the camera frame (e.g., optical frame).\n" " --poses_scan Export optimized poses of the scan frame.\n" + " --poses_gt Export ground truth poses of the robot frame (e.g., base_link).\n" + " --poses_gps Export GPS poses of the GPS frame in local coordinates.\n" " --poses_format # Format used for exported poses (default is 11):\n" " 0=Raw 3x4 transformation matrix (r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz)\n" " 1=RGBD-SLAM (in motion capture coordinate frame)\n" @@ -85,6 +115,9 @@ void showUsage() " 4=g2o\n" " 10=RGBD-SLAM in ROS coordinate frame (stamp x y z qx qy qz qw)\n" " 11=RGBD-SLAM in ROS coordinate frame + ID (stamp x y z qx qy qz qw id)\n" + " --gps # Export GPS values of the GPS frame in world coordinates. Formats:\n" + " 0=Raw (stamp longitude latitude altitude error bearing)\n" + " 1=KML (Google Earth)\n" " --images Export images with stamp as file name.\n" " --images_id Export images with node id as file name.\n" " --ba Do global bundle adjustment before assembling the clouds.\n" @@ -92,13 +125,18 @@ void showUsage() " --gain_gray Do gain estimation compensation on gray channel only (default RGB channels).\n" " --no_blending Disable blending when texturing.\n" " --no_clean Disable cleaning colorless polygons.\n" - " --min_cluster # When meshing, filter clusters of polygons with size less than this threshold (default 200, -1 means keep only biggest contiguous surface).\n" + " --min_cluster # When meshing, filter clusters of polygons with size less than this\n" + " threshold (default 200, -1 means keep only biggest contiguous surface).\n" " --low_gain # Low brightness gain 0-100 (default 0).\n" " --high_gain # High brightness gain 0-100 (default 10).\n" - " --multiband Enable multiband texturing (AliceVision dependency required).\n" + " --multiband Enable multiband texturing (AliceVision dependency required). Used with --texture option.\n" " --multiband_downscale # Downscaling reduce the texture quality but speed up the computation time (default 2).\n" - " --multiband_contrib \"# # # # \" Number of contributions per frequency band for the multi-band blending, should be 4 values! (default \"1 5 10 0\").\n" - " --multiband_unwrap # Method to unwrap input mesh: 0=basic (default, >600k faces, fast), 1=ABF (<=300k faces, generate 1 atlas), 2=LSCM (<=600k faces, optimize space).\n" + " --multiband_contrib \"# # # # \" Number of contributions per frequency band for the\n" + " multi-band blending, should be 4 values! (default \"1 5 10 0\").\n" + " --multiband_unwrap # Method to unwrap input mesh:\n" + " 0=basic (default, >600k faces, fast)\n" + " 1=ABF (<=300k faces, generate 1 atlas)\n" + " 2=LSCM (<=600k faces, optimize space).\n" " --multiband_fillholes Fill Texture holes with plausible values.\n" " --multiband_padding # Texture edge padding size in pixel (0-100) (default 5).\n" " --multiband_scorethr # 0 to disable filtering based on threshold to relative best score (0.0-1.0). (default 0.1).\n" @@ -126,6 +164,8 @@ void showUsage() " --ymax # Maximum range on Y axis to keep nodes to export.\n" " --zmin # Minimum range on Z axis to keep nodes to export.\n" " --zmax # Maximum range on Z axis to keep nodes to export.\n" + " --density_radius # Filter poses in a fixed radius (m) to keep only one to be exported in the assembled cloud.\n" + " --density_angle # Filter poses up to angle (deg) in the --density_radius.\n" " --filter_ceiling # Filter points over a custom height (default 0 m, 0=disabled).\n" " --filter_floor # Filter points below a custom height (default 0 m, 0=disabled).\n" @@ -156,7 +196,8 @@ int main(int argc, char * argv[]) bool binary = true; bool las = false; - bool mesh = false; + bool exportCloud = false; + bool exportMesh = false; bool texture = false; bool ba = false; bool doGainCompensationRGB = true; @@ -206,12 +247,18 @@ int main(int argc, char * argv[]) bool exportPoses = false; bool exportPosesCamera = false; bool exportPosesScan = false; + bool exportPosesGt = false; + bool exportPosesGps = false; int exportPosesFormat = 11; + int exportGps = -1; bool exportImages = false; bool exportImagesId = false; + int optimizationApproach = 0; std::string outputName; std::string outputDir; cv::Vec3f min, max; + float densityRadius = 0.0f; + float densityAngle = 0.0f; float filter_ceiling = 0.0f; float filter_floor = 0.0f; for(int i=1; i1) + { + printf("Wrong GPS format (%d), should be 0 or 1\n", exportGps); + showUsage(); + } + } + else + { + showUsage(); + } + } + else if(std::strcmp(argv[i], "--opt") == 0) + { + ++i; + if(i3) + { + printf("Invalid --opt (%d)\n", optimizationApproach); + showUsage(); + } + } + else + { + showUsage(); + } + } else if(std::strcmp(argv[i], "--images") == 0) { exportImages = true; @@ -870,6 +963,32 @@ int main(int argc, char * argv[]) showUsage(); } } + else if(std::strcmp(argv[i], "--density_radius") == 0) + { + ++i; + if(i=0.0f); + } + else + { + showUsage(); + } + } + else if(std::strcmp(argv[i], "--density_angle") == 0) + { + ++i; + if(i=0.0f); + } + else + { + showUsage(); + } + } else if(std::strcmp(argv[i], "--filter_ceiling") == 0) { ++i; @@ -904,7 +1023,6 @@ int main(int argc, char * argv[]) showUsage(); } } - } if(decimation < 1) @@ -938,6 +1056,27 @@ int main(int argc, char * argv[]) } } + if(!(exportCloud || + exportMesh || + exportImages || + exportPoses || + exportPosesScan || + exportPosesCamera || + exportPosesGt || + exportPosesGps || + exportGps>=0 || + texture)) + { + printf("Launching the tool without any required option(s) is deprecated. We will add --cloud to keep compatibilty with old behavior.\n"); + exportCloud = true; + } + + if(texture && !exportMesh) + { + printf("To use --texture option, --mesh should be also enabled. Enabling --mesh.\n"); + exportMesh = true; + } + ParametersMap params = Parameters::parseArguments(argc, argv, false); std::string dbPath = argv[argc-1]; @@ -971,61 +1110,203 @@ int main(int argc, char * argv[]) UTimer timer; - printf("Loading database \"%s\"...\n", dbPath.c_str()); - // Get the global optimized map - Rtabmap rtabmap; + printf("Opening database \"%s\"...\n", dbPath.c_str()); uInsert(parameters, params); - rtabmap.init(parameters, dbPath); - printf("Loading database \"%s\"... done (%fs).\n", dbPath.c_str(), timer.ticks()); - - std::map nodes; - std::map optimizedPoses; - std::multimap links; - printf("Optimizing the map...\n"); - rtabmap.getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true); - printf("Optimizing the map... done (%fs, poses=%d).\n", timer.ticks(), (int)optimizedPoses.size()); - - if(optimizedPoses.empty()) + std::shared_ptr dbDriver(DBDriver::create(parameters)); + if(!dbDriver->openConnection(dbPath)) { - printf("The optimized graph is empty!? Aborting...\n"); + printf("Failed to open database \"%s\"!\n", dbPath.c_str()); return -1; } + printf("Opening database \"%s\"... done (%fs).\n", dbPath.c_str(), timer.ticks()); - if(min[0] != max[0] || min[1] != max[1] || min[2] != max[2]) + std::map optimizedPoses; + std::map odomPoses; + std::multimap links; + dbDriver->getAllOdomPoses(odomPoses, true); + dbDriver->getAllLinks(links, true, true); + if(optimizationApproach == 3 || !(exportCloud || exportMesh || exportPoses || exportPosesCamera || exportPosesScan)) { - cv::Vec3f minP,maxP; - graph::computeMinMax(optimizedPoses, minP, maxP); - printf("Filtering poses (range: x=%f<->%f, y=%f<->%f, z=%f<->%f, map size=%f x %f x %f)...\n", - min[0],max[0],min[1],max[1],min[2],max[2], - maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2]); - std::map posesFiltered; - for(std::map::const_iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) + // Just use odometry poses when exporting only images + optimizedPoses = odomPoses; + if(optimizationApproach == 3) { - bool ignore = false; - if(min[0] != max[0] && (iter->second.x() < min[0] || iter->second.x() > max[0])) + printf("Loaded %d odometry poses from database.\n", (int)odomPoses.size()); + } + } + else + { + if(optimizationApproach == 2) + { + printf("Loading optimized poses from database...\n"); + optimizedPoses = dbDriver->loadOptimizedPoses(); + if(optimizedPoses.empty()) { - ignore = true; + printf("The are no saved optimized poses in the database, we will do full global optimization instead.\n"); + optimizationApproach = 0; } - if(min[1] != max[1] && (iter->second.y() < min[1] || iter->second.y() > max[1])) + else { - ignore = true; - } - if(min[2] != max[2] && (iter->second.z() < min[2] || iter->second.z() > max[2])) - { - ignore = true; - } - if(!ignore) - { - posesFiltered.insert(*iter); + printf("Loading optimized poses from database... done (%d optimized poses loaded).\n", (int)optimizedPoses.size()); } } - graph::computeMinMax(posesFiltered, minP, maxP); - printf("Filtering poses... done! %d/%d remaining (new map size=%f x %f x %f).\n", (int)posesFiltered.size(), (int)optimizedPoses.size(), maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2]); - optimizedPoses = posesFiltered; + if(optimizationApproach <= 1) + { + std::string optimizationApproachStr = optimizationApproach==1?"Iterative global optimization":"Full global optimization"; + printf("Optimizing the map (%s)...\n", optimizationApproachStr.c_str()); + if(odomPoses.empty()) + { + printf("The are no odometry poses!? Aborting...\n"); + return -1; + } + std::shared_ptr optimizer(Optimizer::create(parameters)); + std::map posesOut; + std::multimap linksOut; + UASSERT(odomPoses.lower_bound(1) != odomPoses.end()); + + // Add landmarks if there are some + // Marker priors parameters + double markerPriorsLinearVariance = Parameters::defaultMarkerPriorsVarianceLinear(); + double markerPriorsAngularVariance = Parameters::defaultMarkerPriorsVarianceAngular(); + std::map markerPriors; + Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), markerPriorsLinearVariance); + UASSERT(markerPriorsLinearVariance>0.0f); + Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), markerPriorsAngularVariance); + UASSERT(markerPriorsAngularVariance>0.0f); + std::string markerPriorsStr; + if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr)) + { + std::list strList = uSplit(markerPriorsStr, '|'); + for(std::list::iterator iter=strList.begin(); iter!=strList.end(); ++iter) + { + std::string markerStr = *iter; + while(!markerStr.empty() && !uIsDigit(markerStr[0])) + { + markerStr.erase(markerStr.begin()); + } + if(!markerStr.empty()) + { + std::string idStr = uSplitNumChar(markerStr).front(); + int id = uStr2Int(idStr); + Transform prior = Transform::fromString(markerStr.substr(idStr.size())); + if(!prior.isNull() && id>0) + { + markerPriors.insert(std::make_pair(-id, prior)); + } + else + { + UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str()); + } + } + else if(!iter->empty()) + { + UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str()); + } + } + } + for(std::multimap::iterator iter=links.begin(); iter!=links.end(); ++iter) + { + if(iter->second.type() == Link::kLandmark) + { + UASSERT(iter->second.from() > 0 && iter->second.to() < 0); + int markerId = iter->second.to(); + if(odomPoses.find(iter->second.from()) != odomPoses.end() && odomPoses.find(markerId) == odomPoses.end()) + { + odomPoses.insert(std::make_pair(markerId, odomPoses.at(iter->second.from())*iter->second.transform())); + // add landmark priors if there are some + if(markerPriors.find(markerId) != markerPriors.end()) + { + cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1); + infMatrix(cv::Range(0,3), cv::Range(0,3)) /= markerPriorsLinearVariance; + infMatrix(cv::Range(3,6), cv::Range(3,6)) /= markerPriorsAngularVariance; + links.insert(std::make_pair(markerId, Link(markerId, markerId, Link::kPosePrior, markerPriors.at(markerId), infMatrix))); + printf("Added prior %d : %s (variance: lin=%f ang=%f)\n", markerId, markerPriors.at(markerId).prettyPrint().c_str(), + markerPriorsLinearVariance, markerPriorsAngularVariance); + } + } + } + } + + + optimizer->getConnectedGraph(odomPoses.lower_bound(1)->first, odomPoses, links, posesOut, linksOut); + if(optimizationApproach == 1) + { + std::list > intermediateGraphes; + optimizedPoses = optimizer->optimize(odomPoses.lower_bound(1)->first, posesOut, linksOut, &intermediateGraphes); + } + else + { + optimizedPoses = optimizer->optimize(odomPoses.lower_bound(1)->first, posesOut, linksOut); + } + printf("Optimizing the map (%s)... done (%fs, poses=%d, links=%d).\n", optimizationApproachStr.c_str(), timer.ticks(), (int)optimizedPoses.size(), (int)linksOut.size()); + } + if(optimizedPoses.empty()) { + printf("The optimized graph is empty!? Aborting...\n"); return -1; } + + if(min[0] != max[0] || min[1] != max[1] || min[2] != max[2]) + { + cv::Vec3f minP,maxP; + graph::computeMinMax(optimizedPoses, minP, maxP); + printf("Filtering poses (range: x=%.1f<->%.1f, y=%.1f<->%.1f, z=%.1f<->%.1f, map size=%.1f x %.1f x %.1f, map min/max: [%.1f, %.1f, %.1f] [%.1f, %.1f, %.1f])...\n", + min[0],max[0],min[1],max[1],min[2],max[2], + maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2], + minP[0],minP[1],minP[2],maxP[0],maxP[1],maxP[2]); + std::map posesFiltered; + for(std::map::const_iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) + { + bool ignore = false; + if(min[0] != max[0] && (iter->second.x() < min[0] || iter->second.x() > max[0])) + { + ignore = true; + } + if(min[1] != max[1] && (iter->second.y() < min[1] || iter->second.y() > max[1])) + { + ignore = true; + } + if(min[2] != max[2] && (iter->second.z() < min[2] || iter->second.z() > max[2])) + { + ignore = true; + } + if(!ignore) + { + posesFiltered.insert(*iter); + } + } + graph::computeMinMax(posesFiltered, minP, maxP); + printf("Filtering poses... done! %d/%d remaining.\n", (int)posesFiltered.size(), (int)optimizedPoses.size()); + optimizedPoses = posesFiltered; + if(optimizedPoses.empty()) + { + printf("All poses filtered! Exiting.\n"); + return -1; + } + } + + if(ba) + { + printf("Global bundle adjustment...\n"); + // TODO: these conversions could be simplified + UASSERT(optimizedPoses.lower_bound(1) != optimizedPoses.end()); + OptimizerG2O g2o(parameters); + std::list ids; + for(std::map::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter) + { + ids.push_back(iter->first); + } + std::list signatures; + dbDriver->loadSignatures(ids, signatures); + std::map nodes; + for(std::list::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter) + { + nodes.insert(std::make_pair((*iter)->id(), *(*iter))); + } + optimizedPoses = ((Optimizer*)&g2o)->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true); + printf("Global bundle adjustment... done (%fs).\n", timer.ticks()); + } } std::string outputDirectory = outputDir.empty()?UDirectory::getDir(dbPath):outputDir; @@ -1035,247 +1316,310 @@ int main(int argc, char * argv[]) } std::string baseName = outputName.empty()?uSplit(UFile::getName(dbPath), '.').front():outputName; - if(ba) - { - printf("Global bundle adjustment...\n"); - OptimizerG2O g2o(parameters); - optimizedPoses = ((Optimizer*)&g2o)->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true); - printf("Global bundle adjustment... done (%fs).\n", timer.ticks()); - } - // Construct the cloud - printf("Create and assemble the clouds...\n"); + if(exportCloud || exportMesh) + { + printf("Create and assemble the clouds...\n"); + } + else if(exportImages || exportImagesId) + { + printf("Export images...\n"); + } pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); pcl::PointCloud::Ptr assembledCloudI(new pcl::PointCloud); std::map robotPoses; std::vector > cameraPoses; std::map scanPoses; + std::map gtPoses; + std::map gpsPoses; + std::map gpsStamps; + GPS gpsOrigin; + std::map gpsValues; std::map cameraStamps; std::map > cameraModels; std::map cameraDepths; int imagesExported = 0; std::vector rawViewpointIndices; std::map rawViewpoints; - for(std::map::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter) + std::map densityPoses; + if(densityRadius && (exportCloud || exportMesh)) { - Signature node = nodes.find(iter->first)->second; + densityPoses = graph::radiusPosesFiltering(optimizedPoses, densityRadius, densityAngle*CV_PI/180.0f); + printf("Keeping %d/%d poses after density filtering (--density_radius = %f --density_angle = %f).\n", + (int)densityPoses.size(), + (int)optimizedPoses.size(), + densityRadius, + densityAngle); + } + int processedNodes = 0; + int lastPercent = 0; + for(std::map::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) + { + if(iter->first<0) + { + // landmark, just add to list of poses + robotPoses.insert(*iter); + cameraStamps.insert(std::make_pair(iter->first, 0)); + continue; + } + + Transform p, gt; + int m; + std::string l; + GPS gps; + std::vector v; + EnvSensors s; + int weight = -1; + double stamp = 0.0; + dbDriver->getNodeInfo(iter->first, p, m, weight, l, stamp, gt, v, gps, s); + + SensorData data; + bool loadImages = ((exportCloud || exportMesh) && (!cloudFromScan || texture || camProjection)) || exportImages; + bool loadScan = ((exportCloud || exportMesh) && cloudFromScan) || exportPosesScan; + if(loadImages || loadScan) + { + dbDriver->getNodeData( + iter->first, + data, + loadImages, + loadScan, + false, + false); + } // uncompress data - std::vector models = node.sensorData().cameraModels(); - std::vector stereoModels = node.sensorData().stereoCameraModels(); - cv::Mat rgb; - cv::Mat depth; - - pcl::IndicesPtr indices(new std::vector); - pcl::PointCloud::Ptr cloud; - pcl::PointCloud::Ptr cloudI; - if(node.getWeight() != -1) + std::vector models; + std::vector stereoModels; + if(loadImages || exportPosesCamera) { - if(cloudFromScan) + dbDriver->getCalibration(iter->first, models, stereoModels); + } + + if(exportCloud || exportMesh || exportImages) + { + bool densityFiltered = !densityPoses.empty() && densityPoses.find(iter->first) == densityPoses.end(); + cv::Mat rgb; + cv::Mat depth; + pcl::IndicesPtr indices(new std::vector); + pcl::PointCloud::Ptr cloud; + pcl::PointCloud::Ptr cloudI; + if(weight != -1) { - cv::Mat tmpDepth; - LaserScan scan; - node.sensorData().uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!node.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0, &scan); - if(scan.empty()) + if(!densityFiltered && cloudFromScan && (exportCloud || exportMesh)) { - printf("Node %d doesn't have scan data, empty cloud is created.\n", iter->first); - } - if(decimation>1 || minRange>0.0f || maxRange) - { - scan = util3d::commonFiltering(scan, decimation, minRange, maxRange); - } - if(scan.hasRGB()) - { - cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform()); - if(noiseRadius>0.0f && noiseMinNeighbors>0) + LaserScan scan; + data.uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!data.depthOrRightCompressed().empty()?&depth:0, &scan); + if(scan.empty()) { - indices = util3d::radiusFiltering(cloud, noiseRadius, noiseMinNeighbors); + printf("Node %d doesn't have scan data, empty cloud is created.\n", iter->first); + } + if(decimation>1 || minRange>0.0f || maxRange) + { + scan = util3d::commonFiltering(scan, decimation, minRange, maxRange); + } + if(scan.hasRGB()) + { + cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform()); + if(noiseRadius>0.0f && noiseMinNeighbors>0) + { + indices = util3d::radiusFiltering(cloud, noiseRadius, noiseMinNeighbors); + } + } + else + { + cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform()); + if(noiseRadius>0.0f && noiseMinNeighbors>0) + { + indices = util3d::radiusFiltering(cloudI, noiseRadius, noiseMinNeighbors); + } } } else { - cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform()); - if(noiseRadius>0.0f && noiseMinNeighbors>0) + data.uncompressData(&rgb, &depth); + if(!densityFiltered && (exportCloud || exportMesh)) { - indices = util3d::radiusFiltering(cloudI, noiseRadius, noiseMinNeighbors); + if(depth.empty()) + { + printf("Node %d doesn't have depth or stereo data, empty cloud is " + "created (if you want to create point cloud from scan, use --scan option).\n", iter->first); + } + cloud = util3d::cloudRGBFromSensorData( + data, + decimation, // image decimation before creating the clouds + maxRange, // maximum depth of the cloud + minRange, + indices.get()); + if(noiseRadius>0.0f && noiseMinNeighbors>0) + { + indices = util3d::radiusFiltering(cloud, indices, noiseRadius, noiseMinNeighbors); + } } } } - else - { - node.sensorData().uncompressData(&rgb, &depth); - if(depth.empty()) - { - printf("Node %d doesn't have depth or stereo data, empty cloud is " - "created (if you want to create point cloud from scan, use --scan option).\n", iter->first); - } - cloud = util3d::cloudRGBFromSensorData( - node.sensorData(), - decimation, // image decimation before creating the clouds - maxRange, // maximum depth of the cloud - minRange, - indices.get()); - if(noiseRadius>0.0f && noiseMinNeighbors>0) - { - indices = util3d::radiusFiltering(cloud, indices, noiseRadius, noiseMinNeighbors); - } - } - } - if(exportImages && !rgb.empty()) - { - std::string dirSuffix = (depth.type() != CV_16UC1 && depth.type() != CV_32FC1 && !depth.empty())?"left":"rgb"; - std::string dir = outputDirectory+"/"+baseName+"_"+dirSuffix; - if(!UDirectory::exists(dir)) { - UDirectory::makeDir(dir); - } - std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",node.getStamp()))+".jpg"; - cv::imwrite(outputPath, rgb); - ++imagesExported; - if(!depth.empty()) + if(exportImages && !rgb.empty()) { - std::string ext; - cv::Mat depthExported = depth; - if(depth.type() != CV_16UC1 && depth.type() != CV_32FC1) + std::string dirSuffix = (depth.type() != CV_16UC1 && depth.type() != CV_32FC1 && !depth.empty())?"left":"rgb"; + std::string dir = outputDirectory+"/"+baseName+"_"+dirSuffix; + if(!UDirectory::exists(dir)) { + UDirectory::makeDir(dir); + } + std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+".jpg"; + cv::imwrite(outputPath, rgb); + ++imagesExported; + if(!depth.empty()) { - ext = ".jpg"; - dir = outputDirectory+"/"+baseName+"_right"; + 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+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+ext; + cv::imwrite(outputPath, depthExported); + } + + // save calibration per image (calibration can change over time, e.g. camera has auto focus) + for(size_t i=0; ifirst):uFormat("%f", stamp)); + if(models.size() > 1) { + modelName += "_" + uNumber2Str((int)i); + } + model.setName(modelName); + std::string dir = outputDirectory+"/"+baseName+"_calib"; + if(!UDirectory::exists(dir)) { + UDirectory::makeDir(dir); + } + model.save(dir); + } + for(size_t i=0; ifirst):uFormat("%f", stamp)); + if(stereoModels.size() > 1) { + modelName += "_" + uNumber2Str((int)i); + } + model.setName(modelName, "left", "right"); + std::string dir = outputDirectory+"/"+baseName+"_calib"; + if(!UDirectory::exists(dir)) { + UDirectory::makeDir(dir); + } + model.save(dir); + } + } + + if(exportCloud || exportMesh) + { + if(voxelSize>0.0f) + { + 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); + } + 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); + + if(filter_ceiling != 0.0 || filter_floor != 0.0f) + { + if(cloud.get() && !cloud->empty()) + { + cloud = util3d::passThrough(cloud, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits::max()); + } + if(cloudI.get() && !cloudI->empty()) + { + cloudI = util3d::passThrough(cloudI, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits::max()); + } + } + + if(cloudFromScan) + { + Transform lidarViewpoint = iter->second * data.laserScanRaw().localTransform(); + rawViewpoints.insert(std::make_pair(iter->first, lidarViewpoint)); + } + else if(!models.empty() && !models[0].localTransform().isNull()) + { + Transform cameraViewpoint = iter->second * models[0].localTransform(); // take the first camera + rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint)); + } + else if(!stereoModels.empty() && !stereoModels[0].localTransform().isNull()) + { + Transform cameraViewpoint = iter->second * stereoModels[0].localTransform(); + rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint)); } else { - ext = ".png"; - dir = outputDirectory+"/"+baseName+"_depth"; - if(depth.type() == CV_32FC1) + rawViewpoints.insert(*iter); + } + + if(cloud.get() && !cloud->empty()) + { + if(assembledCloud->empty()) { - depthExported = rtabmap::util2d::cvtDepthFromFloat(depth); + *assembledCloud = *cloud; } + else + { + *assembledCloud += *cloud; + } + rawViewpointIndices.resize(assembledCloud->size(), iter->first); } - if(!UDirectory::exists(dir)) { - UDirectory::makeDir(dir); + else if(cloudI.get() && !cloudI->empty()) + { + if(assembledCloudI->empty()) + { + *assembledCloudI = *cloudI; + } + else + { + *assembledCloudI += *cloudI; + } + rawViewpointIndices.resize(assembledCloudI->size(), iter->first); } - - outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",node.getStamp()))+ext; - cv::imwrite(outputPath, depthExported); - } - - // save calibration per image (calibration can change over time, e.g. camera has auto focus) - for(size_t i=0; ifirst):uFormat("%f",node.getStamp())); - if(models.size() > 1) { - modelName += "_" + uNumber2Str((int)i); + if(texture && !depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1)) + { + cameraDepths.insert(std::make_pair(iter->first, depth)); } - model.setName(modelName); - std::string dir = outputDirectory+"/"+baseName+"_calib"; - if(!UDirectory::exists(dir)) { - UDirectory::makeDir(dir); - } - model.save(dir); } - for(size_t i=0; ifirst):uFormat("%f",node.getStamp())); - if(stereoModels.size() > 1) { - modelName += "_" + uNumber2Str((int)i); - } - model.setName(modelName, "left", "right"); - std::string dir = outputDirectory+"/"+baseName+"_calib"; - if(!UDirectory::exists(dir)) { - UDirectory::makeDir(dir); - } - model.save(dir); - } - } - - if(voxelSize>0.0f) - { - 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); - } - 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); - - if(filter_ceiling != 0.0 || filter_floor != 0.0f) - { - if(cloud.get() && !cloud->empty()) - { - cloud = util3d::passThrough(cloud, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits::max()); - } - if(cloudI.get() && !cloudI->empty()) - { - cloudI = util3d::passThrough(cloudI, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits::max()); - } - } - - if(cloudFromScan) - { - Transform lidarViewpoint = iter->second * node.sensorData().laserScanRaw().localTransform(); - rawViewpoints.insert(std::make_pair(iter->first, lidarViewpoint)); - } - else if(!node.sensorData().cameraModels().empty() && !node.sensorData().cameraModels()[0].localTransform().isNull()) - { - Transform cameraViewpoint = iter->second * node.sensorData().cameraModels()[0].localTransform(); // take the first camera - rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint)); - } - else if(!node.sensorData().stereoCameraModels().empty() && !node.sensorData().stereoCameraModels()[0].localTransform().isNull()) - { - Transform cameraViewpoint = iter->second * node.sensorData().stereoCameraModels()[0].localTransform(); - rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint)); - } - else - { - rawViewpoints.insert(*iter); - } - - if(cloud.get() && !cloud->empty()) - { - if(assembledCloud->empty()) - { - *assembledCloud = *cloud; - } - else - { - *assembledCloud += *cloud; - } - rawViewpointIndices.resize(assembledCloud->size(), iter->first); - } - else if(cloudI.get() && !cloudI->empty()) - { - if(assembledCloudI->empty()) - { - *assembledCloudI = *cloudI; - } - else - { - *assembledCloudI += *cloudI; - } - rawViewpointIndices.resize(assembledCloudI->size(), iter->first); } if(models.empty()) { - for(size_t i=0; ifirst, iter->second)); - cameraStamps.insert(std::make_pair(iter->first, node.getStamp())); - if(models.empty() && node.getWeight() == -1 && !cameraModels.empty()) + cameraStamps.insert(std::make_pair(iter->first, stamp)); + if(models.empty() && weight == -1 && !cameraModels.empty()) { // For intermediate nodes, use latest models models = cameraModels.rbegin()->second; } if(!models.empty()) { - if(!node.sensorData().imageCompressed().empty()) + if(!data.imageCompressed().empty()) { cameraModels.insert(std::make_pair(iter->first, models)); } @@ -1292,64 +1636,182 @@ int main(int argc, char * argv[]) } } } - if(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1)) + if(exportPosesScan && !data.laserScanCompressed().empty()) { - cameraDepths.insert(std::make_pair(iter->first, depth)); + scanPoses.insert(std::make_pair(iter->first, iter->second*data.laserScanCompressed().localTransform())); } - if(exportPosesScan && !node.sensorData().laserScanCompressed().empty()) + + if(exportPosesGps || exportGps>=0) { - scanPoses.insert(std::make_pair(iter->first, iter->second*node.sensorData().laserScanCompressed().localTransform())); + if(gps.stamp() > 0.0) + { + if(exportPosesGps) + { + cv::Point3f p(0.0f,0.0f,0.0f); + if(!gpsPoses.empty()) + { + GeodeticCoords coords = gps.toGeodeticCoords(); + p = coords.toENU_WGS84(gpsOrigin.toGeodeticCoords()); + } + else + { + gpsOrigin = gps; + } + Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0)); + gpsPoses.insert(std::make_pair(iter->first, pose)); + } + if(exportGps>=0) + { + gpsValues.insert(std::make_pair(iter->first, gps)); + } + gpsStamps.insert(std::make_pair(iter->first, gps.stamp())); + } + } + + if(exportPosesGt && !gt.isNull()) + { + gtPoses.insert(std::make_pair(iter->first, gt)); + } + + if(optimizedPoses.size() >= 500) + { + ++processedNodes; + int percent = processedNodes*100/(int)optimizedPoses.size(); + if(percent != lastPercent) + { + printf("Processed %d/%d (%d%%) nodes...\n", + processedNodes, + (int)optimizedPoses.size(), + percent); + lastPercent = percent; + } } } - printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size()); + if(exportCloud || exportMesh) + { + printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size()); + } - if(imagesExported>0) + if(exportImages || exportImagesId) + { printf("%d images exported!\n", imagesExported); + if(!(exportCloud || exportMesh || exportPoses || exportPosesCamera || exportPosesScan)) { + //images exported, early exit. + return 0; + } + } ConsoleProgessState progressState; + if(saveInDb) + { + driver = DBDriver::create(); + UASSERT(driver->openConnection(dbPath, false)); + Transform lastlocalizationPose; + driver->loadOptimizedPoses(&lastlocalizationPose); + //optimized poses have changed, reset 2d map + driver->save2DMap(cv::Mat(), 0, 0, 0); + driver->saveOptimizedPoses(robotPoses, lastlocalizationPose); + cv::Vec3f vmin, vmax; + graph::computeMinMax(robotPoses, vmin, vmax); + printf("Saved %d poses to database! (min=[%f,%f,%f] max=[%f,%f,%f])\n", + (int)robotPoses.size(), + vmin[0], vmin[1], vmin[2], + vmax[0], vmax[1], vmax[2]); + } + else + { + std::string posesExt = (exportPosesFormat==3?"toro":exportPosesFormat==4?"g2o":"txt"); + if(exportPoses) + { + std::string outputPath=outputDirectory+"/"+baseName+"_poses." + posesExt; + if(robotPoses.begin() != robotPoses.lower_bound(1) && !(exportPosesFormat == 4 || exportPosesFormat == 11)) + { + printf("Note that landmarks won't be exported because --poses_format is not 4 or 11 (currently using %d).\n", exportPosesFormat); + rtabmap::graph::exportPoses( + outputPath, + exportPosesFormat, + std::map(robotPoses.lower_bound(1), robotPoses.end()), + links, + std::map(cameraStamps.lower_bound(1), cameraStamps.end())); + } + else + { + rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, cameraStamps); + } + cv::Vec3f vmin, vmax; + graph::computeMinMax(robotPoses, vmin, vmax); + printf("%d poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n", + (int)robotPoses.size(), + outputPath.c_str(), + vmin[0], vmin[1], vmin[2], + vmax[0], vmax[1], vmax[2]); + } + if(exportPosesCamera) + { + for(size_t i=0; i(), cameraStamps); + cv::Vec3f vmin, vmax; + graph::computeMinMax(cameraPoses[i], vmin, vmax); + printf("%d camera poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n", + (int)cameraPoses[i].size(), + outputPath.c_str(), + vmin[0], vmin[1], vmin[2], + vmax[0], vmax[1], vmax[2]); + } + } + if(exportPosesScan) + { + std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt; + rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap(), cameraStamps); + cv::Vec3f min, max; + graph::computeMinMax(scanPoses, min, max); + printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n", + (int)scanPoses.size(), + outputPath.c_str(), + min[0], min[1], min[2], + max[0], max[1], max[2]); + } + if(exportPosesGps) + { + std::string outputPath=outputDirectory+"/"+baseName+"_gps_poses." + posesExt; + rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gpsPoses, std::multimap(), gpsStamps); + printf("%d GPS poses exported to \"%s\".\n", + (int)gpsPoses.size(), + outputPath.c_str()); + } + if(exportPosesGt) + { + std::string outputPath=outputDirectory+"/"+baseName+"_gt_poses." + posesExt; + rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap(), cameraStamps); + printf("%d scan poses exported to \"%s\".\n", + (int)gtPoses.size(), + outputPath.c_str()); + } + if(exportGps>=0) + { + std::string outputPath=outputDirectory+"/"+baseName+"_gps." + (exportGps==0?"txt":"kml"); + rtabmap::graph::exportGPS(outputPath, gpsValues); + printf("%d GPS values exported to \"%s\".\n", + (int)gpsValues.size(), + outputPath.c_str()); + } + } + + if(!(exportCloud || exportMesh)) + { + // poses exported, early exit + return 0; + } + if(!assembledCloud->empty() || !assembledCloudI->empty()) { - if(saveInDb) - { - driver = DBDriver::create(); - UASSERT(driver->openConnection(dbPath, false)); - Transform lastlocalizationPose; - driver->loadOptimizedPoses(&lastlocalizationPose); - //optimized poses have changed, reset 2d map - driver->save2DMap(cv::Mat(), 0, 0, 0); - driver->saveOptimizedPoses(optimizedPoses, lastlocalizationPose); - } - else - { - std::string posesExt = (exportPosesFormat==3?"toro":exportPosesFormat==4?"g2o":"txt"); - if(exportPoses) - { - std::string outputPath=outputDirectory+"/"+baseName+"_poses." + posesExt; - rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, cameraStamps); - printf("Poses exported to \"%s\".\n", outputPath.c_str()); - } - if(exportPosesCamera) - { - for(size_t i=0; i(), cameraStamps); - printf("Camera poses exported to \"%s\".\n", outputPath.c_str()); - } - } - if(exportPosesScan) - { - std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt; - rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap(), cameraStamps); - printf("Scan poses exported to \"%s\".\n", outputPath.c_str()); - } - } - if(proportionalRadiusFactor>0.0f && proportionalRadiusScale>=1.0f) { printf("Proportional radius filtering of the assembled cloud... (factor=%f scale=%f, %d points)\n", proportionalRadiusFactor, proportionalRadiusScale, !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size()); @@ -1379,72 +1841,72 @@ int main(int argc, char * argv[]) } pcl::PointCloud::Ptr rawAssembledCloud(new pcl::PointCloud); - if(!assembledCloud->empty()) - pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud); // used to adjust normal orientation - else if(!assembledCloudI->empty()) - pcl::copyPointCloud(*assembledCloudI, *rawAssembledCloud); // used to adjust normal orientation - - pcl::PointCloud::Ptr cloudWithoutNormals = rawAssembledCloud; - if(voxelSize>0.0f) { printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size()); - if(!assembledCloud->empty()) - { + if(!assembledCloud->empty()) { + pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud); // used to adjust normal orientation assembledCloud = util3d::voxelize(assembledCloud, voxelSize); - cloudWithoutNormals.reset(new pcl::PointCloud); - pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals); } - else if(!assembledCloudI->empty()) - { + else if(!assembledCloudI->empty()) { + pcl::copyPointCloud(*assembledCloudI, *rawAssembledCloud); // used to adjust normal orientation assembledCloudI = util3d::voxelize(assembledCloudI, voxelSize); - cloudWithoutNormals.reset(new pcl::PointCloud); - pcl::copyPointCloud(*assembledCloudI, *cloudWithoutNormals); } printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size()); } - printf("Computing normals of the assembled cloud... (k=20, %d points)\n", !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size()); - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloudWithoutNormals, 20, 0); - + printf("Computing normals of the assembled cloud... (k=20, %d points)\n", (int)assembledCloud->size()?(int)assembledCloud->size():(int)assembledCloudI->size()); pcl::PointCloud::Ptr cloudToExport(new pcl::PointCloud); pcl::PointCloud::Ptr cloudIToExport(new pcl::PointCloud); - if(!assembledCloud->empty()) - { + if(!assembledCloud->empty()) { + pcl::PointCloud::Ptr normals = util3d::computeNormals(assembledCloud, 20, 0); UASSERT(assembledCloud->size() == normals->size()); pcl::concatenateFields(*assembledCloud, *normals, *cloudToExport); - printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), (int)assembledCloud->size()); - assembledCloud->clear(); + } + else if(!assembledCloudI->empty()) { + pcl::PointCloud::Ptr normals = util3d::computeNormals(assembledCloudI, 20, 0); + UASSERT(assembledCloudI->size() == normals->size()); + pcl::concatenateFields(*assembledCloudI, *normals, *cloudIToExport); + } + printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size()); + assembledCloud->clear(); + assembledCloudI->clear(); - // adjust with point of views - printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (int)cloudToExport->size()); + // adjust with point of views + printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size()); + if(!rawAssembledCloud->empty()) { + if(!cloudToExport->empty()) { + util3d::adjustNormalsToViewPoints( + rawViewpoints, + rawAssembledCloud, + rawViewpointIndices, + cloudToExport, + groundNormalsUp); + } + else if(!cloudIToExport->empty()) { + util3d::adjustNormalsToViewPoints( + rawViewpoints, + rawAssembledCloud, + rawViewpointIndices, + cloudIToExport, + groundNormalsUp); + } + } + else if(!cloudToExport->empty()) { util3d::adjustNormalsToViewPoints( rawViewpoints, - rawAssembledCloud, rawViewpointIndices, cloudToExport, groundNormalsUp); - printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), (int)cloudToExport->size()); } - else if(!assembledCloudI->empty()) - { - UASSERT(assembledCloudI->size() == normals->size()); - pcl::concatenateFields(*assembledCloudI, *normals, *cloudIToExport); - printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), (int)assembledCloudI->size()); - assembledCloudI->clear(); - - // adjust with point of views - printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (int)cloudIToExport->size()); + else if(!cloudIToExport->empty()) { util3d::adjustNormalsToViewPoints( rawViewpoints, - rawAssembledCloud, rawViewpointIndices, cloudIToExport, groundNormalsUp); - printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), (int)cloudIToExport->size()); } - cloudWithoutNormals->clear(); - rawAssembledCloud->clear(); + printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size()); if(randomSamples>0) { @@ -1464,7 +1926,7 @@ int main(int argc, char * argv[]) std::vector pointToCamIntensity; if(camProjection && !robotPoses.empty()) { - printf("Camera projection...\n"); + printf("Camera projection... (cameras=%d)\n", (int)cameraModels.size()); std::map > cameraModelsProj; if(cameraProjDecimation>1) { @@ -1575,13 +2037,16 @@ int main(int argc, char * argv[]) assembledCloudValidPoints->resize(pointToCamId.size()); int imagesDone = 1; - for(std::map::iterator iter=robotPoses.begin(); iter!=robotPoses.end(); ++iter) + int coloredPoints = 0; + for(std::map::iterator iter=robotPoses.lower_bound(1); iter!=robotPoses.end(); ++iter) { int nodeID = iter->first; cv::Mat image; - if(uContains(nodes,nodeID) && !nodes.at(nodeID).sensorData().imageCompressed().empty()) + SensorData data; + dbDriver->getNodeData(nodeID, data, true, false, false, false); + if(!data.imageCompressed().empty()) { - nodes.at(nodeID).sensorData().uncompressDataConst(&image, 0); + data.uncompressDataConst(&image, 0); } if(!image.empty()) { @@ -1589,30 +2054,33 @@ int main(int argc, char * argv[]) { image = util2d::decimate(image, cameraProjDecimation); } - UASSERT(cameraModelsProj.find(nodeID) != cameraModelsProj.end()); - int modelsSize = cameraModelsProj.at(nodeID).size(); - for(size_t i=0; i=0) { - int cameraIndex = pointToPixel[i].first.second; - if(nodeID == pointToPixel[i].first.first && cameraIndex>=0) + pcl::PointXYZRGBNormal pt; + float intensity = 0; + if(!cloudToExport->empty()) { - pcl::PointXYZRGBNormal pt; - float intensity = 0; - 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; - intensity = cloudIToExport->at(i).intensity; - } + 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; + intensity = cloudIToExport->at(i).intensity; + } + if(!image.empty()) + { int subImageWidth = image.cols / modelsSize; cv::Mat subImage = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth)); @@ -1633,19 +2101,28 @@ int main(int argc, char * argv[]) UASSERT(subImage.type()==CV_8UC1); pt.r = pt.g = pt.b = subImage.at(pointToPixel[i].second.y * subImage.rows, pointToPixel[i].second.x * subImage.cols); } - - int exportedId = nodeID; - pointToCamId[i] = exportedId; - if(!pointToCamIntensity.empty()) - { - pointToCamIntensity[i] = intensity; - } - assembledCloudValidPoints->at(i) = pt; + ++coloredPoints; } + + int exportedId = nodeID; + pointToCamId[i] = exportedId; + if(!pointToCamIntensity.empty()) + { + pointToCamIntensity[i] = intensity; + } + assembledCloudValidPoints->at(i) = pt; } } - UINFO("Processed %d/%d images", imagesDone++, (int)robotPoses.size()); + if(!image.empty()) + { + printf("Processed %d/%d images\n", imagesDone++, (int)robotPoses.size()); + } + else + { + printf("Node %d doesn't have image! (%d/%d)\n", iter->first, imagesDone++, (int)robotPoses.size()); + } } + printf("Colored %d/%d points.\n", coloredPoints, (int)assembledCloudValidPoints->size()); pcl::IndicesPtr validIndices(new std::vector(pointToPixel.size())); size_t oi = 0; @@ -1689,40 +2166,51 @@ int main(int argc, char * argv[]) validIndices->at(oi++) = i; } } - if(oi != validIndices->size()) { validIndices->resize(oi); assembledCloudValidPoints = util3d::extractIndices(assembledCloudValidPoints, validIndices, false, false); std::vector pointToCamIdTmp(validIndices->size()); - std::vector pointToCamIntensityTmp(validIndices->size()); + std::vector pointToCamIntensityTmp(pointToCamIntensity.empty()?0:validIndices->size()); for(size_t i=0; isize(); ++i) { - pointToCamIdTmp[i] = pointToCamId[validIndices->at(i)]; - pointToCamIntensityTmp[i] = pointToCamIntensity[validIndices->at(i)]; + size_t index = validIndices->at(i); + UASSERT(index < pointToCamId.size()); + pointToCamIdTmp[i] = pointToCamId[index]; + if(!pointToCamIntensity.empty()) + { + UASSERT(index < pointToCamIntensity.size()); + pointToCamIntensityTmp[i] = pointToCamIntensity[index]; + } } pointToCamId = pointToCamIdTmp; pointToCamIntensity = pointToCamIntensityTmp; pointToCamIdTmp.clear(); pointToCamIntensityTmp.clear(); } - cloudToExport = assembledCloudValidPoints; cloudIToExport->clear(); printf("Camera projection... done! (%fs)\n", timer.ticks()); } - if(!(mesh || texture)) + if(exportCloud) { if(saveInDb) { - 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"); + if(exportMesh) + { + printf("Option --save_in_db is set and both --cloud and --mesh are set, we will only save the mesh in the database.\n"); + } + else + { + printf("Saving cloud 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 cloud in db... done!\n"); + } } else { @@ -1772,7 +2260,7 @@ int main(int argc, char * argv[]) } // Meshing... - if(mesh || texture) + if(exportMesh) { if(!cloudIToExport->empty()) { @@ -1853,11 +2341,12 @@ int main(int argc, char * argv[]) if(laplacianThr>0) { printf("Filtering %ld images from texturing...\n", robotPoses.size()); - for(std::map::iterator iter=robotPoses.begin(); iter!=robotPoses.end(); ++iter) + for(std::map::iterator iter=robotPoses.lower_bound(1); iter!=robotPoses.end(); ++iter) { - UASSERT(nodes.find(iter->first) != nodes.end()); + SensorData data; + dbDriver->getNodeData(iter->first, data, true, false, false, false); cv::Mat img; - nodes.find(iter->first)->second.sensorData().uncompressDataConst(&img, 0); + data.uncompressDataConst(&img, 0); if(!img.empty()) { cv::Mat imgLaplacian; @@ -1925,8 +2414,8 @@ int main(int argc, char * argv[]) *textureMesh, std::map(), std::map >(), - rtabmap.getMemory(), 0, + dbDriver.get(), textureSize, multiband?1:textureCount, // to get contrast values based on all images in multiband mode vertexToPixels, @@ -1975,8 +2464,8 @@ int main(int argc, char * argv[]) vertexToPixels, std::map(), std::map >(), - rtabmap.getMemory(), 0, + dbDriver.get(), textureSize, multibandDownScale, multibandNbContrib, @@ -1999,53 +2488,67 @@ int main(int argc, char * argv[]) printf("MultiBand texturing...failed! (%fs)\n", timer.ticks()); } } - - // TextureMesh OBJ - bool success = false; - UASSERT(!textures.empty()); - for(size_t i=0; itex_materials.size(); ++i) + else { - textureMesh->tex_materials[i].tex_file += ".jpg"; - printf("Saving texture to %s.\n", textureMesh->tex_materials[i].tex_file.c_str()); - UASSERT(textures.cols % textures.rows == 0); - success = cv::imwrite(outputDirectory+"/"+textureMesh->tex_materials[i].tex_file, cv::Mat(textures, cv::Range::all(), cv::Range(textures.rows*i, textures.rows*(i+1)))); - if(!success) + // TextureMesh OBJ + bool success = false; + UASSERT(!textures.empty()); + for(size_t i=0; itex_materials.size(); ++i) { - UERROR("Failed saving %s!", textureMesh->tex_materials[i].tex_file.c_str()); - } - else - { - printf("Saved %s.\n", textureMesh->tex_materials[i].tex_file.c_str()); - } - } - if(success) - { - std::string outputPath=outputDirectory+"/"+baseName+"_mesh.obj"; - printf("Saving obj (%d vertices) to %s.\n", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, outputPath.c_str()); -#if PCL_VERSION_COMPARE(>=, 1, 13, 0) - textureMesh->tex_coord_indices = std::vector>(); - auto nr_meshes = static_cast(textureMesh->tex_polygons.size()); - unsigned f_idx = 0; - for (unsigned m = 0; m < nr_meshes; m++) { - std::vector ci = textureMesh->tex_polygons[m]; - for(std::size_t i = 0; i < ci.size(); i++) { - for (std::size_t j = 0; j < ci[i].vertices.size(); j++) { - ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j; + // Texture file name format is texture[#], replace "texture" by base filename + std::list values = uSplitNumChar(textureMesh->tex_materials[i].tex_file); + textureMesh->tex_materials[i].tex_file.clear(); + for(const auto & v: values) + { + if(v.compare("texture") == 0){ + textureMesh->tex_materials[i].tex_file.append(baseName+"_mesh"); + } + else { + textureMesh->tex_materials[i].tex_file.append(v); } } - textureMesh->tex_coord_indices.push_back(ci); - f_idx += static_cast(textureMesh->tex_polygons[m].size()); + textureMesh->tex_materials[i].tex_file += ".jpg"; + printf("Saving texture to %s.\n", textureMesh->tex_materials[i].tex_file.c_str()); + UASSERT(textures.cols % textures.rows == 0); + success = cv::imwrite(outputDirectory+"/"+textureMesh->tex_materials[i].tex_file, cv::Mat(textures, cv::Range::all(), cv::Range(textures.rows*i, textures.rows*(i+1)))); + if(!success) + { + UERROR("Failed saving %s!", textureMesh->tex_materials[i].tex_file.c_str()); + } + else + { + printf("Saved %s.\n", textureMesh->tex_materials[i].tex_file.c_str()); + } } -#endif - success = pcl::io::saveOBJFile(outputPath, *textureMesh) == 0; - if(success) { - printf("Saved obj to %s!\n", outputPath.c_str()); - } - else - { - UERROR("Failed saving obj to %s!", outputPath.c_str()); + std::string outputPath=outputDirectory+"/"+baseName+"_mesh.obj"; + printf("Saving obj (%d vertices) to %s.\n", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, outputPath.c_str()); + #if PCL_VERSION_COMPARE(>=, 1, 13, 0) + textureMesh->tex_coord_indices = std::vector>(); + auto nr_meshes = static_cast(textureMesh->tex_polygons.size()); + unsigned f_idx = 0; + for (unsigned m = 0; m < nr_meshes; m++) { + std::vector ci = textureMesh->tex_polygons[m]; + for(std::size_t i = 0; i < ci.size(); i++) { + for (std::size_t j = 0; j < ci[i].vertices.size(); j++) { + ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j; + } + } + textureMesh->tex_coord_indices.push_back(ci); + f_idx += static_cast(textureMesh->tex_polygons[m].size()); + } + #endif + success = pcl::io::saveOBJFile(outputPath, *textureMesh) == 0; + + if(success) + { + printf("Saved obj to %s!\n", outputPath.c_str()); + } + else + { + UERROR("Failed saving obj to %s!", outputPath.c_str()); + } } } }