GridMap integration

.github/workflows/cmake-ros.yml

@@ -21,6 +21,7 @@ jobs:
     name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
     runs-on: ${{ matrix.os }}
     strategy:
+      fail-fast: false
       matrix:
         ros_distribution: [ noetic, humble, iron, rolling]
         include:
@@ -38,7 +39,7 @@ jobs:
       with:
         required-ros-distributions: ${{ matrix.ros_distribution }}
 
-    - uses: actions/checkout@v2
+    - uses: actions/checkout@v4
 
     - name: Install dependencies
       run: |

.github/workflows/cmake.yml

@@ -16,6 +16,7 @@ jobs:
     name: ${{ matrix.os }}
     runs-on: ${{ matrix.os }}
     strategy:
+      fail-fast: false
       matrix:
         os: [ubuntu-22.04, ubuntu-20.04]
 
@@ -26,7 +27,7 @@ jobs:
         sudo apt-get update
         sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev
 
-    - uses: actions/checkout@v2
+    - uses: actions/checkout@v4
 
     - name: Configure CMake
       run: |

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 2)
+SET(RTABMAP_PATCH_VERSION 3)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
 
@@ -203,7 +203,8 @@ option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
 option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
 option(WITH_DEPTHAI       "Include depthai-core support"         OFF)
-option(WITH_OCTOMAP       "Include Octomap support"              ON)
+option(WITH_OCTOMAP       "Include OctoMap support"              ON)
+option(WITH_GRIDMAP       "Include GridMap support"              OFF)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)
 option(WITH_OPENCHISEL    "Include open_chisel support"          OFF)
 option(WITH_ALICE_VISION  "Include AliceVision support"          OFF)
@@ -660,6 +661,13 @@ IF(WITH_OCTOMAP)
     ENDIF(octomap_FOUND)
 ENDIF(WITH_OCTOMAP)
 
+IF(WITH_GRIDMAP)
+    FIND_PACKAGE(grid_map_core QUIET)
+    IF(grid_map_core_FOUND)
+       MESSAGE(STATUS "Found grid_map_core ${grid_map_core_VERSION}: ${grid_map_core_INCLUDE_DIRS}")
+    ENDIF(grid_map_core_FOUND)
+ENDIF(WITH_GRIDMAP)
+
 IF(WITH_CPUTSDF)
     FIND_PACKAGE(CPUTSDF QUIET)
     IF(CPUTSDF_FOUND)
@@ -1002,6 +1010,12 @@ IF(NOT octomap_FOUND)
 ELSE()
    SET(CONF_WITH_OCTOMAP 1)
 ENDIF()
+IF(NOT grid_map_core_FOUND)
+   SET(GRIDMAP "//")
+   SET(CONF_WITH_GRIDMAP 0)
+ELSE()
+   SET(CONF_WITH_GRIDMAP 1)
+ENDIF()
 IF(NOT CPUTSDF_FOUND)
    SET(CPUTSDF "//")
 ENDIF()
@@ -1474,11 +1488,19 @@ ENDIF()
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Reconstruction Approaches:")
 IF(octomap_FOUND)
-MESSAGE(STATUS "  With OCTOMAP ${octomap_VERSION}        = YES (License: BSD)")
+MESSAGE(STATUS "  With OctoMap ${octomap_VERSION}        = YES (License: BSD)")
+ELSEIF(NOT WITH_OCTOMAP)
+MESSAGE(STATUS "  With OctoMap              = NO (WITH_OCTOMAP=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OctoMap              = NO (octomap not found)")
+ENDIF()
+
+IF(grid_map_core_FOUND)
+MESSAGE(STATUS "  With GridMap ${grid_map_core_VERSION}        = YES (License: BSD)")
 ELSEIF(NOT WITH_OCTOMAP)
-MESSAGE(STATUS "  With OCTOMAP              = NO (WITH_OCTOMAP=OFF)")
+MESSAGE(STATUS "  With GridMap              = NO (WITH_GRIDMAP=OFF)")
 ELSE()
-MESSAGE(STATUS "  With OCTOMAP              = NO (octomap not found)")
+MESSAGE(STATUS "  With GridMap              = NO (octomap not found)")
 ENDIF()
 
 IF(CPUTSDF_FOUND)

RTABMapConfig.cmake.in

@@ -50,6 +50,10 @@ IF(@CONF_WITH_OCTOMAP@)
   find_dependency(octomap)
 ENDIF()
 
+IF(@CONF_WITH_GRIDMAP@)
+  find_dependency(grid_map_core)
+ENDIF()
+
 IF(@CONF_WITH_PYTHON@)
   find_dependency(Python3 COMPONENTS Interpreter Development NumPy)
 ENDIF()

Version.h.in

@@ -69,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @MYNTEYE@#define RTABMAP_MYNTEYE
 @DEPTHAI@#define RTABMAP_DEPTHAI
 @OCTOMAP@#define RTABMAP_OCTOMAP
+@GRIDMAP@#define RTABMAP_GRIDMAP
 @CPUTSDF@#define RTABMAP_CPUTSDF
 @ALICE_VISION@#define RTABMAP_ALICE_VISION
 @OPENCHISEL@#define RTABMAP_OPENCHISEL

corelib/include/rtabmap/core/GlobalMap.h

@@ -0,0 +1,102 @@
+/*
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef SRC_MAP_H_
+#define SRC_MAP_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/LocalGrid.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Transform.h>
+#include <list>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT GlobalMap
+{
+public:
+	inline static float logodds(double probability)
+	{
+		return (float) log(probability/(1-probability));
+	}
+
+	inline static double probability(double logodds)
+	{
+		return 1. - ( 1. / (1. + exp(logodds)));
+	}
+
+public:
+	virtual ~GlobalMap();
+
+	bool update(const std::map<int, Transform> & poses); // return true if map has changed
+
+	virtual void clear();
+
+	float getCellSize() const {return cellSize_;}
+	float getUpdateError() const {return updateError_;}
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+
+	void getGridMin(double & x, double & y) const {x=minValues_[0];y=minValues_[1];}
+	void getGridMax(double & x, double & y) const {x=maxValues_[0];y=maxValues_[1];}
+	void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
+	void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
+
+	virtual unsigned long getMemoryUsed() const;
+
+protected:
+	GlobalMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
+
+	virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses) = 0;
+
+	const std::map<int, LocalGrid> & cache() const {return cache_->localGrids();}
+
+	const std::map<int, Transform> & assembledNodes() const {return addedNodes_;}
+	bool isNodeAssembled(int id) {return addedNodes_.find(id) != addedNodes_.end();}
+	void addAssembledNode(int id, const Transform & pose);
+
+protected:
+	float cellSize_;
+	float updateError_;
+
+	float occupancyThr_;
+	float logOddsHit_;
+	float logOddsMiss_;
+	float logOddsClampingMin_;
+	float logOddsClampingMax_;
+
+	double minValues_[3];
+	double maxValues_[3];
+
+private:
+	const LocalGridCache * cache_;
+	std::map<int, Transform> addedNodes_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* SRC_MAP_H_ */

corelib/include/rtabmap/core/LocalGrid.h

@@ -0,0 +1,90 @@
+/*
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef SRC_LOCALGRID_H_
+#define SRC_LOCALGRID_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <opencv2/core.hpp>
+#include <map>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT LocalGrid
+{
+public:
+	LocalGrid(const cv::Mat & ground,
+		 const cv::Mat & obstacles,
+		 const cv::Mat & empty,
+		 float cellSize,
+		 const cv::Point3f & viewPoint = cv::Point3f(0,0,0));
+	virtual ~LocalGrid() {}
+	bool is3D() const;
+public:
+	cv::Mat groundCells;
+	cv::Mat obstacleCells;
+	cv::Mat emptyCells;
+	float cellSize;
+	cv::Point3f viewPoint;
+};
+
+class RTABMAP_CORE_EXPORT LocalGridCache
+{
+public:
+	LocalGridCache() {}
+	virtual ~LocalGridCache() {}
+
+	void add(int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty,
+			float cellSize,
+			const cv::Point3f & viewPoint = cv::Point3f(0,0,0));
+
+	void add(int nodeId, const LocalGrid & localGrid);
+
+	bool shareTo(int nodeId, LocalGridCache & anotherCache) const;
+
+	unsigned long getMemoryUsed() const;
+	void clear(bool temporaryOnly = false);
+
+	size_t size() const {return localGrids_.size();}
+	bool empty() const {return localGrids_.empty();}
+	const std::map<int, LocalGrid> & localGrids() const {return localGrids_;}
+
+	std::map<int, LocalGrid>::const_iterator find(int nodeId) const {return localGrids_.find(nodeId);}
+	std::map<int, LocalGrid>::const_iterator begin() const {return localGrids_.begin();}
+	std::map<int, LocalGrid>::const_iterator end() const {return localGrids_.end();}
+
+private:
+	std::map<int, LocalGrid> localGrids_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* SRC_LOCALGRID_H_ */