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

merged master to velodyne

Browse Source
This commit is contained in:
matlabbe
2015-11-26 13:37:37 -05:00
parent 7d521819f0 ee81bfc1d7
commit ba6733411a
3 changed files with 18 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -95,6 +95,7 @@ public:
const Memory * getMemory() const {return _memory;} const Memory * getMemory() const {return _memory;}
float getGoalReachedRadius() const {return _goalReachedRadius;} float getGoalReachedRadius() const {return _goalReachedRadius;}
float getLocalRadius() const {return _localRadius;} float getLocalRadius() const {return _localRadius;}
const Transform & getLastLocalizationPose() const {return _lastLocalizationPose;}
float getTimeThreshold() const {return _maxTimeAllowed;} // in ms float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
void setTimeThreshold(float maxTimeAllowed); // in ms void setTimeThreshold(float maxTimeAllowed); // in ms
@@ -232,7 +233,7 @@ private:
std::map<int, Transform> _optimizedPoses; std::map<int, Transform> _optimizedPoses;
std::multimap<int, Link> _constraints; std::multimap<int, Link> _constraints;
Transform _mapCorrection; Transform _mapCorrection;
Transform _lastLocalizationPose; // for localization mode Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
int _lastLocalizationNodeId; // for localization mode int _lastLocalizationNodeId; // for localization mode
// Planning stuff // Planning stuff

4
corelib/src/Rtabmap.cpp
View File

@@ -1067,7 +1067,7 @@ bool Rtabmap::process(
// Update Poses and Constraints // Update Poses and Constraints
_optimizedPoses.insert(std::make_pair(signature->id(), newPose)); _optimizedPoses.insert(std::make_pair(signature->id(), newPose));
_lastLocalizationPose = newPose; // used in localization mode only (path planning) _lastLocalizationPose = newPose; // keep in cache the latest corrected pose
if(signature->getLinks().size() == 1 && if(signature->getLinks().size() == 1 &&
signature->getLinks().begin()->second.type() == Link::kNeighbor) signature->getLinks().begin()->second.type() == Link::kNeighbor)
{ {
@@ -2131,7 +2131,7 @@ bool Rtabmap::process(
// Update map correction, it should be identify when optimizing from the last node // Update map correction, it should be identify when optimizing from the last node
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse(); _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update in case we switch to localization mode _lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd) if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
{ {
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str()); UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());

19
guilib/src/MainWindow.cpp
View File

@@ -773,6 +773,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
if(_ui->dockWidget_cloudViewer->isVisible()) if(_ui->dockWidget_cloudViewer->isVisible())
{ {
bool cloudUpdated = false;
bool scanUpdated = false;
if(!pose.isNull()) if(!pose.isNull())
{ {
// 3d cloud // 3d cloud
@@ -798,11 +800,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true); _ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1)); _ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1)); _ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
}
else cloudUpdated = true;
{
UWARN("Empty cloudOdom!");
_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", false);
} }
} }
@@ -827,8 +826,18 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true); _ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1)); _ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1)); _ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
scanUpdated = true;
} }
} }
if(!cloudUpdated && _ui->widget_cloudViewer->getAddedClouds().contains("cloudOdom"))
{
_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", false);
}
if(!scanUpdated && _ui->widget_cloudViewer->getAddedClouds().contains("scanOdom"))
{
_ui->widget_cloudViewer->setCloudVisibility("scanOdom", false);
}
} }
if(!odom.pose().isNull()) if(!odom.pose().isNull())