i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-12 06:20:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added MRPT optional dependency. Updated localization_pose + its covariance to be always published.

Browse Source
This commit is contained in:
matlabbe
2023-02-03 17:25:38 -08:00
parent e4edee33eb
commit dcedd8bb6b
10 changed files with 120 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+30 -10
View File
@@ -183,6 +183,7 @@ option(WITH_G2O "Include g2o support" ON)
option(WITH_GTSAM "Include GTSAM support" ON)
option(WITH_TORO "Include TORO support" ON)
option(WITH_CERES "Include Ceres support" OFF)
option(WITH_MRPT "Include MRPT support" ON)
option(WITH_VERTIGO "Include Vertigo support" ON)
option(WITH_CVSBA "Include cvsba support" OFF)
option(WITH_POINTMATCHER "Include libpointmatcher support" ON)
@@ -477,6 +478,14 @@ IF(WITH_GTSAM)
FIND_PACKAGE(GTSAM CONFIG QUIET)
ENDIF(WITH_GTSAM)
IF(WITH_MRPT)
FIND_PACKAGE(MRPT COMPONENTS poses QUIET)
IF(MRPT_FOUND)
message(STATUS "MRPT_VERSION: ${MRPT_VERSION}")
message(STATUS "MRPT_LIBRARIES: ${MRPT_LIBRARIES}")
ENDIF(MRPT_FOUND)
ENDIF(WITH_MRPT)
IF(WITH_FLYCAPTURE2)
FIND_PACKAGE(FlyCapture2 QUIET)
IF(FlyCapture2_FOUND)
@@ -849,6 +858,9 @@ IF(NOT GTSAM_FOUND)
ELSE()
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
ENDIF()
IF(NOT MRPT_FOUND)
SET(MRPT "//")
ENDIF(NOT MRPT_FOUND)
IF(NOT CERES_FOUND)
SET(CERES "//")
ENDIF(NOT CERES_FOUND)
@@ -1318,7 +1330,7 @@ MESSAGE(STATUS " With FastCV = NO (FastCV not found)")
ENDIF()
IF(PDAL_FOUND)
MESSAGE(STATUS " With PDAL = YES (License: BSD)")
MESSAGE(STATUS " With PDAL ${PDAL_VERSION} = YES (License: BSD)")
ELSEIF(NOT WITH_PDAL)
MESSAGE(STATUS " With PDAL = NO (WITH_PDAL=OFF)")
ELSE()
@@ -1334,7 +1346,7 @@ MESSAGE(STATUS " With TORO = NO (WITH_TORO=OFF)")
ENDIF()
IF(G2O_FOUND)
MESSAGE(STATUS " *With g2o = YES (License: BSD)")
MESSAGE(STATUS " *With g2o ${g2o_VERSION} = YES (License: BSD)")
ELSEIF(NOT WITH_G2O)
MESSAGE(STATUS " *With g2o = NO (WITH_G2O=OFF)")
ELSE()
@@ -1342,7 +1354,7 @@ MESSAGE(STATUS " *With g2o = NO (g2o not found)")
ENDIF()
IF(GTSAM_FOUND)
MESSAGE(STATUS " *With GTSAM = YES (License: BSD)")
MESSAGE(STATUS " *With GTSAM ${GTSAM_VERSION} = YES (License: BSD)")
ELSEIF(NOT WITH_GTSAM)
MESSAGE(STATUS " *With GTSAM = NO (WITH_GTSAM=OFF)")
ELSE()
@@ -1351,9 +1363,9 @@ ENDIF()
IF(CERES_FOUND)
IF(WITH_CERES)
MESSAGE(STATUS " *With Ceres ${Ceres_VERSION} = YES (License: BSD)")
MESSAGE(STATUS " *With Ceres ${Ceres_VERSION} = YES (License: BSD)")
ELSE()
MESSAGE(STATUS " *With Ceres ${Ceres_VERSION} = YES (License: BSD, WITH_CERES=OFF but it is enabled by okvis or floam dependencies)")
MESSAGE(STATUS " *With Ceres ${Ceres_VERSION} = YES (License: BSD, WITH_CERES=OFF but it is enabled by okvis or floam dependencies)")
ENDIF()
ELSEIF(NOT WITH_CERES)
MESSAGE(STATUS " *With Ceres = NO (WITH_CERES=OFF)")
@@ -1361,6 +1373,14 @@ ELSE()
MESSAGE(STATUS " *With Ceres = NO (Ceres not found)")
ENDIF()
IF(MRPT_FOUND)
MESSAGE(STATUS " With MRPT ${MRPT_VERSION} = YES (License: BSD)")
ELSEIF(NOT WITH_MRPT)
MESSAGE(STATUS " With MRPT = NO (WITH_MRPT=OFF)")
ELSE()
MESSAGE(STATUS " With MRPT = NO (MRPT not found)")
ENDIF()
IF(G2O_FOUND OR GTSAM_FOUND)
IF(WITH_VERTIGO)
MESSAGE(STATUS " With VERTIGO = YES (License: GPLv3)")
@@ -1380,7 +1400,7 @@ MESSAGE(STATUS " With cvsba = NO (cvsba not found)")
ENDIF()
IF(libpointmatcher_FOUND)
MESSAGE(STATUS " *With libpointmatcher = YES (License: BSD)")
MESSAGE(STATUS " *With libpointmatcher ${libpointmatcher_VERSION} = YES (License: BSD)")
ELSEIF(NOT WITH_POINTMATCHER)
MESSAGE(STATUS " *With libpointmatcher = NO (WITH_POINTMATCHER=OFF)")
ELSE()
@@ -1406,7 +1426,7 @@ MESSAGE(STATUS " With Open3D = NO (Open3D not found)")
ENDIF()
IF(opengv_FOUND)
MESSAGE(STATUS " With OpenGV = YES (License: BSD)")
MESSAGE(STATUS " With OpenGV ${opengv_VERSION} = YES (License: BSD)")
ELSEIF(NOT WITH_OPENGV)
MESSAGE(STATUS " With OpenGV = NO (WITH_OPENGV=OFF)")
ELSE()
@@ -1416,7 +1436,7 @@ ENDIF()
MESSAGE(STATUS "")
MESSAGE(STATUS " Reconstruction Approaches:")
IF(octomap_FOUND)
MESSAGE(STATUS " With OCTOMAP = YES (License: BSD)")
MESSAGE(STATUS " With OCTOMAP ${octomap_VERSION} = YES (License: BSD)")
ELSEIF(NOT WITH_OCTOMAP)
MESSAGE(STATUS " With OCTOMAP = NO (WITH_OCTOMAP=OFF)")
ELSE()
@@ -1537,7 +1557,7 @@ MESSAGE(STATUS " With RealSense = NO (librealsense not found)")
ENDIF()
IF(realsense2_FOUND)
MESSAGE(STATUS " With RealSense2 = YES (License: Apache-2)")
MESSAGE(STATUS " With RealSense2 ${realsense2_VERSION} = YES (License: Apache-2)")
ELSEIF(NOT WITH_REALSENSE2)
MESSAGE(STATUS " With RealSense2 = NO (WITH_REALSENSE2=OFF)")
ELSE()
@@ -1553,7 +1573,7 @@ MESSAGE(STATUS " With MyntEyeS = NO (mynteye s sdk not found)")
ENDIF()
IF(depthai_FOUND)
MESSAGE(STATUS " With DepthAI = YES (License: MIT)")
MESSAGE(STATUS " With DepthAI ${depthai_VERSION} = YES (License: MIT)")
ELSEIF(NOT WITH_DEPTHAI)
MESSAGE(STATUS " With DepthAI = NO (WITH_DEPTHAI=OFF)")
ELSE()
Version.h.in
+1
View File
@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@
@GTSAM@#define RTABMAP_GTSAM
@CERES@#define RTABMAP_CERES
@MRPT@#define RTABMAP_MRPT
@VERTIGO@#define RTABMAP_VERTIGO
@OPENNI2@#define RTABMAP_OPENNI2
@FREENECT@#define RTABMAP_FREENECT
corelib/include/rtabmap/core/Rtabmap.h
+1
View File
@@ -361,6 +361,7 @@ private:
Transform _mapCorrectionBackup; // used in localization mode when odom is lost
Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
int _lastLocalizationNodeId; // for localization mode
cv::Mat _localizationCovariance;
std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
bool _currentSessionHasGPS;
LaserScan _globalScanMap;
corelib/include/rtabmap/core/Statistics.h
+2
View File
@@ -115,6 +115,8 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, MapToBase_roll, deg);
RTABMAP_STATS(Loop, MapToBase_pitch, deg);
RTABMAP_STATS(Loop, MapToBase_yaw, deg);
RTABMAP_STATS(Loop, MapToBase_lin_std, m);
RTABMAP_STATS(Loop, MapToBase_lin_var, m2);
RTABMAP_STATS(Proximity, Time_detections,);
RTABMAP_STATS(Proximity, Space_last_detection_id,);
corelib/src/CMakeLists.txt
+7
View File
@@ -433,6 +433,13 @@ IF(CERES_FOUND)
)
ENDIF(CERES_FOUND)
IF(MRPT_FOUND)
SET(LIBRARIES
${LIBRARIES}
${MRPT_LIBRARIES}
)
ENDIF(MRPT_FOUND)
IF(libpointmatcher_FOUND)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
corelib/src/Rtabmap.cpp
+69 -7
View File
@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/BayesFilter.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/Registration.h"
#include "rtabmap/core/RegistrationInfo.h"
#include <rtabmap/utilite/ULogger.h>
@@ -57,6 +58,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/PythonInterface.h"
#endif
#ifdef RTABMAP_MRPT
// Used for odometry error propagation
#include <mrpt/poses/CPose3DPDFGaussian.h>
#endif
#include <pcl/search/kdtree.h>
#include <pcl/filters/crop_box.h>
#include <pcl/io/pcd_io.h>
@@ -461,6 +467,7 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
_mapCorrection.setIdentity();
_mapCorrectionBackup.setNull();
_localizationCovariance = cv::Mat();
_lastLocalizationNodeId = 0;
_odomCachePoses.clear();
_odomCacheConstraints.clear();
@@ -844,6 +851,7 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
if(!_memory->isIncremental())
{
_lastLocalizationPose = initialPose;
_localizationCovariance = 0;
_lastLocalizationNodeId = 0;
_odomCachePoses.clear();
_odomCacheConstraints.clear();
@@ -870,6 +878,7 @@ int Rtabmap::triggerNewMap()
int mapId = -1;
if(_memory)
{
_localizationCovariance = cv::Mat();
_lastLocalizationNodeId = 0;
_odomCachePoses.clear();
_odomCacheConstraints.clear();
@@ -1053,6 +1062,7 @@ void Rtabmap::resetMemory()
_mapCorrection.setIdentity();
_mapCorrectionBackup.setNull();
_lastLocalizationPose.setNull();
_localizationCovariance = cv::Mat();
_lastLocalizationNodeId = 0;
_odomCachePoses.clear();
_odomCacheConstraints.clear();
@@ -1654,6 +1664,38 @@ bool Rtabmap::process(
_constraints.insert(std::make_pair(tmp.from(), tmp));
}
// Localization mode stuff
if( signature->getWeight() >= 0 &&
!smallDisplacement &&
odomCovariance.cols == 6 &&
odomCovariance.rows == 6 &&
odomCovariance.type() == CV_64FC1 &&
odomCovariance.at<double>(0,0) < 1)
{
if(_localizationCovariance.empty() || _lastLocalizationPose.isNull())
{
_localizationCovariance = odomCovariance.clone();
}
else
{
#ifdef RTABMAP_MRPT
// Transform odometry covariance (which in base frame) into global frame
// "odometry error propagation law"
Eigen::Quaterniond rotation = _lastLocalizationPose.getQuaterniond();
mrpt::poses::CPose3D pose = mrpt::poses::CPose3D::FromQuaternion(mrpt::math::CQuaternionDouble(rotation.w(), rotation.x(), rotation.y(), rotation.z()));
mrpt::math::CMatrixDouble66 gaussian;
gaussian.loadFromRawPointer((const double*)odomCovariance.data);
mrpt::poses::CPose3DPDFGaussian gaussianTransformed(mrpt::poses::CPose3D(), gaussian);
gaussianTransformed.changeCoordinatesReference(pose);
_localizationCovariance += cv::Mat(6,6,CV_64FC1, gaussianTransformed.cov.data());
#else
// Assuming diagonal uniform covariance matrix!
// If variance is different for each axis,
// build rtabmap with MRPT to use approach above.
_localizationCovariance += odomCovariance;
#endif
}
}
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
if(!_memory->isIncremental() && signature->getWeight() >= 0)
{
@@ -2971,7 +3013,6 @@ bool Rtabmap::process(
float maxAngularErrorRatio = 0.0f;
double optimizationError = 0.0;
int optimizationIterations = 0;
cv::Mat localizationCovariance;
Transform previousMapCorrection;
bool rejectedLandmark = false;
bool delayedLocalization = false;
@@ -3085,7 +3126,8 @@ bool Rtabmap::process(
_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
// If slam2d: get connected graph while keeping original roll,pitch,z values.
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
std::map<int, Transform> optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut);
cv::Mat locOptCovariance;
std::map<int, Transform> optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
_graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
{
@@ -3199,7 +3241,7 @@ bool Rtabmap::process(
_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
// If slam2d: get connected graph while keeping original roll,pitch,z values.
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut);
optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
_graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
{
@@ -3460,7 +3502,7 @@ bool Rtabmap::process(
}
_optimizedPoses.at(signature->id()) = newPose;
}
localizationCovariance = localizationLinks.rbegin()->second.infMatrix().inv();
_localizationCovariance = locOptCovariance.empty()?localizationLinks.rbegin()->second.infMatrix().inv():locOptCovariance;
}
else //delayed localization (wait for more than 1 link)
{
@@ -3607,7 +3649,7 @@ bool Rtabmap::process(
UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
_optimizedPoses = poses;
_constraints = constraints;
localizationCovariance = covariance;
_localizationCovariance = covariance;
}
}
@@ -3675,6 +3717,14 @@ bool Rtabmap::process(
refWordsCount = (int)signature->getWords().size();
refUniqueWordsCount = (int)uUniqueKeys(signature->getWords()).size();
if(_graphOptimizer->isSlam2d())
{
// set very small
_localizationCovariance.at<double>(2,2) = Registration::COVARIANCE_LINEAR_EPSILON;
_localizationCovariance.at<double>(3,3) = Registration::COVARIANCE_ANGULAR_EPSILON;
_localizationCovariance.at<double>(4,4) = Registration::COVARIANCE_ANGULAR_EPSILON;
}
// Posterior is empty if a bad signature is detected
float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
@@ -3737,6 +3787,8 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopId(), loopId);
statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1);
statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization);
float x,y,z,roll,pitch,yaw;
if(_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLandmark && !landmarksDetected.empty()))
{
@@ -3760,7 +3812,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
}
}
statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization);
_distanceTravelledSinceLastLocalization = 0.0f;
statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm());
@@ -3822,11 +3874,21 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopMapToBase_pitch(), pitch*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopMapToBase_yaw(), yaw*180.0f/M_PI);
UINFO("Localization pose = %s", _lastLocalizationPose.prettyPrint().c_str());
if(_localizationCovariance.total()==36)
{
double varLin = _graphOptimizer->isSlam2d()?
std::max(_localizationCovariance.at<double>(0,0), _localizationCovariance.at<double>(1,1)):
uMax3(_localizationCovariance.at<double>(0,0), _localizationCovariance.at<double>(1,1), _localizationCovariance.at<double>(2,2));
statistics_.addStatistic(Statistics::kLoopMapToBase_lin_std(), sqrt(varLin));
statistics_.addStatistic(Statistics::kLoopMapToBase_lin_var(), varLin);
}
}
statistics_.setMapCorrection(_mapCorrection);
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
statistics_.setLocalizationCovariance(localizationCovariance);
statistics_.setLocalizationCovariance(_localizationCovariance);
// timings...
statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
docker/bionic/Dockerfile
+1 -25
View File
@@ -4,7 +4,7 @@ FROM ros:melodic-perception
# Install build dependencies
RUN apt-get update && \
apt-get install -y git software-properties-common ros-melodic-rtabmap-ros && \
apt-get install -y git software-properties-common ros-melodic-rtabmap-ros libmrpt-dev && \
apt-get remove -y ros-melodic-rtabmap && \
rm -rf /var/lib/apt/lists/
@@ -14,30 +14,6 @@ WORKDIR /root/
RUN add-apt-repository ppa:borglab/gtsam-release-4.0 -y
RUN apt install libgtsam-dev libgtsam-unstable-dev -y
# libpointmatcher
RUN git clone https://github.com/ethz-asl/libnabo.git
#commit February 13 2021
RUN cd libnabo && \
git checkout 3cab7eed92bd5d4aed997347b8c8a2692a83a532 && \
mkdir build && \
cd build && \
cmake -DCMAKE_BUILD_TYPE=Release .. && \
make -j$(nproc) && \
make install && \
cd && \
rm -r libnabo
RUN git clone https://github.com/ethz-asl/libpointmatcher.git
#commit April 6 2021
RUN cd libpointmatcher && \
git checkout 76f99fce0fe69e6384102a0343fdf8d262626e1f && \
mkdir build && \
cd build && \
cmake -DCMAKE_BUILD_TYPE=Release .. && \
make -j$(nproc) && \
make install && \
cd && \
rm -r libpointmatcher
ARG TARGETPLATFORM
ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64}
RUN echo "I am building for $TARGETPLATFORM"
docker/focal-foxy/Dockerfile
+4
View File
@@ -12,6 +12,10 @@ WORKDIR /root/
# GTSAM
RUN add-apt-repository ppa:borglab/gtsam-release-4.0 -y
RUN apt install libgtsam-dev libgtsam-unstable-dev -y
# MRPT
RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y
RUN apt install libmrpt-poses-dev
# PDAL
RUN apt-get install -y libpdal-dev
docker/focal/Dockerfile
+4 -24
View File
@@ -14,30 +14,10 @@ WORKDIR /root/
RUN add-apt-repository ppa:borglab/gtsam-release-4.0 -y
RUN apt install libgtsam-dev libgtsam-unstable-dev -y
# libpointmatcher
RUN git clone https://github.com/ethz-asl/libnabo.git
#commit February 13 2021
RUN cd libnabo && \
git checkout 3cab7eed92bd5d4aed997347b8c8a2692a83a532 && \
mkdir build && \
cd build && \
cmake -DCMAKE_BUILD_TYPE=Release .. && \
make -j$(nproc) && \
make install && \
cd && \
rm -r libnabo
RUN git clone https://github.com/ethz-asl/libpointmatcher.git
#commit April 6 2021
RUN cd libpointmatcher && \
git checkout 76f99fce0fe69e6384102a0343fdf8d262626e1f && \
mkdir build && \
cd build && \
cmake -DCMAKE_BUILD_TYPE=Release .. && \
make -j$(nproc) && \
make install && \
cd && \
rm -r libpointmatcher
#MRPT
RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y
RUN apt install libmrpt-poses-dev
# PDAL
RUN apt-get install -y libpdal-dev
docker/jammy/Dockerfile
+1 -1
View File
@@ -4,7 +4,7 @@ FROM osrf/ros:humble-desktop
# Install build dependencies
RUN apt-get update && \
apt-get install -y git software-properties-common ros-humble-rtabmap-ros && \
apt-get install -y git software-properties-common ros-humble-rtabmap-ros libmrpt-poses-dev && \
apt-get remove -y ros-humble-rtabmap
WORKDIR /root/