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

DBReader: added ignore priors option (added to gui and reprocess), Statistics: added info about distance/angle of the localization, RegVis: fixed inliers mean distance on multicam setup, About: added MRPT info

Browse Source
This commit is contained in:
matlabbe
2023-02-10 17:29:10 -08:00
parent 59d220c54f
commit 437ed09335
11 changed files with 1250 additions and 1158 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/include/rtabmap/core/DBReader.h
View File

@@ -57,7 +57,8 @@ public:
bool landmarksIgnored = false,
bool featuresIgnored = false,
int startMapId = 0,
int stopMapId = -1);
int stopMapId = -1,
bool priorsIgnored = false);
DBReader(const std::list<std::string> & databasePaths,
float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
bool odometryIgnored = false,
@@ -70,7 +71,8 @@ public:
bool landmarksIgnored = false,
bool featuresIgnored = false,
int startMapId = 0,
int stopMapId = -1);
int stopMapId = -1,
bool priorsIgnored = false);
virtual ~DBReader();
virtual bool init(
@@ -100,6 +102,7 @@ private:
bool _intermediateNodesIgnored;
bool _landmarksIgnored;
bool _featuresIgnored;
bool _priorsIgnored;
int _startMapId;
int _stopMapId;

1
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -368,7 +368,6 @@ private:
std::map<int, Transform> _globalScanMapPoses;
std::map<int, Transform> _odomCachePoses; // used in localization mode to reject loop closures
std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
std::vector<float> _odomCorrectionAcc;
std::map<int, Transform> _markerPriors;
std::set<int> _nodesToRepublish;

13
corelib/include/rtabmap/core/Statistics.h
View File

@@ -67,7 +67,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Visual_inliers,);
RTABMAP_STATS(Loop, Visual_inliers_ratio,);
RTABMAP_STATS(Loop, Visual_matches,);
RTABMAP_STATS(Loop, Distance_since_last_loc,);
RTABMAP_STATS(Loop, Distance_since_last_loc, m);
RTABMAP_STATS(Loop, Last_id,);
RTABMAP_STATS(Loop, Optimization_max_error, m);
RTABMAP_STATS(Loop, Optimization_max_error_ratio, );
@@ -90,15 +90,6 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Odom_correction_roll, deg);
RTABMAP_STATS(Loop, Odom_correction_pitch, deg);
RTABMAP_STATS(Loop, Odom_correction_yaw, deg);
//Odom correction
RTABMAP_STATS(Loop, Odom_correction_acc_norm, m);
RTABMAP_STATS(Loop, Odom_correction_acc_angle, deg);
RTABMAP_STATS(Loop, Odom_correction_acc_x, m);
RTABMAP_STATS(Loop, Odom_correction_acc_y, m);
RTABMAP_STATS(Loop, Odom_correction_acc_z, m);
RTABMAP_STATS(Loop, Odom_correction_acc_roll, deg);
RTABMAP_STATS(Loop, Odom_correction_acc_pitch, deg);
RTABMAP_STATS(Loop, Odom_correction_acc_yaw, deg);
// Map to Odom
RTABMAP_STATS(Loop, MapToOdom_norm, m);
RTABMAP_STATS(Loop, MapToOdom_angle, deg);
@@ -160,6 +151,8 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Memory, RAM_usage, MB);
RTABMAP_STATS(Memory, RAM_estimated, MB);
RTABMAP_STATS(Memory, Triangulated_points, );
RTABMAP_STATS(Memory, Closest_node_distance, m);
RTABMAP_STATS(Memory, Closest_node_angle, rad);
RTABMAP_STATS(Timing, Memory_update, ms);
RTABMAP_STATS(Timing, Neighbor_link_refining, ms);

37
corelib/src/DBReader.cpp
View File

@@ -54,7 +54,8 @@ DBReader::DBReader(const std::string & databasePath,
bool landmarksIgnored,
bool featuresIgnored,
int startMapId,
int stopMapId) :
int stopMapId,
bool priorsIgnored) :
Camera(frameRate),
_paths(uSplit(databasePath, ';')),
_odometryIgnored(odometryIgnored),
@@ -66,6 +67,7 @@ DBReader::DBReader(const std::string & databasePath,
_intermediateNodesIgnored(intermediateNodesIgnored),
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
_priorsIgnored(priorsIgnored),
_startMapId(startMapId),
_stopMapId(stopMapId),
_dbDriver(0),
@@ -98,7 +100,8 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
bool landmarksIgnored,
bool featuresIgnored,
int startMapId,
int stopMapId) :
int stopMapId,
bool priorsIgnored) :
Camera(frameRate),
_paths(databasePaths),
_odometryIgnored(odometryIgnored),
@@ -110,6 +113,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
_intermediateNodesIgnored(intermediateNodesIgnored),
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
_priorsIgnored(priorsIgnored),
_startMapId(startMapId),
_stopMapId(stopMapId),
_dbDriver(0),
@@ -408,21 +412,24 @@ SensorData DBReader::getNextData(CameraInfo * info)
cv::Mat globalPoseCov;
std::multimap<int, Link> priorLinks;
_dbDriver->loadLinks(*_currentId, priorLinks, Link::kPosePrior);
if( priorLinks.size() &&
!priorLinks.begin()->second.transform().isNull() &&
priorLinks.begin()->second.infMatrix().cols == 6 &&
priorLinks.begin()->second.infMatrix().rows == 6)
if(!_priorsIgnored)
{
globalPose = priorLinks.begin()->second.transform();
globalPoseCov = priorLinks.begin()->second.infMatrix().inv();
if(data.gps().stamp() != 0.0 &&
globalPoseCov.at<double>(3,3)>=9999 &&
globalPoseCov.at<double>(4,4)>=9999 &&
globalPoseCov.at<double>(5,5)>=9999)
_dbDriver->loadLinks(*_currentId, priorLinks, Link::kPosePrior);
if( priorLinks.size() &&
!priorLinks.begin()->second.transform().isNull() &&
priorLinks.begin()->second.infMatrix().cols == 6 &&
priorLinks.begin()->second.infMatrix().rows == 6)
{
// clear global pose as GPS was used for prior
globalPose.setNull();
globalPose = priorLinks.begin()->second.transform();
globalPoseCov = priorLinks.begin()->second.infMatrix().inv();
if(data.gps().stamp() != 0.0 &&
globalPoseCov.at<double>(3,3)>=9999 &&
globalPoseCov.at<double>(4,4)>=9999 &&
globalPoseCov.at<double>(5,5)>=9999)
{
// clear global pose as GPS was used for prior
globalPose.setNull();
}
}
}

40
corelib/src/RegistrationVis.cpp
View File

@@ -2021,29 +2021,31 @@ Transform RegistrationVis::computeTransformationImpl(
}
for(unsigned int i=0; i<allInliers.size(); ++i)
{
if(_maxInliersMeanDistance>0.0f)
std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]);
if(wordsIter != toSignature.getWords().end() && !toSignature.getWordsKpts().empty())
{
std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
if(wordsIter != fromSignature.getWords().end() && !fromSignature.getWords3().empty())
{
const cv::Point3f & pt = fromSignature.getWords3()[wordsIter->second];
if(uIsFinite(pt.x))
{
distances.push_back(util3d::transformPoint(pt, transformInv).x);
}
}
}
if(!pcaData.empty())
{
std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]);
UASSERT(wordsIter != fromSignature.getWords().end() && !toSignature.getWordsKpts().empty());
float * ptr = pcaData.ptr<float>(i, 0);
const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second];
int cameraIndex = (int)(kpt.pt.x / cameraModelsTo[0].imageWidth());
UASSERT_MSG(cameraIndex < (int)cameraModelsTo.size(), uFormat("cameraIndex=%d (x=%f models=%d camera width = %d)", cameraIndex, kpt.pt.x, (int)cameraModelsTo.size(), cameraModelsTo[0].imageWidth()).c_str());
ptr[0] = (kpt.pt.x-cameraIndex*cameraModelsTo[cameraIndex].imageWidth()-cameraModelsTo[cameraIndex].cx()) / cameraModelsTo[cameraIndex].imageWidth();
ptr[1] = (kpt.pt.y-cameraModelsTo[cameraIndex].cy()) / cameraModelsTo[cameraIndex].imageHeight();
if(_maxInliersMeanDistance>0.0f && !toSignature.getWords3().empty())
{
const cv::Point3f & pt = toSignature.getWords3()[wordsIter->second];
if(util3d::isFinite(pt))
{
UASSERT(cameraModelsTo[cameraIndex].isValidForProjection());
float depth = util3d::transformPoint(pt, cameraModelsTo[cameraIndex].localTransform().inverse()).z;
distances.push_back(depth);
}
}
if(!pcaData.empty())
{
float * ptr = pcaData.ptr<float>(i, 0);
ptr[0] = (kpt.pt.x-cameraIndex*cameraModelsTo[cameraIndex].imageWidth()-cameraModelsTo[cameraIndex].cx()) / cameraModelsTo[cameraIndex].imageWidth();
ptr[1] = (kpt.pt.y-cameraModelsTo[cameraIndex].cy()) / cameraModelsTo[cameraIndex].imageHeight();
}
}
}

70
corelib/src/Rtabmap.cpp
View File

@@ -167,7 +167,6 @@ Rtabmap::Rtabmap() :
_mapCorrection(Transform::getIdentity()),
_lastLocalizationNodeId(0),
_currentSessionHasGPS(false),
_odomCorrectionAcc(6,0),
_pathStatus(0),
_pathCurrentIndex(0),
_pathGoalIndex(0),
@@ -471,7 +470,6 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
_lastLocalizationNodeId = 0;
_odomCachePoses.clear();
_odomCacheConstraints.clear();
_odomCorrectionAcc = std::vector<float>(6,0);
_distanceTravelled = 0.0f;
_distanceTravelledSinceLastLocalization = 0.0f;
_optimizeFromGraphEndChanged = false;
@@ -855,7 +853,6 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
_lastLocalizationNodeId = 0;
_odomCachePoses.clear();
_odomCacheConstraints.clear();
_odomCorrectionAcc = std::vector<float>(6,0);
_mapCorrection.setIdentity();
_mapCorrectionBackup.setNull();
@@ -882,7 +879,6 @@ int Rtabmap::triggerNewMap()
_lastLocalizationNodeId = 0;
_odomCachePoses.clear();
_odomCacheConstraints.clear();
_odomCorrectionAcc = std::vector<float>(6,0);
_distanceTravelled = 0.0f;
_distanceTravelledSinceLastLocalization = 0.0f;
@@ -1066,7 +1062,6 @@ void Rtabmap::resetMemory()
_lastLocalizationNodeId = 0;
_odomCachePoses.clear();
_odomCacheConstraints.clear();
_odomCorrectionAcc = std::vector<float>(6,0);
_distanceTravelled = 0.0f;
_distanceTravelledSinceLastLocalization = 0.0f;
_optimizeFromGraphEndChanged = false;
@@ -1436,6 +1431,8 @@ bool Rtabmap::process(
std::list<int> signaturesRemoved;
bool neighborLinkRefined = false;
bool addedNewLandmark = false;
float distanceToClosestNodeInTheGraph = 0;
float angleToClosestNodeInTheGraph = 0;
if(_rgbdSlamMode)
{
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
@@ -1617,6 +1614,28 @@ bool Rtabmap::process(
newPose = _mapCorrection * signature->getPose();
}
// Get statistics about the closest node in the graph
if(!_memory->isIncremental())
{
int closestNode = 0;
float sqrdDistance = 0.0f;
if(_optimizedPoses.begin()->first < 0)
{
std::map<int, Transform> poses(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
closestNode = graph::findNearestNode(poses, newPose, &sqrdDistance);
}
else
{
closestNode = graph::findNearestNode(_optimizedPoses, newPose, &sqrdDistance);
}
if(closestNode>0 && sqrdDistance>0.0f)
{
distanceToClosestNodeInTheGraph = sqrt(sqrdDistance);
UDEBUG("Last localization pose = %s, closest node=%d (%f m)", newPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph);
angleToClosestNodeInTheGraph = (newPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
}
}
UDEBUG("Added pose %s (odom=%s)", newPose.prettyPrint().c_str(), signature->getPose().prettyPrint().c_str());
// Update Poses and Constraints
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
@@ -3661,6 +3680,17 @@ bool Rtabmap::process(
}
previousMapCorrection = _mapCorrection;
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
// Update statistics about the closest node in the graph using the actual loop closure
if(!_memory->isIncremental() && !_lastLocalizationPose.isNull())
{
int closestNode = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
if(closestNode>0)
{
distanceToClosestNodeInTheGraph = _lastLocalizationPose.getDistance(_optimizedPoses.at(closestNode));
UDEBUG("Last localization pose = %s, updated closest node=%d (%f m)", _lastLocalizationPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph);
angleToClosestNodeInTheGraph = (_lastLocalizationPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
}
}
_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
if(_mapCorrection.getNormSquared() > 0.1f && _optimizeFromGraphEnd)
{
@@ -3838,30 +3868,6 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopOdom_correction_roll(), roll*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOdom_correction_pitch(), pitch*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOdom_correction_yaw(), yaw*180.0f/M_PI);
_odomCorrectionAcc[0]+=x;
_odomCorrectionAcc[1]+=y;
_odomCorrectionAcc[2]+=z;
_odomCorrectionAcc[3]+=roll;
_odomCorrectionAcc[4]+=pitch;
_odomCorrectionAcc[5]+=yaw;
Transform odomCorrectionAcc(
_odomCorrectionAcc[0],
_odomCorrectionAcc[1],
_odomCorrectionAcc[2],
_odomCorrectionAcc[3],
_odomCorrectionAcc[4],
_odomCorrectionAcc[5]);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_norm(), odomCorrectionAcc.getNorm());
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_angle(), odomCorrectionAcc.getAngle()*180.0f/M_PI);
odomCorrectionAcc.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_x(), x);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_y(), y);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_z(), z);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_roll(), roll*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_pitch(), pitch*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_yaw(), yaw*180.0f/M_PI);
}
}
if(!_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
@@ -3922,6 +3928,12 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0);
statistics_.addStatistic(Statistics::kMemoryNew_landmark(), addedNewLandmark?1.0f:0);
if(distanceToClosestNodeInTheGraph>0.0)
{
statistics_.addStatistic(Statistics::kMemoryClosest_node_distance(), distanceToClosestNodeInTheGraph);
statistics_.addStatistic(Statistics::kMemoryClosest_node_angle(), angleToClosestNodeInTheGraph);
}
if(_publishRAMUsage)
{
UTimer ramTimer;