i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 00:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:introlab/rtabmap into gtest

Browse Source
This commit is contained in:
matlabbe
2025-05-03 11:47:56 -07:00
parent bb36ce7b8b 6b5b1c9617
commit 30b9f20bbd
13 changed files with 413 additions and 365 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -390,6 +390,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(RGBD, MaxOdomCacheSize, int, 10, uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
RTABMAP_PARAM(RGBD, LocalizationSmoothing, bool, true, uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).", kRGBDMaxOdomCacheSize().c_str()));
RTABMAP_PARAM(RGBD, LocalizationPriorError, double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).", kRGBDMaxOdomCacheSize().c_str()));
RTABMAP_PARAM(RGBD, LocalizationSecondTryWithoutProximityLinks, bool, true, uFormat("When localization is rejected by graph optimization validation, try a second time without proximity links if landmark or loop closure links are also present in odometry cache (see %s). If it succeeds, the proximity links are removed. This assumes that global loop closure and landmark links are more accurate than proximity links.", kRGBDMaxOdomCacheSize().c_str()));
// Local/Proximity loop closure detection
RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM.");
corelib/include/rtabmap/core/Rtabmap.h
+1
View File
@@ -333,6 +333,7 @@ private:
int _maxOdomCacheSize;
bool _localizationSmoothing;
double _localizationPriorInf;
bool _localizationSecondTryWithoutProximityLinks;
bool _createGlobalScanMap;
float _markerPriorsLinearVariance;
float _markerPriorsAngularVariance;
corelib/include/rtabmap/core/Statistics.h
+1
View File
@@ -81,6 +81,7 @@ class RTABMAP_CORE_EXPORT Statistics
RTABMAP_STATS(Loop, Landmark_detected_node_ref,);
RTABMAP_STATS(Loop, Visual_inliers_mean_dist,m);
RTABMAP_STATS(Loop, Visual_inliers_distribution,);
RTABMAP_STATS(Loop, Proximity_links_cleared,);
//Odom correction
RTABMAP_STATS(Loop, Odom_correction_norm, m);
RTABMAP_STATS(Loop, Odom_correction_angle, deg);
corelib/src/DBReader.cpp
+9 -3
View File
@@ -780,13 +780,19 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
}
else if(!combinedLocalTransforms.empty())
{
// We are overriding the camra local transforms, let's move 3D words accordingly
// We are overriding the camera local transforms, let's move 3D words accordingly
UASSERT(dbModels.size() == combinedLocalTransforms.size());
std::vector<cv::Point3f> newKeypoints3D;
UASSERT(dbModels[0].imageWidth()>0);
int subImageWidth = dbModels[0].imageWidth();
for(size_t i = 0; i<keypoints3D.size(); ++i)
{
cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[i].localTransform().inverse());
pt = util3d::transformPoint(pt, combinedLocalTransforms[i]);
int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)dbModels.size(),
uFormat("cameraIndex=%d, db models=%d, kpt.x=%f, image width=%d",
cameraIndex, (int)dbModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[cameraIndex].localTransform().inverse());
pt = util3d::transformPoint(pt, combinedLocalTransforms[cameraIndex]);
newKeypoints3D.push_back(pt);
}
data.setFeatures(keypoints, newKeypoints3D, descriptors);
corelib/src/Rtabmap.cpp
+21 -4
View File
@@ -152,6 +152,7 @@ Rtabmap::Rtabmap() :
_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
_localizationSmoothing(Parameters::defaultRGBDLocalizationSmoothing()),
_localizationPriorInf(1.0/(Parameters::defaultRGBDLocalizationPriorError()*Parameters::defaultRGBDLocalizationPriorError())),
_localizationSecondTryWithoutProximityLinks(Parameters::defaultRGBDLocalizationSecondTryWithoutProximityLinks()),
_createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
_markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()),
_markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()),
@@ -632,6 +633,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError);
UASSERT(localizationPriorError>0.0);
_localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError);
Parameters::parse(parameters, Parameters::kRGBDLocalizationSecondTryWithoutProximityLinks(), _localizationSecondTryWithoutProximityLinks);
Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
@@ -3181,6 +3183,7 @@ bool Rtabmap::process(
int optimizationIterations = 0;
Transform previousMapCorrection;
bool delayedLocalization = false;
int odomCacheProximityLinksCleared = 0;
UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
UDEBUG("Incremental: %d", _memory->isIncremental());
UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first);
@@ -3299,7 +3302,7 @@ bool Rtabmap::process(
if(!posesOut.empty() &&
posesOut.begin()->first < _odomCachePoses.begin()->first)
{
optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance, 0, &optimizationError, &optimizationIterations);
}
else
{
@@ -3424,7 +3427,8 @@ bool Rtabmap::process(
}
bool hasGlobalLoopClosuresOrLandmarks = false;
if(rejectLocalization && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty())
if(rejectLocalization &&
(_localizationSecondTryWithoutProximityLinks && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty()))
{
// Let's try again without local loop closures
localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure);
@@ -3448,7 +3452,7 @@ bool Rtabmap::process(
if(!posesOut.empty() &&
posesOut.begin()->first < _odomCachePoses.begin()->first)
{
optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance, 0, &optimizationError, &optimizationIterations);
}
else
{
@@ -3584,13 +3588,14 @@ bool Rtabmap::process(
_odomCacheConstraints = graph::filterLinks(_odomCacheConstraints, Link::kLocalSpaceClosure);
if(before != _odomCacheConstraints.size())
{
UWARN("Successfully optimized without local loop closures! Clear them from local odometry cache. %ld/%ld have been removed.",
UWARN("Successfully optimized without local loop closures! Clearing them from local odometry cache. %ld/%ld have been removed.",
before - _odomCacheConstraints.size(), before);
}
else
{
UWARN("Successfully optimized without local loop closures!");
}
odomCacheProximityLinksCleared = before - _odomCacheConstraints.size();
}
// Count how many localization links are in the constraints
@@ -3645,6 +3650,14 @@ bool Rtabmap::process(
if(hadAlreadyLocalizationLinks || _maxOdomCacheSize == 0)
{
UINFO("Update localization");
// update odomCachePoses with optimized poses (but make sure to put them back in odom frame)
Transform mapToOdomCache = signature->getPose() * newOptPoseInv;
for(std::map<int, Transform>::iterator iter = _odomCachePoses.begin(); iter!=_odomCachePoses.end(); ++iter)
{
iter->second = mapToOdomCache * optPoses.at(iter->first);
}
if(_optimizeFromGraphEnd)
{
// update all previous nodes
@@ -4134,6 +4147,10 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopMapToBase_yaw(), yaw*180.0f/M_PI);
UINFO("Localization pose = %s", _lastLocalizationPose.prettyPrint().c_str());
if(_localizationSecondTryWithoutProximityLinks) {
statistics_.addStatistic(Statistics::kLoopProximity_links_cleared(), (float)odomCacheProximityLinksCleared);
}
if(_localizationCovariance.total()==36)
{
double varLin = _graphOptimizer->isSlam2d()?
corelib/src/camera/CameraDepthAI.cpp
+27 -23
View File
@@ -843,34 +843,38 @@ SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
std::vector<cv::Point> kpts;
cv::findNonZero(scores > threshold_, kpts);
std::vector<cv::KeyPoint> keypoints;
for(auto& kpt : kpts)
{
float response = scores.at<float>(kpt);
keypoints.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
}
cv::Mat coarse_desc(25, 40, CV_32FC(256), local_descriptor_map.data());
if(detectFeatures_ == 2)
coarse_desc.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
if(!kpts.empty()){
std::vector<cv::KeyPoint> keypoints;
for(auto& kpt : kpts)
{
float response = scores.at<float>(kpt);
keypoints.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
}
cv::Mat coarse_desc(25, 40, CV_32FC(256), local_descriptor_map.data());
if(detectFeatures_ == 2)
coarse_desc.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
cv::normalize(descriptor, descriptor);
});
cv::Mat mapX(keypoints.size(), 1, CV_32FC1);
cv::Mat mapY(keypoints.size(), 1, CV_32FC1);
for(size_t i=0; i<keypoints.size(); ++i)
{
mapX.at<float>(i) = (keypoints[i].pt.x - (targetSize_.width-1)/2) * 40/targetSize_.width + (40-1)/2;
mapY.at<float>(i) = (keypoints[i].pt.y - (targetSize_.height-1)/2) * 25/targetSize_.height + (25-1)/2;
}
cv::Mat map1, map2, descriptors;
cv::convertMaps(mapX, mapY, map1, map2, CV_16SC2);
cv::remap(coarse_desc, descriptors, map1, map2, cv::INTER_LINEAR);
descriptors.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
cv::normalize(descriptor, descriptor);
});
cv::Mat mapX(keypoints.size(), 1, CV_32FC1);
cv::Mat mapY(keypoints.size(), 1, CV_32FC1);
for(size_t i=0; i<keypoints.size(); ++i)
{
mapX.at<float>(i) = (keypoints[i].pt.x - (targetSize_.width-1)/2) * 40/targetSize_.width + (40-1)/2;
mapY.at<float>(i) = (keypoints[i].pt.y - (targetSize_.height-1)/2) * 25/targetSize_.height + (25-1)/2;
descriptors = descriptors.reshape(1);
data.setFeatures(keypoints, std::vector<cv::Point3f>(), descriptors);
}
cv::Mat map1, map2, descriptors;
cv::convertMaps(mapX, mapY, map1, map2, CV_16SC2);
cv::remap(coarse_desc, descriptors, map1, map2, cv::INTER_LINEAR);
descriptors.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
cv::normalize(descriptor, descriptor);
});
descriptors = descriptors.reshape(1);
data.setFeatures(keypoints, std::vector<cv::Point3f>(), descriptors);
if(detectFeatures_ == 3)
data.addGlobalDescriptor(GlobalDescriptor(1, cv::Mat(1, global_descriptor.size(), CV_32FC1, global_descriptor.data()).clone()));
}
corelib/src/util3d_surface.cpp
+1 -4
View File
@@ -781,10 +781,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
tm.setMaxDistance(maxDistance);
tm.setMaxAngle(maxAngle);
if(maxDepthError > 0.0f)
{
tm.setMaxDepthError(maxDepthError);
}
tm.setMaxDepthError(maxDepthError);
tm.setMinClusterSize(minClusterSize);
if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels, distanceToCamPolicy))
{