Merge branch 'master' of github.com:introlab/rtabmap into humble-devel

bump 0.22.1
ImageView: Depth color map in base frame (#1553 )
2026-09-09 04:50:20 +08:00 · 2025-07-12 10:38:39 -07:00 · 2025-07-12 10:11:55 -07:00 · 2025-07-12 10:07:47 -07:00 · 2025-07-06 23:08:47 +00:00 · 2025-07-06 15:51:49 -07:00
34 changed files with 1196 additions and 255 deletions
@@ -0,0 +1,24 @@
+FROM introlab3it/rtabmap:android-noble-deps
+
+# remove ubuntu user
+RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
+
+RUN apt-get update && apt-get install -y sudo && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN chmod +x /opt/android-sdk/tools/android
+
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+
+
@@ -1,9 +1,21 @@
 {
-    "image": "introlab3it/rtabmap:android-deps",
+    "build": {
+      "dockerfile": "Dockerfile"
+    },
    "customizations": {
        "vscode": {
          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
        }
      },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
+    "workspaceFolder": "/home/vscode/rtabmap",
+    "postStartCommand": "./.devcontainer/android/init.sh",
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
    "runArgs": ["--privileged", "--network=host"]
 }
+
+
+  
@@ -0,0 +1,17 @@
+#!/bin/bash
+set -e
+
+echo "Running post-start initialization..."
+
+# copy required jars
+cp /opt/android/lib/*.jar app/android/libs/.
+
+mkdir -p build_android/arm64-v8a
+
+# resource tool
+cd build_android
+cmake -DANDROID_PREBUILD=ON ..
+make
+
+echo -e "\nTo build the APK, do (adjust API number):"
+echo -e '\nexport ANDROID_API=30 && cd build_android/arm64-v8a && cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_API -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DOpenCV_DIR=/opt/android/arm64-v8a/sdk/native/jni ../..\nmake -j6\n'
@@ -0,0 +1,77 @@
+
+FROM ubuntu:24.04
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+# Install ROS2
+RUN apt update && \
+    apt install software-properties-common -y && \
+    add-apt-repository universe && \
+    apt update && \
+    apt install curl -y && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+# Install build dependencies
+RUN apt-get update && \
+    apt upgrade -y && \
+    apt-get install -y \
+      git \
+      wget \
+      libtbb-dev \
+      libproj-dev \
+      libpcl-dev \
+      liboctomap-dev \
+      libfreenect-dev \
+      ros-rolling-ros-base \
+      ros-dev-tools \
+      ros-rolling-cv-bridge \
+      ros-rolling-image-geometry \
+      ros-rolling-laser-geometry \
+      ros-rolling-pcl-conversions \
+      ros-rolling-rviz-common \
+      ros-rolling-rviz-rendering \
+      ros-rolling-rviz-default-plugins \
+      ros-rolling-pcl-ros \
+      ros-rolling-imu-filter-madgwick \
+      ros-rolling-image-transport \
+      ros-rolling-octomap-msgs \
+      ros-rolling-libg2o \
+      ros-rolling-gtsam \
+      ros-rolling-libpointmatcher \
+      ros-rolling-qt-gui-cpp \
+      ros-rolling-diagnostic-updater && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+WORKDIR /root/
+
+RUN rm /bin/sh && ln -s /bin/bash /bin/sh
+
+RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/rolling/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh
+RUN chmod +x /ros_entrypoint.sh
+ENTRYPOINT [ "/ros_entrypoint.sh" ]
+
+# ros2 seems not sourcing by default its multi-arch folders
+ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/rolling/lib/x86_64-linux-gnu
+
+# For devcontainer
+# remove ubuntu user
+RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
+
+RUN apt-get update && apt-get install -y sudo && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+
@@ -0,0 +1,17 @@
+{
+    "build": {
+      "dockerfile": "Dockerfile"
+    },
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
+    "workspaceFolder": "/home/vscode/rtabmap",
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
+    "runArgs": ["--privileged", "--network=host"]
+}
@@ -3,7 +3,7 @@ name: CMake-ROS
 on:
  push:
    branches: 
-      - kilted-devel
+      - humble-devel
  pull_request:
    branches: 
      - '**'
@@ -23,10 +23,10 @@ jobs:
    strategy:
      fail-fast: false
      matrix:
-        ros_distribution: [ kilted ]
+        ros_distribution: [ humble ]
        include:
-        - ros_distribution: 'kilted'
-          os: ubuntu-24.04
+        - ros_distribution: 'humble'
+          os: ubuntu-22.04

    steps:   
    - name: Setup ROS2
@@ -19,19 +19,26 @@ jobs:
      fail-fast: false
      matrix:
        os: [ubuntu-24.04, ubuntu-22.04]
+        include:
+        - os: ubuntu-22.04
+          extra_deps: "libunwind-dev libceres-dev"
+          extra_cmake_def: ""
+        - os: ubuntu-24.04
+          extra_deps: "libg2o-dev libceres-dev"
+          extra_cmake_def: "-DWITH_CERES=ON"

    steps:    
    - name: Install dependencies
      run: |
        DEBIAN_FRONTEND=noninteractive
        sudo apt-get update
-        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }}

    - uses: actions/checkout@v4

    - name: Configure CMake
      run: |
-        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}

    - name: Build
      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
@@ -150,7 +150,7 @@ jobs:
            API_VERSION=23
          docker_platforms: |
            linux/amd64
-          docker_path: 'bionic/android/rtabmap_apiXX'
+          docker_path: 'noble/android/rtabmap_apiXX'
        - docker_tag: android24
          docker_tags: |
            introlab3it/rtabmap:android24
@@ -158,7 +158,7 @@ jobs:
            API_VERSION=24
          docker_platforms: |
            linux/amd64
-          docker_path: 'bionic/android/rtabmap_apiXX'
+          docker_path: 'noble/android/rtabmap_apiXX'
        - docker_tag: android26
          docker_tags: |
            introlab3it/rtabmap:android26
@@ -166,7 +166,7 @@ jobs:
            API_VERSION=26
          docker_platforms: |
            linux/amd64
-          docker_path: 'bionic/android/rtabmap_apiXX'
+          docker_path: 'noble/android/rtabmap_apiXX'
        - docker_tag: android30
          docker_tags: |
            introlab3it/rtabmap:android30
@@ -174,7 +174,7 @@ jobs:
            API_VERSION=30
          docker_platforms: |
            linux/amd64
-          docker_path: 'bionic/android/rtabmap_apiXX'
+          docker_path: 'noble/android/rtabmap_apiXX'
    
    steps:
      -
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 22)
-SET(RTABMAP_PATCH_VERSION 0)
+SET(RTABMAP_PATCH_VERSION 1)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -501,21 +501,30 @@ IF(WITH_G2O)
         get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES)
         MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}")
         FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
-         PATHS ${G2O_INCLUDES}
-         NO_DEFAULT_PATH)
-       FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
-       STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
-       IF(${matchres} EQUAL -1)
-           MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-           SET(G2O_CPP11 2)
-       ELSE()
-           MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-           SET(G2O_CPP11 1)
-       ENDIF()
+            PATHS ${G2O_INCLUDES}
+            NO_DEFAULT_PATH)
+         FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+         STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+         IF(${matchres} EQUAL -1)
+            MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+            SET(G2O_CPP11 2)
+         ELSE()
+            MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+            SET(G2O_CPP11 1)
+         ENDIF()
    ELSE()
        FIND_PACKAGE(G2O QUIET)
        IF(G2O_FOUND)
            MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
+            FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
+               PATHS ${G2O_INCLUDES}
+               NO_DEFAULT_PATH)
+            FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+            STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+            IF(NOT ${matchres} EQUAL -1)
+               MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+               SET(G2O_CPP11 1)
+            ENDIF()
        ENDIF(G2O_FOUND)
    ENDIF()
 ENDIF(WITH_G2O)
@@ -1410,11 +1419,11 @@ MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
 ENDIF()

 IF(TORCH_FOUND)
-MESSAGE(STATUS "  With SupertPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
+MESSAGE(STATUS "  With SuperPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
 ELSEIF(NOT WITH_TORCH)
-MESSAGE(STATUS "  With SupertPoint          = NO (WITH_TORCH=OFF)")
+MESSAGE(STATUS "  With SuperPoint          = NO (WITH_TORCH=OFF)")
 ELSE()
-MESSAGE(STATUS "  With SupertPoint          = NO (libtorch not found)")
+MESSAGE(STATUS "  With SuperPoint          = NO (libtorch not found)")
 ENDIF()

 IF(WITH_PYTHON AND Python3_FOUND)
@@ -67,8 +67,8 @@ private:
 	unsigned int _dataBufferMaxSize;
 	bool _resetOdometry;
 	Transform _resetPose;
-	double _lastImuStamp;
-	double _imuEstimatedDelay;
+	double _oldestAsyncImuStamp;
+	double _newestAsyncImuStamp;
 };

 } // namespace rtabmap
@@ -5526,7 +5526,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 								materialPolygons[p][i] = serializedPolygons.at<int>(t + p*polygonSize + i);
 							}
 						}
-						t+=materialPolygons.size()*polygonSize;
+						t+=materialPolygons.size()*polygonSize-1;
 						polygons->push_back(materialPolygons);
 					}
 				}
@@ -5543,7 +5543,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 						UASSERT(serializedTexCoords.total());
 						for(int t=0; t<serializedTexCoords.cols; ++t)
 						{
-							UASSERT(int(serializedTexCoords.at<float>(t)) > 0);
+							UASSERT_MSG(int(serializedTexCoords.at<float>(t)) > 0, uFormat("serializedTexCoords.at<float>(%d)=%f", t, serializedTexCoords.at<float>(t)).c_str());
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 							std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > materialtexCoords(int(serializedTexCoords.at<float>(t)));
 #else
@@ -5556,8 +5556,10 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 							{
 								materialtexCoords[p][0] = serializedTexCoords.at<float>(t + p*2);
 								materialtexCoords[p][1] = serializedTexCoords.at<float>(t + p*2 + 1);
+								UASSERT(materialtexCoords[p][0]>=0.0f && materialtexCoords[p][0] <= 1.0f);
+								UASSERT(materialtexCoords[p][1]>=0.0f && materialtexCoords[p][1] <= 1.0f);
 							}
-							t+=materialtexCoords.size()*2;
+							t+=materialtexCoords.size()*2-1;
 							texCoords->push_back(materialtexCoords);
 						}
 					}
@@ -688,7 +688,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #ifndef RTABMAP_TORCH
 	if(type == Feature2D::kFeatureSuperPointTorch)
 	{
-		UWARN("SupertPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
+		UWARN("SuperPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
 		type = Feature2D::kFeatureGfttOrb;
 	}
 #endif
@@ -445,6 +445,10 @@ bool importPoses(
 				std::list<std::string> strList = uSplit(str);
 				if((strList.size() >=  8 && format!=11) || (strList.size() ==  9 && format==11))
 				{
+					if(!uIsNumber(strList.front())) {
+						UWARN("Skipping \"%s\"", str.c_str());
+						continue;
+					}
 					double stamp = uStr2Double(strList.front());
 					strList.pop_front();
 					if(format==11)
@@ -322,7 +322,9 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 				Transform previous = this->getPose();
 				Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
 				UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
+				std::map<double, rtabmap::Transform> imus = imus_;
 				this->reset(newFramePose);
+				imus_ = imus;
 			}

 			imus_.insert(std::make_pair(data.stamp(), imuT));
@@ -40,8 +40,8 @@ OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSi
 	_dataBufferMaxSize(dataBufferMaxSize),
 	_resetOdometry(false),
 	_resetPose(Transform::getIdentity()),
-	_lastImuStamp(0.0),
-	_imuEstimatedDelay(0.0)
+	_oldestAsyncImuStamp(0.0),
+	_newestAsyncImuStamp(0.0)
 {
 	UASSERT(_odometry != 0);
 }
@@ -110,7 +110,8 @@ void OdometryThread::mainLoop()
 		UScopeMutex lock(_dataMutex);
 		_dataBuffer.clear();
 		_imuBuffer.clear();
-		_lastImuStamp = 0.0f;
+		_oldestAsyncImuStamp = 0.0;
+		_newestAsyncImuStamp = 0.0;
 	}

 	SensorData data;
@@ -161,22 +162,39 @@ void OdometryThread::addData(const SensorData & data)
 			!data.laserScanCompressed().empty() || 
 			data.imu().empty())
 		{
-			_dataBuffer.push_back(data);
-			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
-			{
-				UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
-				_dataBuffer.erase(_dataBuffer.begin());
+			if(_oldestAsyncImuStamp > 0.0 && data.stamp() < _oldestAsyncImuStamp) {
+				UWARN("Received image/lidar with stamp (%f) older than oldest received imu "
+					"(%f), skipping that frame (imu buffer size=%ld). "
+					"When using async IMU, make sure IMU is published faster "
+					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
+					data.stamp(), _oldestAsyncImuStamp, _imuBuffer.size());
 				notify = false;
 			}
+			else if(_newestAsyncImuStamp > 0.0 && data.stamp()>=_newestAsyncImuStamp) {
+				UWARN("Received image/lidar with stamp (%f) newer than latest received imu "
+					"(%f), skipping that frame (imu buffer size=%ld). "
+					"When using async IMU, make sure IMU is published faster "
+					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
+					data.stamp(), _newestAsyncImuStamp, _imuBuffer.size());
+				notify = false;
+			}
+			else {
+				_dataBuffer.push_back(data);
+				while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
+				{
+					UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
+					_dataBuffer.erase(_dataBuffer.begin());
+					notify = false;
+				}
+			}
 		}
 		else
 		{
 			_imuBuffer.push_back(data);
-			if(_lastImuStamp != 0.0 && data.stamp() > _lastImuStamp)
-			{
-				_imuEstimatedDelay = data.stamp() - _lastImuStamp;
+			if(_oldestAsyncImuStamp == 0) {
+				_oldestAsyncImuStamp = data.stamp();
 			}
-			_lastImuStamp = data.stamp();
+			_newestAsyncImuStamp = data.stamp();
 		}
 	}
 	_dataMutex.unlock();
@@ -195,18 +213,14 @@ bool OdometryThread::getData(SensorData & data)
 	{
 		if(!_dataBuffer.empty())
 		{
-			if(!_imuBuffer.empty())
+			// Send IMU up to stamp greater than image (OpenVINS needs this).
+			while(!_imuBuffer.empty())
 			{
-				// Send IMU up to stamp greater than image (OpenVINS needs this).
-				while(!_imuBuffer.empty())
-				{
-					_odometry->process(_imuBuffer.front());
-					double stamp = _imuBuffer.front().stamp();
-					_imuBuffer.pop_front();
-					if(stamp > _dataBuffer.front().stamp())
-					{
-						break;
-					}
+				_odometry->process(_imuBuffer.front());
+				double stamp =_imuBuffer.front().stamp();
+				_imuBuffer.pop_front();
+				if(stamp > _dataBuffer.front().stamp()) {
+					break;
 				}
 			}

@@ -197,7 +197,7 @@ std::map<int, Transform> OptimizerCeres::optimize(

 					if(angle_local_manifold == NULL)
 					{
-						angle_local_manifold = ceres::examples::AngleManfold::Create();
+						angle_local_manifold = ceres::examples::AngleManifold::Create();
 					}
 					SetCeresProblemManifold(problem, &pose_begin_iter->second.yaw_radians, angle_local_manifold);
 					SetCeresProblemManifold(problem, &pose_end_iter->second.yaw_radians, angle_local_manifold);
@@ -74,8 +74,8 @@ public:
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
-    ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
+    /*ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
+    ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));*/
  }
 #endif
 };
@@ -158,10 +158,10 @@ private:
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & boost::serialization::make_nvp("NoiseModelFactor1",
+    /*ar & boost::serialization::make_nvp("NoiseModelFactor1",
        boost::serialization::base_object<Base>(*this));
    ar & boost::serialization::make_nvp("GravityFactor",
-        boost::serialization::base_object<GravityFactor>(*this));
+        boost::serialization::base_object<GravityFactor>(*this));*/
  }
 #endif

@@ -254,10 +254,10 @@ private:
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & boost::serialization::make_nvp("NoiseModelFactor1",
+    /*ar & boost::serialization::make_nvp("NoiseModelFactor1",
        boost::serialization::base_object<Base>(*this));
    ar & boost::serialization::make_nvp("GravityFactor",
-        boost::serialization::base_object<GravityFactor>(*this));
+        boost::serialization::base_object<GravityFactor>(*this));*/
  }
 #endif
 public:
@@ -11,7 +11,7 @@ ENV DEBIAN_FRONTEND=noninteractive
 WORKDIR /root/

 # issue: https://github.com/introlab/rtabmap/issues/1523
-RUN rm /etc/apt/sources.list.d/ros1-latest.list && \
+RUN rm /etc/apt/sources.list.d/ros1-latest.list || true && \
    apt-get update && apt-get install -y curl && \
    sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && \
    curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - && \
@@ -155,7 +155,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/
    cd AliceVision && \
    git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \
    git submodule update -i && \
-    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \
+    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/5e3437cf6229c8d534bbaee475ed9bcb92eb84a1/alicevision_0f6115b.patch && \
    git apply alicevision_0f6115b.patch && \
    mkdir build && \
    cd build && \
@@ -0,0 +1,206 @@
+# Image: introlab3it/rtabmap:android-noble-deps
+
+FROM ubuntu:24.04
+
+# Install build dependencies
+RUN apt-get update && apt-get install -y --no-install-recommends apt-utils
+RUN apt-get update && apt-get install -y \
+      git unzip wget ant cmake \
+      g++ lib32stdc++6 lib32z1 \
+      software-properties-common \
+      freeglut3-dev \
+      openjdk-8-jdk openjdk-8-jre \
+      curl
+
+ENV ANDROID_HOME=/opt/android-sdk
+ENV PATH=$PATH:/opt/android-sdk/cmdline-tools/latest/bin:/opt/android-sdk/tools:/opt/android-sdk/platform-tools:/opt/android-sdk/ndk/21.4.7075529
+ENV ANDROID_NDK=/opt/android-sdk/ndk/21.4.7075529
+ENV JAVA_HOME=/usr/lib/jvm/java-8-openjdk-amd64
+
+WORKDIR /root/
+
+# Setup android sdk
+RUN wget -nv https://dl.google.com/android/repository/commandlinetools-linux-7583922_latest.zip && \
+    unzip -qq commandlinetools-linux-7583922_latest.zip && \
+    rm commandlinetools-linux-7583922_latest.zip && \
+    mkdir $ANDROID_HOME && \
+    mkdir $ANDROID_HOME/cmdline-tools && \
+    mv cmdline-tools $ANDROID_HOME/cmdline-tools/latest
+# We should use build-tools <=30 to avoid dx missing error
+RUN echo y | sdkmanager --install "platform-tools" "platforms;android-23" "platforms;android-24" "platforms;android-26" "platforms;android-30" "build-tools;30.0.3" "ndk;21.4.7075529"
+
+# we need <=r25 tools to use "android" command (now deprecated)
+RUN wget -nv http://dl-ssl.google.com/android/repository/tools_r25.2.5-linux.zip && \
+    unzip -qq tools_r25.2.5-linux.zip && \
+    mv tools $ANDROID_HOME/. && \
+    rm tools_r25.2.5-linux.zip
+
+##############
+# Dependencies (took from docker/bionic/android/deps.bash)
+##############
+
+# Install directory for all dependencies
+RUN mkdir -p /opt/android/arm64-v8a
+
+# Boost
+RUN echo "Install boost..." && \
+    wget -nv https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz && \
+    tar -xzf boost_1_59_0.tar.gz && \
+    cd boost_1_59_0 && \
+    wget -nv https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/1733253195bc4d4d9b7f9eda1e60628dc1e51429/BoostConfig.cmake.in && \
+    wget -nv https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/1733253195bc4d4d9b7f9eda1e60628dc1e51429/CMakeLists.txt && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" ..  && \
+    make -j4 && \
+    make install && \
+    cd /root  && \
+    rm -r boost_1_59_0.tar.gz boost_1_59_0
+
+# eigen
+RUN echo "Install eigen..." && \
+    curl -L https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz -o 3.3.9.tar.gz && \
+    tar -xzf 3.3.9.tar.gz && \
+    cd eigen-3.3.9 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" ..  && \
+    make -j4 && \
+    make install && \
+    cd /root && \
+    rm -r 3.3.9.tar.gz eigen-3.3.9
+
+# FLANN
+RUN echo "Install flann..." && \
+    git clone -b 1.8.4 https://github.com/mariusmuja/flann.git && \
+    cd flann && \
+    wget -nv https://gist.githubusercontent.com/matlabbe/cacff9f8271d0c42acd622939a26cab4/raw/85baf4927b32844ebd7f8eccce421bde181cd190/flann_1_8_4_android_fix.patch && \
+    git apply flann_1_8_4_android_fix.patch && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DBUILD_PYTHON_BINDINGS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" .. && \
+    make -j4 && \
+    make install && \
+    cd /root && \
+    rm -rf flann
+
+# GTSAM
+RUN echo "Install gtsam..." && \
+    git clone https://bitbucket.org/gtborg/gtsam.git && \
+    cd gtsam && \
+    git checkout fbb9d3bdda8b88df51896bc401bfd170573e66f5 && \
+    wget -nv https://gist.github.com/matlabbe/726b490c658afd3293f4b3f2f501b863/raw/df09fc8e238a495d66b062d92dc1c1fb20a581e8/gtsam_GKlib_android_fix.patch && \
+    git apply gtsam_GKlib_android_fix.patch && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \
+    make -j4 && \
+    make install && \
+    cd /root && \
+    rm -rf gtsam
+
+# g2o
+RUN echo "Install g2o..." && \
+    git clone https://github.com/RainerKuemmerle/g2o.git && \
+    cd g2o && \
+    git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF .. && \
+    make -j4 && \
+    make install && \
+    cd /root && \
+    rm -rf g2o
+
+# VTK
+RUN echo "Install VTK..." && \
+    git clone https://github.com/Kitware/VTK.git && \
+    cd VTK && \
+    git checkout tags/v8.2.0 && \
+    wget https://gist.github.com/matlabbe/e217259fb8ece9ee6daf5a8f70e896a0/raw/2214b503a537d6431d764526b5b780f07d6f168d/vtk_8_2_0_android_r21_fix.patch && \
+    git apply vtk_8_2_0_android_r21_fix.patch && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DANDROID_ARCH_ABI=arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" .. && \
+    make -j4 && \
+    cp -r CMakeExternals/Install/vtk-android/* /opt/android/arm64-v8a/. && \
+    cd /root && \
+    rm -rf VTK
+
+# PCL
+RUN echo "Install pcl..." && \
+    git clone https://github.com/PointCloudLibrary/pcl.git && \
+    cd pcl && \
+    git checkout tags/pcl-1.8.0 && \
+    wget https://gist.github.com/matlabbe/41812e50e459b2f27b331a2343569e5d/raw/b2fc0c4d1cfffb3a9f2811abae782e317c539bfb/pcl_1_8_0_vtk_android_support.patch && \
+    git apply pcl_1_8_0_vtk_android_support.patch && \
+    mkdir build && \
+    cd build && \
+    # do it 2 times because there is a cmake error on the first time and not the second time!?
+    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF .. || true && \
+    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF .. && \
+    make -j4 && \
+    make install && \
+    cd /root && \
+    rm -rf pcl
+
+# make sure opencv is using the shared version of zlib
+# see https://github.com/android/ndk/issues/1179
+RUN mv $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/sysroot/usr/lib/aarch64-linux-android/libz.a $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/sysroot/usr/lib/aarch64-linux-android/libz.a.back
+
+# OpenCV
+RUN echo "Install OpenCV..." && \
+    git clone https://github.com/opencv/opencv_contrib.git && \
+    git clone https://github.com/opencv/opencv.git && \
+    cd opencv_contrib && \
+    git checkout tags/4.5.5 && \
+    cd /root && \
+    cd opencv && \
+    git checkout tags/4.5.5 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DBUILD_opencv_structured_light=OFF -DBUILD_ANDROID_PROJECTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DBUILD_ANDROID_EXAMPLES=OFF -DWITH_PROTOBUF=OFF -DBUILD_opencv_stereo=OFF -DBUILD_JAVA=OFF -DWITH_QUIRC=OFF -DBUILD_opencv_js_bindings_generator=OFF -DBUILD_opencv_objc_bindings_generator=OFF -DBUILD_opencv_objdetect=OFF -DBUILD_opencv_xobjdetect=OFF .. && \
+    make -j4 && \
+    make install && \
+    cd /root && \
+    rm -rf opencv opencv_contrib
+
+RUN echo "Strip libraries..." && \
+    $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/lib/*.a && \
+    $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/sdk/native/staticlibs/arm64-v8a/*.a
+
+RUN mkdir /opt/android/lib
+
+# tango
+RUN wget 'https://docs.google.com/uc?authuser=0&id=12rHHkYM5k-UnQn-xGXs9JqYWhSXrgJr3&export=download' -O TangoSDK_Ikariotikos_C.zip && \
+    unzip -qq TangoSDK_Ikariotikos_C.zip && \
+    rm TangoSDK_Ikariotikos_C.zip && \
+    cp -r lib_tango_client_api/include/* /opt/android/arm64-v8a/include/. && \
+    cp -r lib_tango_client_api/lib/arm64-v8a/* /opt/android/arm64-v8a/lib/. && \
+    rm -r lib_tango_client_api && \
+    wget 'https://docs.google.com/uc?authuser=0&id=1AqVuEVu5284X6OgrGWu12VTrx4pY99Jb&export=download' -O TangoSupport_Ikariotikos_C.zip && \
+    unzip -qq TangoSupport_Ikariotikos_C.zip && \
+    rm TangoSupport_Ikariotikos_C.zip && \
+    cp -r lib_tango_support_api/include/* /opt/android/arm64-v8a/include/. && \
+    cp -r lib_tango_support_api/lib/arm64-v8a/* /opt/android/arm64-v8a/lib/. && \
+    rm -r lib_tango_support_api && \
+    wget 'https://docs.google.com/uc?authuser=0&id=1s5iPJ7xiridj9Jj--gCy2XiQFniheVm6&export=download' -O TangoSDK_Ikariotikos_Java.jar && \
+    mv TangoSDK_Ikariotikos_Java.jar /opt/android/lib/
+
+# ARCore
+RUN wget 'https://docs.google.com/uc?authuser=0&id=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip && \
+    unzip -qq arcore.zip && \
+    rm arcore.zip && \
+    cp -r arcore1_18/include/* /opt/android/arm64-v8a/include/. && \
+    cp -r arcore1_18/arm64-v8a/* /opt/android/arm64-v8a/lib/. && \
+    cp arcore1_18/*.jar /opt/android/lib/ && \
+    rm -r arcore1_18
+
+# AREngine
+RUN wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip && \
+    unzip -qq arengine.zip && \
+    rm arengine.zip && \
+    cp -r arengine/include/* /opt/android/arm64-v8a/include/. && \
+    cp -r arengine/arm64-v8a/* /opt/android/arm64-v8a/lib/. && \
+    cp arengine/*.jar /opt/android/lib/ && \
+    rm -r arengine
@@ -0,0 +1,13 @@
+# Image: introlab3it/rtabmap:androidXX
+
+FROM introlab3it/rtabmap:android-noble-deps
+
+ARG API_VERSION=23
+
+# Copy current source code
+COPY . /root/rtabmap-tango
+
+WORKDIR /root/rtabmap-tango
+
+RUN /bin/bash -c "./docker/noble/android/rtabmap_apiXX/rtabmap.bash /opt/android $API_VERSION"
+
@@ -0,0 +1,29 @@
+#!/bin/bash
+
+set -e
+
+if [ $# -ne 2 ]; then
+    echo "rtabmap.bash android_install_prefix api_level (23 for tango, 24 for arengine)   # Example: build.bash /opt/android 24"
+    exit 1
+fi
+
+prefix=$1
+api=$2
+
+# copy required jars
+cp /opt/android/lib/*.jar app/android/libs/.
+
+# resource tool
+cd build
+cmake -DANDROID_PREBUILD=ON ..
+make
+
+# rtabmap
+mkdir arm64-v8a
+cd arm64-v8a
+cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$api -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni ../..
+make
+make clean
+
+
+
@@ -334,6 +334,9 @@ public:

 	bool getPose(const std::string & id, Transform & pose); //including meshes
 	bool getCloudVisibility(const std::string & id);
+	int getCloudColorIndex(const std::string & id) const;
+	double getCloudOpacity(const std::string & id) const;
+	int getCloudPointSize(const std::string & id) const;

 	const QMap<std::string, Transform> & getAddedClouds() const {return _addedClouds;} //including meshes
 	const QColor & getDefaultBackgroundColor() const;
@@ -399,6 +402,12 @@ public:
 	void setIntensityRedColormap(bool value);
 	void setIntensityRainbowColormap(bool value);
 	void setIntensityMax(float value);
+	float getCloudColorRangeMin() const;
+	float getCloudColorRangeMax() const;
+	bool isCloudColorRangeInverted() const;
+	void setCloudColorRangeMin(float value);
+	void setCloudColorRangeMax(float value);
+	void setCloudColorRangeInverted(bool enabled);
 	void buildPickingLocator(bool enable);
 	const std::map<std::string, vtkSmartPointer<vtkOBBTree> > & getLocators() const {return _locators;}

@@ -454,6 +463,10 @@ private:
 	QAction * _aSetIntensityRedColormap;
 	QAction * _aSetIntensityRainbowColormap;
 	QAction * _aSetIntensityMaximum;
+	QAction * _aSetCloudColorRangeMin;
+	QAction * _aSetCloudColorRangeMax;
+	QAction * _aCloudColorRangeInverted;
+	QAction * _aClearCloudColorRanges;
    QAction * _aSetBackgroundColor;
    QAction * _aSetRenderingRate;
    QAction * _aSetEDLShading;
@@ -494,6 +507,8 @@ private:
    double _renderingRate;
    vtkProp * _octomapActor;
    float _intensityAbsMax;
+	float _cloudColorRangeMin;
+	float _cloudColorRangeMax;
    double  _coordinateFrameScale;
 };

@@ -80,6 +80,7 @@ public:
 	void updateLocalPath(const std::vector<int> & localPath);
 	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
 	void setCurrentGoalID(int id, const Transform & pose = Transform());
+	void setNodeInfo(int id, const QString & info);
 	void setLocalRadius(float radius);
 	void highlightNode(int nodeId, int highlightIndex);
 	void clearGraph();
@@ -77,6 +77,7 @@ public:
 	float getDepthColorMapMinRange() const;
 	float getDepthColorMapMaxRange() const;
 	uCvQtDepthColorMap getDepthColorMap() const;
+	bool isDepthColorMapInCameraFrame() const;

 	float viewScale() const;

@@ -94,6 +95,7 @@ public:
 	void setDefaultMatchingLineColor(const QColor & color);
 	void setBackgroundColor(const QColor & color);
 	void setDepthColorMapRange(float min, float max);
+	void setDepthColorMapInCameraFrame(bool enabled);

 	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
 	void setFeatures(const std::vector<cv::KeyPoint> & features, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
@@ -167,6 +169,7 @@ private:
 	QAction * _colorMapBlackToWhite;
 	QAction * _colorMapRedToBlue;
 	QAction * _colorMapBlueToRed;
+	QAction * _colorMapInCameraFrame;
 	QAction * _colorMapMinRange;
 	QAction * _colorMapMaxRange;
 	QAction * _mouseTracking;
@@ -0,0 +1,186 @@
+/*
+Copyright (c) 2010-2025, 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 GUILIB_SRC_POINTCLOUDCOLORHANDLEINTENSITYFIELD_H_
+#define GUILIB_SRC_POINTCLOUDCOLORHANDLEINTENSITYFIELD_H_
+
+#include <pcl/visualization/point_cloud_color_handlers.h>
+#include <pcl/pcl_config.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/UMath.h>
+
+namespace rtabmap
+{
+
+class PointCloudColorHandlerIntensityField : public pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>
+{
+	typedef pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloud PointCloud;
+	typedef PointCloud::Ptr PointCloudPtr;
+	typedef PointCloud::ConstPtr PointCloudConstPtr;
+
+public:
+	/** \brief Constructor. */
+	PointCloudColorHandlerIntensityField(const PointCloudConstPtr &cloud, float maxAbsIntensity = 0.0f, int colorMap = 0) : pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloudColorHandler(cloud),
+																															maxAbsIntensity_(maxAbsIntensity),
+																															colormap_(colorMap)
+	{
+		field_idx_ = pcl::getFieldIndex(*cloud, "intensity");
+		if (field_idx_ != -1)
+			capable_ = true;
+		else
+			capable_ = false;
+	}
+
+	/** \brief Empty destructor */
+	virtual ~PointCloudColorHandlerIntensityField() {}
+
+	/** \brief Obtain the actual color for the input dataset as vtk scalars.
+	 * \param[out] scalars the output scalars containing the color for the dataset
+	 * \return true if the operation was successful (the handler is capable and
+	 * the input cloud was given as a valid pointer), false otherwise
+	 */
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+	virtual vtkSmartPointer<vtkDataArray> getColor() const
+	{
+		vtkSmartPointer<vtkDataArray> scalars;
+		if (!capable_ || !cloud_)
+			return scalars;
+#else
+	virtual bool getColor(vtkSmartPointer<vtkDataArray> &scalars) const
+	{
+		if (!capable_ || !cloud_)
+			return (false);
+#endif
+		if (!scalars)
+			scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
+		scalars->SetNumberOfComponents(3);
+
+		vtkIdType nr_points = cloud_->width * cloud_->height;
+		// Allocate enough memory to hold all colors
+		float *intensities = new float[nr_points];
+		float intensity;
+		size_t point_offset = cloud_->fields[field_idx_].offset;
+		size_t j = 0;
+
+		// If XYZ present, check if the points are invalid
+		int x_idx = pcl::getFieldIndex(*cloud_, "x");
+		if (x_idx != -1)
+		{
+			float x_data, y_data, z_data;
+			size_t x_point_offset = cloud_->fields[x_idx].offset;
+
+			// Color every point
+			for (vtkIdType cp = 0; cp < nr_points; ++cp,
+							point_offset += cloud_->point_step,
+							x_point_offset += cloud_->point_step)
+			{
+				// Copy the value at the specified field
+				memcpy(&intensity, &cloud_->data[point_offset], sizeof(float));
+
+				memcpy(&x_data, &cloud_->data[x_point_offset], sizeof(float));
+				memcpy(&y_data, &cloud_->data[x_point_offset + sizeof(float)], sizeof(float));
+				memcpy(&z_data, &cloud_->data[x_point_offset + 2 * sizeof(float)], sizeof(float));
+
+				if (!std::isfinite(x_data) || !std::isfinite(y_data) || !std::isfinite(z_data))
+					continue;
+
+				intensities[j++] = intensity;
+			}
+		}
+		// No XYZ data checks
+		else
+		{
+			// Color every point
+			for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
+			{
+				// Copy the value at the specified field
+				memcpy(&intensity, &cloud_->data[point_offset], sizeof(float));
+
+				intensities[j++] = intensity;
+			}
+		}
+		if (j != 0)
+		{
+			// Allocate enough memory to hold all colors
+			unsigned char *colors = new unsigned char[j * 3];
+			float min, max;
+			if (maxAbsIntensity_ > 0.0f)
+			{
+				max = maxAbsIntensity_;
+			}
+			else
+			{
+				uMinMax(intensities, j, min, max);
+			}
+			for (size_t k = 0; k < j; ++k)
+			{
+				colors[k * 3 + 0] = colors[k * 3 + 1] = colors[k * 3 + 2] = max > 0 ? (unsigned char)(std::min(intensities[k] / max * 255.0f, 255.0f)) : 255;
+				if (colormap_ == 1)
+				{
+					colors[k * 3 + 0] = 255;
+					colors[k * 3 + 2] = 0;
+				}
+				else if (colormap_ == 2)
+				{
+					float r, g, b;
+					util2d::HSVtoRGB(&r, &g, &b, colors[k * 3 + 0] * 299.0f / 255.0f, 1.0f, 1.0f);
+					colors[k * 3 + 0] = r * 255.0f;
+					colors[k * 3 + 1] = g * 255.0f;
+					colors[k * 3 + 2] = b * 255.0f;
+				}
+			}
+			reinterpret_cast<vtkUnsignedCharArray *>(&(*scalars))->SetNumberOfTuples(j);
+			reinterpret_cast<vtkUnsignedCharArray *>(&(*scalars))->SetArray(colors, j * 3, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
+		}
+		else
+			reinterpret_cast<vtkUnsignedCharArray *>(&(*scalars))->SetNumberOfTuples(0);
+		// delete [] colors;
+		delete[] intensities;
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+		return scalars;
+#else
+		return (true);
+#endif
+	}
+
+protected:
+	/** \brief Get the name of the class. */
+	virtual std::string
+	getName() const { return ("PointCloudColorHandlerIntensityField"); }
+
+	/** \brief Get the name of the field used. */
+	virtual std::string
+	getFieldName() const { return ("intensity"); }
+
+private:
+	float maxAbsIntensity_;
+	int colormap_; // 0=grayscale, 1=redYellow, 2=RainbowHSV
+};
+
+} /* namespace rtabmap */
+
+#endif /* GUILIB_SRC_POINTCLOUDCOLORHANDLEINTENSITYFIELD_H_ */
@@ -0,0 +1,187 @@
+/*
+Copyright (c) 2010-2025, 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 GUILIB_SRC_POINTCLOUDCOLORHANDLEMINMAXGENERICFIELD_H_
+#define GUILIB_SRC_POINTCLOUDCOLORHANDLEMINMAXGENERICFIELD_H_
+
+#include <limits>
+#include <pcl/visualization/point_cloud_color_handlers.h>
+#include <pcl/pcl_config.h>
+
+namespace rtabmap
+{
+
+	/// Same than pcl::visualization::PointCloudColorHandlerGenericField but with min and max parameters
+	class PointCloudColorHandlerMinMaxGenericField : public pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>
+	{
+		using PointCloud = typename PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloud;
+		using PointCloudPtr = typename PointCloud::Ptr;
+		using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+	public:
+		/** \brief Constructor. */
+		PointCloudColorHandlerMinMaxGenericField(const PointCloudConstPtr &cloud,
+												 const std::string &field_name,
+												 float min = std::numeric_limits<float>::lowest(),
+												 float max = std::numeric_limits<float>::max(),
+												 bool inverted_color_scale = false)
+			: pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>(cloud),
+			  field_name_(field_name),
+			  min_(min),
+			  max_(max),
+			  inverted_color_scale_(inverted_color_scale)
+		{
+			setInputCloud(cloud);
+		}
+
+		/** \brief Destructor. */
+		virtual ~PointCloudColorHandlerMinMaxGenericField() {}
+
+		/** \brief Get the name of the field used. */
+		virtual std::string getFieldName() const { return (field_name_); }
+
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+		virtual vtkSmartPointer<vtkDataArray> getColor() const
+		{
+			vtkSmartPointer<vtkDataArray> scalars;
+			if (!capable_ || !cloud_)
+				return scalars;
+#else
+		virtual bool getColor(vtkSmartPointer<vtkDataArray> &scalars) const
+		{
+			if (!capable_ || !cloud_)
+				return (false);
+#endif
+			if (!scalars)
+			    scalars = vtkSmartPointer<vtkFloatArray>::New ();
+			scalars->SetNumberOfComponents(1);
+
+			vtkIdType nr_points = cloud_->width * cloud_->height;
+			scalars->SetNumberOfTuples(nr_points);
+
+			float *colors = new float[nr_points];
+			float field_data;
+			int j = 0;
+			int point_offset = cloud_->fields[field_idx_].offset;
+
+			// If XYZ present, check if the points are invalid
+			int x_idx = pcl::getFieldIndex(*cloud_, "x");
+			if (x_idx != -1)
+			{
+				float x_data, y_data, z_data;
+				int x_point_offset = cloud_->fields[x_idx].offset;
+
+				// Color every point
+				for (vtkIdType cp = 0; cp < nr_points; ++cp,
+							   point_offset += cloud_->point_step,
+							   x_point_offset += cloud_->point_step)
+				{
+					memcpy(&x_data, &cloud_->data[x_point_offset], sizeof(float));
+					memcpy(&y_data, &cloud_->data[x_point_offset + sizeof(float)], sizeof(float));
+					memcpy(&z_data, &cloud_->data[x_point_offset + 2 * sizeof(float)], sizeof(float));
+					if (!std::isfinite(x_data) || !std::isfinite(y_data) || !std::isfinite(z_data))
+						continue;
+
+					// Copy the value at the specified field
+					memcpy(&field_data, &cloud_->data[point_offset], pcl::getFieldSize(cloud_->fields[field_idx_].datatype));
+					if(field_data < min_) {
+						field_data = min_;
+					}
+					if(field_data > max_) {
+						field_data = max_;
+					}
+					colors[j] = field_data * (inverted_color_scale_?-1.0f:1.0f);
+					j++;
+				}
+			}
+			// No XYZ data checks
+			else
+			{
+				// Color every point
+				for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
+				{
+					// Copy the value at the specified field
+					// memcpy (&field_data, &cloud_->data[point_offset], sizeof (float));
+					memcpy(&field_data, &cloud_->data[point_offset], pcl::getFieldSize(cloud_->fields[field_idx_].datatype));
+
+					if (!std::isfinite(field_data))
+						continue;
+
+					if(field_data < min_) {
+						field_data = min_;
+					}
+					if(field_data > max_) {
+						field_data = max_;
+					}
+					
+					colors[j] = field_data * (inverted_color_scale_?-1.0f:1.0f);
+					j++;
+				}
+			}
+			reinterpret_cast<vtkFloatArray *>(&(*scalars))->SetArray(colors, j, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
+
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+			return scalars;
+#else
+			return (true);
+#endif
+		}
+
+		using PointCloudColorHandler<pcl::PCLPointCloud2>::getColor;
+
+		/** \brief Set the input cloud to be used.
+		 * \param[in] cloud the input cloud to be used by the handler
+		 */
+		virtual void
+		setInputCloud(const PointCloudConstPtr &cloud)
+		{
+			PointCloudColorHandler<pcl::PCLPointCloud2>::setInputCloud(cloud);
+			field_idx_ = pcl::getFieldIndex(*cloud, field_name_);
+			capable_ = field_idx_ != -1;
+			if (field_idx_ != -1 && cloud_->fields[field_idx_].datatype != pcl::PCLPointField::PointFieldTypes::FLOAT32)
+			{
+				capable_ = false;
+				PCL_ERROR("[pcl::PointCloudColorHandlerGenericField] This currently only works with float32 fields, but field %s has a different type.\n", field_name_.c_str());
+			}
+		}
+
+	protected:
+		/** \brief Class getName method. */
+		virtual std::string
+		getName() const { return ("PointCloudColorHandlerMinMaxGenericField"); }
+
+	private:
+		/** \brief Name of the field used to create the color handler. */
+		std::string field_name_;
+		float min_;
+		float max_;
+		bool inverted_color_scale_;
+	};
+
+} /* namespace rtabmap */
+
+#endif /* GUILIB_SRC_POINTCLOUDCOLORHANDLEMINMAXGENERICFIELD_H_ */
@@ -117,7 +117,7 @@ inline QImage uCvMat2QImage(
 		// Assume depth image (float in meters)
 		const float * data = (const float *)image.data;
 		float min,max;
-		if(depthMax>depthMin)
+		if(depthMin != 0 && depthMax != 0 && depthMax > depthMin)
 		{
 			min = depthMin;
 			max = depthMax;
@@ -127,23 +127,23 @@ inline QImage uCvMat2QImage(
 			min = max = data[0];
 			for(unsigned int i=1; i<image.total(); ++i)
 			{
-				if(uIsFinite(data[i]) && data[i] > 0)
+				if(uIsFinite(data[i]) && data[i] != 0)
 				{
-					if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
+					if(!uIsFinite(min) || (data[i] != 0 && data[i]<min))
 					{
 						min = data[i];
 					}
-					if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
+					if(!uIsFinite(max) || (data[i] != 0 && data[i]>max))
 					{
 						max = data[i];
 					}
 				}
 			}
-			if(depthMax > 0 && depthMax > depthMin)
+			if(depthMax != 0 && depthMax > depthMin)
 			{
 				max = depthMax;
 			}
-			if(depthMin>0 && (depthMin < depthMax || depthMin < max))
+			if(depthMin != 0 && (depthMin < depthMax || depthMin < max))
 			{
 				min = depthMin;
 			}
@@ -198,7 +198,7 @@ inline QImage uCvMat2QImage(
 		// Assume depth image (unsigned short in mm)
 		const unsigned short * data = (const unsigned short *)image.data;
 		unsigned short min,max;
-		if(depthMax>depthMin)
+		if(depthMin != 0 && depthMax != 0 && depthMax > depthMin)
 		{
 			min = depthMin*1000;
 			max = depthMax*1000;
@@ -208,23 +208,23 @@ inline QImage uCvMat2QImage(
 			min = max = data[0];
 			for(unsigned int i=1; i<image.total(); ++i)
 			{
-				if(uIsFinite(data[i]) && data[i] > 0)
+				if(uIsFinite(data[i]) && data[i] != 0)
 				{
-					if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
+					if(!uIsFinite(min) || (data[i] != 0 && data[i]<min))
 					{
 						min = data[i];
 					}
-					if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
+					if(!uIsFinite(max) || (data[i] != 0 && data[i]>max))
 					{
 						max = data[i];
 					}
 				}
 			}
-			if(depthMax > 0 && depthMax > depthMin)
+			if(depthMax != 0 && depthMax > depthMin)
 			{
 				max = depthMax*1000;
 			}
-			if(depthMin>0 && (depthMin < depthMax || depthMin*1000 < max))
+			if(depthMin != 0 && (depthMin < depthMax || depthMin*1000 < max))
 			{
 				min = depthMin*1000;
 			}
@@ -27,6 +27,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap/gui/CloudViewer.h"
 #include "rtabmap/gui/CloudViewerCellPicker.h"
+#include "rtabmap/gui/PointCloudColorHandlerIntensityField.h"
+#include "rtabmap/gui/PointCloudColorHandlerMinMaxGenericField.h"

 #include <rtabmap/core/Version.h>
 #include <rtabmap/core/util3d_transforms.h>
@@ -150,6 +152,8 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 		_renderingRate(5.0),
 		_octomapActor(0),
 		_intensityAbsMax(100.0f),
+		_cloudColorRangeMin(0.0f),
+		_cloudColorRangeMax(0.0f),
 		_coordinateFrameScale(1.0)
 {
 	this->setMinimumSize(200, 200);
@@ -337,6 +341,12 @@ void CloudViewer::createMenu()
 	_aSetIntensityRainbowColormap->setCheckable(true);
 	_aSetIntensityRainbowColormap->setChecked(false);
 	_aSetIntensityMaximum = new QAction("Set maximum absolute intensity...", this);
+	_aSetCloudColorRangeMin = new QAction("Set minimum color range...", this);
+	_aSetCloudColorRangeMax = new QAction("Set maximum color range...", this);
+	_aCloudColorRangeInverted = new QAction("Inverted color scale", this);
+	_aCloudColorRangeInverted->setCheckable(true);
+	_aCloudColorRangeInverted->setChecked(false);
+	_aClearCloudColorRanges = new QAction("Reset ranges", this);
 	_aSetBackgroundColor = new QAction("Set background color...", this);	
 	_aSetRenderingRate = new QAction("Set rendering rate...", this);
 	_aSetEDLShading = new QAction("Eye-Dome Lighting Shading", this);
@@ -402,6 +412,12 @@ void CloudViewer::createMenu()
 	scanMenu->addAction(_aSetIntensityRainbowColormap);
 	scanMenu->addAction(_aSetIntensityMaximum);

+	QMenu * cloudMenu = new QMenu("XYZ color", this);
+	cloudMenu->addAction(_aSetCloudColorRangeMin);
+	cloudMenu->addAction(_aSetCloudColorRangeMax);
+	cloudMenu->addAction(_aCloudColorRangeInverted);
+	cloudMenu->addAction(_aClearCloudColorRanges);
+
 	//menus
 	_menu = new QMenu(this);
 	_menu->addMenu(cameraMenu);
@@ -412,6 +428,7 @@ void CloudViewer::createMenu()
 	_menu->addMenu(gridMenu);
 	_menu->addMenu(normalsMenu);
 	_menu->addMenu(scanMenu);
+	_menu->addMenu(cloudMenu);
 	_menu->addAction(_aSetBackgroundColor);
 	_menu->addAction(_aSetRenderingRate);
 	_menu->addAction(_aSetEDLShading);
@@ -465,6 +482,10 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("intensity_rainbow_colormap", this->isIntensityRainbowColormap());
 	settings.setValue("intensity_max", (double)this->getIntensityMax());

+	settings.setValue("color_range_min", (double)this->getCloudColorRangeMin());
+	settings.setValue("color_range_max", (double)this->getCloudColorRangeMax());
+	settings.setValue("color_range_inverted", (double)this->isCloudColorRangeInverted());
+
 	settings.setValue("trajectory_shown", this->isTrajectoryShown());
 	settings.setValue("trajectory_size", this->getTrajectorySize());

@@ -516,6 +537,10 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setIntensityRainbowColormap(settings.value("intensity_rainbow_colormap", this->isIntensityRainbowColormap()).toBool());
 	this->setIntensityMax(settings.value("intensity_max", this->getIntensityMax()).toFloat());

+	this->setCloudColorRangeMin(settings.value("color_range_min", this->getCloudColorRangeMin()).toFloat());
+	this->setCloudColorRangeMax(settings.value("color_range_max", this->getCloudColorRangeMax()).toFloat());
+	this->setCloudColorRangeInverted(settings.value("color_range_inverted", this->isCloudColorRangeInverted()).toBool());
+
 	this->setTrajectoryShown(settings.value("trajectory_shown", this->isTrajectoryShown()).toBool());
 	this->setTrajectorySize(settings.value("trajectory_size", this->getTrajectorySize()).toUInt());

@@ -606,153 +631,6 @@ bool CloudViewer::updateCloudPose(
 	return false;
 }

-class PointCloudColorHandlerIntensityField : public pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>
-{
-	typedef pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloud PointCloud;
-	typedef PointCloud::Ptr PointCloudPtr;
-	typedef PointCloud::ConstPtr PointCloudConstPtr;
-
-public:
-	typedef boost::shared_ptr<PointCloudColorHandlerIntensityField > Ptr;
-	typedef boost::shared_ptr<const PointCloudColorHandlerIntensityField > ConstPtr;
-
-	/** \brief Constructor. */
-	PointCloudColorHandlerIntensityField (const PointCloudConstPtr &cloud, float maxAbsIntensity = 0.0f, int colorMap = 0) :
-		pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloudColorHandler (cloud),
-		maxAbsIntensity_(maxAbsIntensity),
-		colormap_(colorMap)
-		{
-		field_idx_  = pcl::getFieldIndex (*cloud, "intensity");
-		if (field_idx_ != -1)
-			capable_ = true;
-		else
-			capable_ = false;
-		}
-
-	/** \brief Empty destructor */
-	virtual ~PointCloudColorHandlerIntensityField () {}
-
-	/** \brief Obtain the actual color for the input dataset as vtk scalars.
-	 * \param[out] scalars the output scalars containing the color for the dataset
-	 * \return true if the operation was successful (the handler is capable and
-	 * the input cloud was given as a valid pointer), false otherwise
-	 */
-#if PCL_VERSION_COMPARE(>, 1, 11, 1)
-	virtual vtkSmartPointer<vtkDataArray> getColor () const {
-		vtkSmartPointer<vtkDataArray> scalars;
-		if (!capable_ || !cloud_)
-			return scalars;
-#else
-	virtual bool getColor (vtkSmartPointer<vtkDataArray> &scalars) const {
-		if (!capable_ || !cloud_)
-			return (false);
-#endif
-		if (!scalars)
-			scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
-		scalars->SetNumberOfComponents (3);
-
-		vtkIdType nr_points = cloud_->width * cloud_->height;
-		// Allocate enough memory to hold all colors
-		float * intensities = new float[nr_points];
-		float intensity;
-		size_t point_offset = cloud_->fields[field_idx_].offset;
-		size_t j = 0;
-
-		// If XYZ present, check if the points are invalid
-		int x_idx = pcl::getFieldIndex (*cloud_, "x");
-		if (x_idx != -1)
-		{
-			float x_data, y_data, z_data;
-			size_t x_point_offset = cloud_->fields[x_idx].offset;
-
-			// Color every point
-			for (vtkIdType cp = 0; cp < nr_points; ++cp,
-			point_offset += cloud_->point_step,
-			x_point_offset += cloud_->point_step)
-			{
-				// Copy the value at the specified field
-				memcpy (&intensity, &cloud_->data[point_offset], sizeof (float));
-
-				memcpy (&x_data, &cloud_->data[x_point_offset], sizeof (float));
-				memcpy (&y_data, &cloud_->data[x_point_offset + sizeof (float)], sizeof (float));
-				memcpy (&z_data, &cloud_->data[x_point_offset + 2 * sizeof (float)], sizeof (float));
-
-				if (!std::isfinite (x_data) || !std::isfinite (y_data) || !std::isfinite (z_data))
-					continue;
-
-				intensities[j++] = intensity;
-			}
-		}
-		// No XYZ data checks
-		else
-		{
-			// Color every point
-			for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
-			{
-				// Copy the value at the specified field
-				memcpy (&intensity, &cloud_->data[point_offset], sizeof (float));
-
-				intensities[j++] = intensity;
-			}
-		}
-		if (j != 0)
-		{
-			// Allocate enough memory to hold all colors
-			unsigned char* colors = new unsigned char[j * 3];
-			float min, max;
-			if(maxAbsIntensity_>0.0f)
-			{
-				max = maxAbsIntensity_;
-			}
-			else
-			{
-				uMinMax(intensities, j, min, max);
-			}
-			for(size_t k=0; k<j; ++k)
-			{
-				colors[k*3+0] = colors[k*3+1] = colors[k*3+2] = max>0?(unsigned char)(std::min(intensities[k]/max*255.0f, 255.0f)):255;
-				if(colormap_ == 1)
-				{
-					colors[k*3+0] = 255;
-					colors[k*3+2] = 0;
-				}
-				else if(colormap_ == 2)
-				{
-					float r,g,b;
-					util2d::HSVtoRGB(&r, &g, &b, colors[k*3+0]*299.0f/255.0f, 1.0f, 1.0f);
-					colors[k*3+0] = r*255.0f;
-					colors[k*3+1] = g*255.0f;
-					colors[k*3+2] = b*255.0f;
-				}
-			}
-			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (j);
-			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, j*3, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
-		}
-		else
-			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (0);
-		//delete [] colors;
-		delete [] intensities;
-#if PCL_VERSION_COMPARE(>, 1, 11, 1)
-		return scalars;
-#else
-		return (true);
-#endif
-	}
-
-protected:
-	/** \brief Get the name of the class. */
-	virtual std::string
-	getName () const { return ("PointCloudColorHandlerIntensityField"); }
-
-	/** \brief Get the name of the field used. */
-	virtual std::string
-	getFieldName () const { return ("intensity"); }
-
-private:
-	float maxAbsIntensity_;
-	int colormap_; // 0=grayscale, 1=redYellow, 2=RainbowHSV
-};
-
 bool CloudViewer::addCloud(
 		const std::string & id,
 		const pcl::PCLPointCloud2Ptr & binaryCloud,
@@ -799,11 +677,20 @@ bool CloudViewer::addCloud(
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);

 		// x,y,z
-		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "x"));
+		colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "x",
+			_cloudColorRangeMin==0.0f?std::numeric_limits<float>::lowest():_cloudColorRangeMin,
+			_cloudColorRangeMax==0.0f?std::numeric_limits<float>::max():_cloudColorRangeMax,
+			_aCloudColorRangeInverted->isChecked()));
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
-		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "y"));
+		colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "y",
+			_cloudColorRangeMin==0.0f?std::numeric_limits<float>::lowest():_cloudColorRangeMin,
+			_cloudColorRangeMax==0.0f?std::numeric_limits<float>::max():_cloudColorRangeMax,
+			_aCloudColorRangeInverted->isChecked()));
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
-		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "z"));
+		colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "z",
+			_cloudColorRangeMin==0.0f?std::numeric_limits<float>::lowest():_cloudColorRangeMin,
+			_cloudColorRangeMax==0.0f?std::numeric_limits<float>::max():_cloudColorRangeMax,
+			_aCloudColorRangeInverted->isChecked()));
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);

 		if(rgb)
@@ -3285,6 +3172,12 @@ bool CloudViewer::getCloudVisibility(const std::string & id)
 	return false;
 }

+
+int CloudViewer::getCloudColorIndex(const std::string & id) const
+{
+	return _visualizer->getColorHandlerIndex(id);
+}
+	
 void CloudViewer::setCloudColorIndex(const std::string & id, int index)
 {
 	if(index>0)
@@ -3293,6 +3186,26 @@ void CloudViewer::setCloudColorIndex(const std::string & id, int index)
 	}
 }

+double CloudViewer::getCloudOpacity(const std::string & id) const
+{
+	double opacity = 1.0;
+	if(!_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id))
+	{
+#if VTK_MAJOR_VERSION >= 7
+		pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
+		if (am_it != _visualizer->getShapeActorMap()->end ())
+		{
+			vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
+			if(actor)
+			{
+				opacity = actor->GetProperty ()->GetOpacity ();
+			}
+		}
+#endif
+	}
+	return opacity;
+}
+	
 void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
 {
 	double lastOpacity;
@@ -3320,6 +3233,12 @@ void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
 #endif
 }

+int CloudViewer::getCloudPointSize(const std::string & id) const
+{
+	double size = 1.0;
+	_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size, id);
+	return (int)size;
+}
 void CloudViewer::setCloudPointSize(const std::string & id, int size)
 {
 	double lastSize;
@@ -3611,9 +3530,33 @@ void CloudViewer::setIntensityMax(float value)
 	}
 	else
 	{
-		UERROR("Cannot set normals scale < 0, value=%f", value);
+		UERROR("Cannot set intensity < 0, value=%f", value);
 	}
 }
+float CloudViewer::getCloudColorRangeMin() const
+{
+	return _cloudColorRangeMin;
+}
+float CloudViewer::getCloudColorRangeMax() const
+{
+	return _cloudColorRangeMax;
+}
+bool CloudViewer::isCloudColorRangeInverted() const
+{
+	return _aCloudColorRangeInverted->isChecked();
+}
+void CloudViewer::setCloudColorRangeMin(float value)
+{
+	_cloudColorRangeMin = value;
+}
+void CloudViewer::setCloudColorRangeMax(float value)
+{
+	_cloudColorRangeMax = value;
+}
+void CloudViewer::setCloudColorRangeInverted(bool enabled)
+{
+	_aCloudColorRangeInverted->setChecked(enabled);
+}

 void CloudViewer::buildPickingLocator(bool enable)
 {
@@ -3984,6 +3927,30 @@ void CloudViewer::handleAction(QAction * a)
 	{
 		this->setIntensityRainbowColormap(_aSetIntensityRainbowColormap->isChecked());
 	}
+	else if(a == _aSetCloudColorRangeMin)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Set minimum axis color range"), tr("Range (0=auto)"), _cloudColorRangeMin, -99999, 99999, 2, &ok);
+		if(ok)
+		{
+			this->setCloudColorRangeMin(value);
+		}
+	}
+	else if(a == _aSetCloudColorRangeMax)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Set maximum axis color range"), tr("Range (0=auto)"), _cloudColorRangeMax, -99999, 99999, 2, &ok);
+		if(ok)
+		{
+			this->setCloudColorRangeMax(value);
+		}
+	}
+	else if(a == _aClearCloudColorRanges)
+	{
+		_cloudColorRangeMin = 0.0f;
+		_cloudColorRangeMax = 0.0f;
+		_aCloudColorRangeInverted->setChecked(false);
+	}
 	else if(a == _aSetBackgroundColor)
 	{
 		QColor color = this->getDefaultBackgroundColor();
@@ -445,6 +445,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
+	connect(cloudViewer_, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
 	connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
 	connect(ui_->actionConcise_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
@@ -653,6 +654,9 @@ void DatabaseViewer::readSettings()
 	ui_->graphicsView_A->loadSettings(settings, "ImageViewA");
 	ui_->graphicsView_B->loadSettings(settings, "ImageViewB");

+	// CloudViewer
+	cloudViewer_->loadSettings(settings, "CloudViewer");
+
 	// ICP parameters
 	settings.beginGroup("icp");
 	ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
@@ -747,6 +751,9 @@ void DatabaseViewer::writeSettings()
 	ui_->graphicsView_A->saveSettings(settings, "ImageViewA");
 	ui_->graphicsView_B->saveSettings(settings, "ImageViewB");

+	// CloudViewer
+	cloudViewer_->saveSettings(settings, "CloudViewer");
+
 	// save ICP parameters
 	settings.beginGroup("icp");
 	settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
@@ -3481,18 +3488,24 @@ void DatabaseViewer::exportOptimizedMesh()
 					path += ".obj";
 				}
 				QString baseName = QFileInfo(path).baseName();
+				UDEBUG("Materials: %d", mesh->tex_materials.size());
 				if(mesh->tex_materials.size() == 1)
 				{
 					mesh->tex_materials.at(0).tex_file = baseName.toStdString() + ".png";
-					cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png", textures);
+					std::string filename = (QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png";
+					cv::imwrite(filename, textures);
+					UDEBUG("Saved %s", filename.c_str());
 				}
 				else
 				{
+					QDir(QFileInfo(path).absoluteDir().absolutePath()).mkdir(baseName);
 					for(unsigned int i=0; i<mesh->tex_materials.size(); ++i)
 					{
 						mesh->tex_materials.at(i).tex_file = (baseName+QDir::separator()+QString::number(i)+".png").toStdString();
 						UASSERT((i+1)*textures.rows <= (unsigned int)textures.cols);
-						cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName+QDir::separator()+QString::number(i)+".png").toStdString(), textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)));
+						std::string filename = (QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName+QDir::separator()+QString::number(i)+".png").toStdString();
+						cv::imwrite(filename, textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)));
+						UDEBUG("Saved %s", filename.c_str());
 					}
 				}
 				pcl::io::saveOBJFile(path.toStdString(), *mesh);
@@ -5132,6 +5145,15 @@ void DatabaseViewer::update(int value,
 					cloudViewer_->removeAllLines();
 					cloudViewer_->removeAllFrustums();
 					cloudViewer_->removeOccupancyGridMap();
+					std::map<std::string, std::pair<int, int> > colorIndexAndPointSizeMap;
+					for(auto iter=cloudViewer_->getAddedClouds().constBegin(); iter!=cloudViewer_->getAddedClouds().constEnd(); ++iter) {
+						if(uStrContains(iter.key(), "cloud") || uStrContains(iter.key(), "scan")) {
+							colorIndexAndPointSizeMap.insert(std::make_pair(iter.key(), 
+								std::make_pair(
+									cloudViewer_->getCloudColorIndex(iter.key())+1,
+									cloudViewer_->getCloudPointSize(iter.key()))));
+						}
+					}
 					cloudViewer_->removeAllClouds();
 					cloudViewer_->removeOctomap();
 					cloudViewer_->removeElevationMap();
@@ -5189,6 +5211,10 @@ void DatabaseViewer::update(int value,
 							pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(laserScanRaw, laserScanRaw.localTransform());
 							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
 						}
+						if(colorIndexAndPointSizeMap.find("scan") != colorIndexAndPointSizeMap.end()) {
+							cloudViewer_->setCloudColorIndex("scan", colorIndexAndPointSizeMap.at("scan").first);
+							cloudViewer_->setCloudPointSize("scan", colorIndexAndPointSizeMap.at("scan").second);
+						}
 					}

 					// add RGB-D cloud
@@ -5275,6 +5301,10 @@ void DatabaseViewer::update(int value,
 								}

 								cloudViewer_->addCloud("cloud", cloudValidPoints, pose);
+								if(colorIndexAndPointSizeMap.find("cloud") != colorIndexAndPointSizeMap.end()) {
+									cloudViewer_->setCloudColorIndex("cloud", colorIndexAndPointSizeMap.at("cloud").first);
+									cloudViewer_->setCloudPointSize("cloud", colorIndexAndPointSizeMap.at("cloud").second);
+								}
 							}
 							else
 							{
@@ -5398,7 +5428,12 @@ void DatabaseViewer::update(int value,
 										}
 										if(ui_->checkBox_showCloud->isChecked())
 										{
-											cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
+											std::string cloudName = uFormat("cloud_%d", i);
+											cloudViewer_->addCloud(cloudName, cloud, pose);
+											if(colorIndexAndPointSizeMap.find(cloudName) != colorIndexAndPointSizeMap.end()) {
+												cloudViewer_->setCloudColorIndex(cloudName, colorIndexAndPointSizeMap.at(cloudName).first);
+												cloudViewer_->setCloudPointSize(cloudName, colorIndexAndPointSizeMap.at(cloudName).second);
+											}
 										}
 									}
 								}
@@ -5428,7 +5463,12 @@ void DatabaseViewer::update(int value,
 											cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
 										}

-										cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
+										std::string cloudName = uFormat("cloud_%d", i);
+										cloudViewer_->addCloud(cloudName, cloud, pose);
+										if(colorIndexAndPointSizeMap.find(cloudName) != colorIndexAndPointSizeMap.end()) {
+											cloudViewer_->setCloudColorIndex(cloudName, colorIndexAndPointSizeMap.at(cloudName).first);
+											cloudViewer_->setCloudPointSize(cloudName, colorIndexAndPointSizeMap.at(cloudName).second);
+										}
 									}
 								}
 							}
@@ -109,6 +109,10 @@ public:
 		_value = value;
 	}

+	void setToolTipInfo(const QString & info) {
+		_info = info;
+	}
+
 	void setRadius(float radius)
 	{
 		float r,p,yaw;
@@ -160,6 +164,10 @@ protected:
 		{
 			msg += QString("\n%1=%2").arg(_valueName).arg(_value);
 		}
+		if(!_info.isEmpty())
+		{
+			msg += QString("\n%1").arg(_info);
+		}

 		this->setToolTip(msg);
 		
@@ -181,6 +189,7 @@ private:
 	QGraphicsLineItem * _line;
 	QString _valueName;
 	float _value;
+	QString _info;
 };

 class NodeGPSItem: public NodeItem
@@ -522,6 +531,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 		}
 		iter.value()->hide();
 		iter.value()->setColor(color); // reset color
+		iter.value()->setToolTipInfo(QString());
 		iter.value()->setZValue(iter.key()<0?21:20);
 	}
 	for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter)
@@ -558,7 +568,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				item->setZValue(iter->first<0?21:20);
 				item->setColor(color);
 				item->setParentItem(_graphRoot);
-				item->setVisible(_nodeVisible);
+				item->show();
 				_nodeItems.insert(iter->first, item);
 			}
 		}
@@ -567,33 +577,40 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 	for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 	{
 		// make the first id the smallest one
-		int idFrom = iter->first<iter->second.to()?iter->first:iter->second.to();
-		int idTo = iter->first<iter->second.to()?iter->second.to():iter->first;
+		int idFrom = iter->second.from() < iter->second.to() ? iter->second.from() : iter->second.to();
+		int idTo = iter->second.from() < iter->second.to() ? iter->second.to() : iter->second.from();
+
+		if(idFrom == idTo) {
+			continue;
+		}

 		std::map<int, Transform>::const_iterator jterA = poses.find(idFrom);
 		std::map<int, Transform>::const_iterator jterB = poses.find(idTo);
 		LinkItem * linkItem = 0;
 		if(jterA != poses.end() && jterB != poses.end() &&
-		   _nodeItems.contains(iter->first) && _nodeItems.contains(idTo))
+		   _nodeItems.contains(idFrom) && _nodeItems.contains(idTo))
 		{
 			const Transform & poseA = jterA->second;
 			const Transform & poseB = jterB->second;

 			QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.end();
+
 			if(_linkItems.contains(idFrom))
 			{
-				itemIter = _linkItems.find(iter->first);
-				while(itemIter.key() == idFrom && itemIter != _linkItems.end())
+				itemIter = _linkItems.find(idFrom);
+				bool alreadyAdded = false;
+				while(itemIter != _linkItems.end() && itemIter.key() == idFrom)
 				{
-					if(itemIter.value()->to() == idTo && itemIter.value()->type() == iter->second.type())
+					if(itemIter.value()->to() == idTo && itemIter.value()->isVisible())
 					{
-						itemIter.value()->setPoses(poseA, poseB, _viewPlane);
-						itemIter.value()->show();
-						linkItem = itemIter.value();
+						alreadyAdded = true;
 						break;
 					}
 					++itemIter;
 				}
+				if(alreadyAdded){
+					continue;
+				}
 			}

 			bool interSessionClosure = false;
@@ -732,6 +749,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 		}
 		else
 		{
+			iter.value()->setVisible(_nodeVisible);
 			++iter;
 		}
 	}
@@ -1166,6 +1184,19 @@ void GraphViewer::setCurrentGoalID(int id, const Transform & pose)
 	}
 }

+void GraphViewer::setNodeInfo(int id, const QString & info)
+{
+	NodeItem * node = _nodeItems.value(id, 0);
+	if(node)
+	{
+		node->setToolTipInfo(info);
+	}
+	else
+	{
+		UWARN("Node %d not found in the graph", id);
+	}
+}
+
 void GraphViewer::setLocalRadius(float radius)
 {
 	_localRadius->setRect(-radius*100, -radius*100, radius*200, radius*200);
@@ -266,14 +266,16 @@ ImageView::ImageView(QWidget * parent) :
 	_colorMapBlueToRed = colorMap->addAction(tr("Blue to red"));
 	_colorMapBlueToRed->setCheckable(true);
 	_colorMapBlueToRed->setChecked(false);
-	_colorMapMinRange = colorMap->addAction(tr("Min Range..."));
-	_colorMapMaxRange = colorMap->addAction(tr("Max Range..."));
+	_colorMapInCameraFrame = colorMap->addAction(tr("Camera Frame"));
+	_colorMapInCameraFrame->setCheckable(true);
+	_colorMapInCameraFrame->setChecked(true);
+	_colorMapMinRange = colorMap->addAction(tr("Min Z..."));
+	_colorMapMaxRange = colorMap->addAction(tr("Max Z..."));
 	group = new QActionGroup(this);
 	group->addAction(_colorMapWhiteToBlack);
 	group->addAction(_colorMapBlackToWhite);
 	group->addAction(_colorMapRedToBlue);
 	group->addAction(_colorMapBlueToRed);
-	group->addAction(_colorMapMaxRange);
 	_mouseTracking = _menu->addAction(tr("Show pixel depth"));
 	_mouseTracking->setCheckable(true);
 	_mouseTracking->setChecked(false);
@@ -311,6 +313,7 @@ void ImageView::saveSettings(QSettings & settings, const QString & group) const
 	settings.setValue("graphics_view_scale", this->isGraphicsViewScaled());
 	settings.setValue("graphics_view_scale_to_height", this->isGraphicsViewScaledToHeight());
 	settings.setValue("colormap", _colorMapWhiteToBlack->isChecked()?0:_colorMapBlackToWhite->isChecked()?1:_colorMapRedToBlue->isChecked()?2:3);
+	settings.setValue("colormap_camera_frame", this->isDepthColorMapInCameraFrame());
 	settings.setValue("colormap_min_range", this->getDepthColorMapMinRange());
 	settings.setValue("colormap_max_range", this->getDepthColorMapMaxRange());
 	if(!group.isEmpty())
@@ -345,6 +348,7 @@ void ImageView::loadSettings(QSettings & settings, const QString & group)
 	_colorMapBlackToWhite->setChecked(colorMap==1);
 	_colorMapRedToBlue->setChecked(colorMap==2);
 	_colorMapBlueToRed->setChecked(colorMap==3);
+	this->setDepthColorMapInCameraFrame(settings.value("colormap_camera_frame", this->isDepthColorMapInCameraFrame()).toBool());
 	this->setDepthColorMapRange(
 			settings.value("colormap_min_range", this->getDepthColorMapMinRange()).toFloat(),
 			settings.value("colormap_max_range", settings.value("colormap_range" /*backward compatibility*/, this->getDepthColorMapMaxRange())).toFloat());
@@ -763,13 +767,20 @@ void ImageView::setBackgroundColor(const QColor & color)
 	}
 }

+void ImageView::setDepthColorMapInCameraFrame(bool enabled) {
+	_colorMapInCameraFrame->setChecked(enabled);
+}
+
+bool ImageView::isDepthColorMapInCameraFrame() const {
+	return _colorMapInCameraFrame->isChecked();
+}
+
 void ImageView::setDepthColorMapRange(float min, float max)
 {
 	_depthColorMapMinRange = min;
 	_depthColorMapMaxRange = max;
 }

-
 void ImageView::computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const
 {
 	scale = 1.0f;
@@ -1054,10 +1065,17 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 		}
 		Q_EMIT configChanged();
 	}
+	else if(action == _colorMapInCameraFrame)
+	{
+		if(!_imageDepthCv.empty()) {
+			this->setImageDepth(_imageDepthCv, _imageDepthConfidenceCv);
+		}
+		Q_EMIT configChanged();
+	}
 	else if(action == _colorMapMinRange)
 	{
 		bool ok = false;
-		double value = QInputDialog::getDouble(this, tr("Set depth colormap min range"), tr("Range (m), 0=no limit"), _depthColorMapMinRange, 0, 9999, 1, &ok);
+		double value = QInputDialog::getDouble(this, tr("Set depth colormap min range"), tr("Range (m), 0=no limit"), _depthColorMapMinRange, -9999, 9999, 2, &ok);
 		if(ok)
 		{
 			this->setDepthColorMapRange(value, _depthColorMapMaxRange);
@@ -1070,7 +1088,7 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 	else if(action == _colorMapMaxRange)
 	{
 		bool ok = false;
-		double value = QInputDialog::getDouble(this, tr("Set depth colormap max range"), tr("Range (m), 0=no limit"), _depthColorMapMaxRange, 0, 9999, 1, &ok);
+		double value = QInputDialog::getDouble(this, tr("Set depth colormap max range"), tr("Range (m), 0=no limit"), _depthColorMapMaxRange, -9999, 9999, 2, &ok);
 		if(ok)
 		{
 			this->setDepthColorMapRange(_depthColorMapMinRange, value);
@@ -1350,8 +1368,61 @@ void ImageView::setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageD
 {
 	_imageDepthCv = imageDepth;
 	_imageDepthConfidenceCv = imageDepthConfidence;
+
+	QImage depth;
+	if(!_imageDepthCv.empty() && (_imageDepthCv.type() == CV_16UC1 || _imageDepthCv.type() == CV_32FC1)) {
+		if(_colorMapInCameraFrame->isChecked() || _models.empty() || !_models[0].isValidForProjection()) {
+			depth = uCvMat2QImage(_imageDepthCv, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange);
+			if(!_colorMapInCameraFrame->isChecked()) {
+				UWARN("Trying to set depth color map in base frame but the the camera model "
+					  "is not valid for projection, showing depth in camera frame instead.");
+			}
+		}
+		else {
+			// convert the depth values in height values
+			cv::Mat depthInBaseFrame = _imageDepthCv.clone();
+			int subImageWidth = _imageDepthCv.cols / _models.size();
+			if(depthInBaseFrame.type() == CV_16UC1) {
+				for(int v=0; v<depthInBaseFrame.rows; ++v){
+					unsigned short * rowPtr = depthInBaseFrame.ptr<unsigned short>(v);
+					for(int u=0; u<depthInBaseFrame.cols; ++u){
+						unsigned short & val = rowPtr[u];
+						if(val > 0) {
+							cv::Point3f pt;
+							int cameraIndex = u/subImageWidth;
+							UASSERT(cameraIndex>=0 && cameraIndex < (int)_models.size() && subImageWidth == _models[cameraIndex].imageWidth());
+							_models[cameraIndex].project(u,v,float(val)/1000.0f, pt.x, pt.y, pt.z);
+							pt = util3d::transformPoint(pt, _models[cameraIndex].localTransform());
+							val = (unsigned short)(pt.z*1000.0f);
+						}
+					}
+				}
+			}
+			else { // CV_32FC1
+				for(int v=0; v<depthInBaseFrame.rows; ++v){
+					float * rowPtr = depthInBaseFrame.ptr<float>(v);
+					for(int u=0; u<depthInBaseFrame.cols; ++u){
+						float & val = rowPtr[u];
+						if(val > 0) {
+							cv::Point3f pt;
+							int cameraIndex = u/subImageWidth;
+							UASSERT(cameraIndex>=0 && cameraIndex < (int)_models.size() && subImageWidth == _models[cameraIndex].imageWidth());
+							_models[cameraIndex].project(u,v,val, pt.x, pt.y, pt.z);
+							pt = util3d::transformPoint(pt, _models[cameraIndex].localTransform());
+							val = pt.z;
+						}
+					}
+				}
+			}
+			depth = uCvMat2QImage(depthInBaseFrame, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange);
+		}
+	}
+	else {
+		// right image grayscale or color
+		depth = uCvMat2QImage(_imageDepthCv, true, uCvQtDepthBlackToWhite);
+	}
 	setImageDepth(
-		uCvMat2QImage(_imageDepthCv, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange),
+		depth,
 		uCvMat2QImage(_imageDepthConfidenceCv, true, getDepthColorMap()));
 }

@@ -5878,8 +5878,11 @@ void MainWindow::startDetection()
 		}
 	}

-	if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase &&
-	   camera && camera->odomProvided())
+	if((_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase ||
+	    _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
+	    _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
+	    _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages) &&
+	    camera && camera->odomProvided())
 	{
 		odomSensor = camera;
 	}
@@ -8690,11 +8693,13 @@ void MainWindow::changeState(MainWindow::State newState)

 		if(_sensorCapture)
 		{
-			_sensorCapture->start();
 			if(_imuThread)
 			{
 				_imuThread->start();
+				// give imu thread a head start
+				uSleep(10);
 			}
+			_sensorCapture->start();
 			ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
 		}
 		break;
@@ -8726,11 +8731,13 @@ void MainWindow::changeState(MainWindow::State newState)

 			if(_sensorCapture)
 			{
-				_sensorCapture->start();
 				if(_imuThread)
 				{
 					_imuThread->start();
+					// give imu thread a head start
+					uSleep(10);
 				}
+				_sensorCapture->start();
 				ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
 			}
 		}
@@ -27222,13 +27222,13 @@ Lower the ratio -&gt; higher the precision.</string>
                 <item>
                  <widget class="QGroupBox" name="groupBox_detector_superpoint_torch2">
                   <property name="title">
-                    <string>SupertPoint Torch</string>
+                    <string>SuperPoint Torch</string>
                   </property>
                   <layout class="QVBoxLayout" name="verticalLayout_147">
                    <item>
                     <widget class="QLabel" name="label_575">
                      <property name="text">
-                       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;SupertPoint c++ implementation from &lt;a href=&quot;https://github.com/KinglittleQ/SuperPoint_SLAM&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;SuperPoint_SLAM&lt;/span&gt;&lt;/a&gt; project based on pytorch. A &lt;span style=&quot; font-weight:600;&quot;&gt;superpoint.pt&lt;/span&gt; weights file can be downloaded from its Git. It should be the same weights file &lt;a href=&quot;https://pytorch.org/tutorials/advanced/cpp_export.html&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;converted&lt;/span&gt;&lt;/a&gt; from &lt;span style=&quot; font-weight:600;&quot;&gt;superpoint.pth&lt;/span&gt; of the &lt;a href=&quot;https://github.com/magicleap/SuperPointPretrainedNetwork&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;SuperPointPretrainedNetwork&lt;/span&gt;&lt;/a&gt; project.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;SuperPoint c++ implementation from &lt;a href=&quot;https://github.com/KinglittleQ/SuperPoint_SLAM&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;SuperPoint_SLAM&lt;/span&gt;&lt;/a&gt; project based on pytorch. A &lt;span style=&quot; font-weight:600;&quot;&gt;superpoint.pt&lt;/span&gt; weights file can be downloaded from its Git. It should be the same weights file &lt;a href=&quot;https://pytorch.org/tutorials/advanced/cpp_export.html&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;converted&lt;/span&gt;&lt;/a&gt; from &lt;span style=&quot; font-weight:600;&quot;&gt;superpoint.pth&lt;/span&gt; of the &lt;a href=&quot;https://github.com/magicleap/SuperPointPretrainedNetwork&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;SuperPointPretrainedNetwork&lt;/span&gt;&lt;/a&gt; project.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                      </property>
                      <property name="wordWrap">
                       <bool>true</bool>
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
  <name>rtabmap</name>
-  <version>0.22.0</version>
+  <version>0.22.1</version>
  <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
  <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
Author	SHA1	Message	Date
matlabbe	d11aa0a881	Merge branch 'master' of github.com:introlab/rtabmap into humble-devel	2025-07-12 10:38:39 -07:00
matlabbe	fa6deba7df	bump 0.22.1	2025-07-12 10:11:55 -07:00
matlabbe	d34962f908	ImageView: Depth color map in base frame (#1553 ) * ImageView: Depth colormap in base frame option. CloudViewer: added min/max range options for XYZ axes coloring. * CloudViewer: saving rendering options of the 3D view, re-apply current color index when moving through nodes. * reset color inverted when reset	2025-07-12 10:07:47 -07:00
matlabbe	e5655b0b96	Fixed #1549	2025-07-06 23:08:47 +00:00
matlabbe	14d270241c	Added Rolling dev container	2025-07-06 15:51:49 -07:00
matlabbe	0579d4f39c	ImageView: fixed right grayscale image shown with depth colormap	2025-07-04 20:43:06 -07:00
matlabbe	2e71831324	Added libceres-dev dep in CI (#1547 ) * Added libceres-dev dep in CI * trying to fix 22.04 apt install * added g2o and gtam deps * removed libgtsam-dev because missing libCppUnitLite.a * fixing support of libg2o-dev on ubuntu 24.04 * update cmake	2025-06-30 21:32:27 -07:00
HO4X	2fdd2337c7	TASK: Remove typo in ceres optimizer branch (#1544 )	2025-06-30 10:39:53 -07:00
matlabbe	963ba42a1a	OdometryThread: Updated logic on how to sync imu and sensor data (to make sure odom has always all IMUs it needs prior to process images)	2025-06-29 17:00:12 -07:00
matlabbe	c9292bea5b	GUI: Fixed provided odometry ignored with CameraImages, CameraRGBDImages and CameraStereoImage types	2025-06-28 20:23:34 -07:00
matlabbe	30962119cb	GraphViewer: fixed duplicated links added to view	2025-06-26 13:30:51 -07:00
matlabbe	ee98be9ef7	Updated base android image (bionic->noble), updated android dev container	2025-06-20 22:38:05 +00:00
matlabbe	0536ddf0c9	Fixed loadOptimizedMeshQuery() when the TextureMesh has more than 1 texture. (#1538 )	2025-06-17 20:16:53 -07:00
matlabbe	6e439d2342	GraphView: fixed nodes not added when updating the graph when "Hide Nodes" option is enabled.	2025-06-16 09:45:19 -07:00
matlabbe	4a37855526	CI: update AliceVision patch (#1536 )	2025-06-15 17:00:09 -07:00
Petar Velev	db59344007	Fixed a typo with SuperPoint (#1535 )	2025-06-15 12:39:52 -07:00
matlabbe	105ac3bf9e	focal: fixing docker-deps	2025-06-11 15:52:05 -07:00
matlabbe	471ed6ade2	GraphViewer: added setNodeInfo() function	2025-06-11 13:55:45 -07:00
matlabbe	bc9da10589	removed not used distro on that brnach	2025-06-08 16:12:19 -07:00
matlabbe	f365a96446	merged master->humble-devel	2025-06-08 15:54:39 -07:00
matlabbe	6df70269c5	Disabled grid_map dep for humble (#1450 )	2025-02-12 19:30:59 -08:00
matlabbe	a952034604	Merge branch 'master' of github.com:introlab/rtabmap into humble-devel	2025-02-12 19:28:50 -08:00
matlabbe	bd99ac1007	merged master	2024-11-30 19:49:10 -08:00
matlabbe	0a9f0bfe4d	CI: only build humble in humble-devel branch	2024-11-30 18:32:29 -08:00