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

fixed multi scans matching with normals, removed some debug logs

Browse Source
This commit is contained in:
matlabbe
2017-08-28 16:15:22 -04:00
parent 423b47a5ff
commit b70ffb6331
8 changed files with 135 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
guilib/src/CloudViewer.cpp
View File

@@ -496,7 +496,7 @@ bool CloudViewer::updateCloudPose(
{
if(_addedClouds.contains(id))
{
UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
//UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
bool samePose = _addedClouds.find(id).value() == pose;
Eigen::Affine3f posef = pose.toEigen3f();
if(samePose ||

24
guilib/src/DatabaseViewer.cpp
View File

@@ -4020,19 +4020,19 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
UINFO("rotational_min=%f", rotational_min);
UINFO("rotational_max=%f", rotational_max);
ui_->toolBox_statistics->updateStat("GT/translational_rmse/", translational_rmse, false);
ui_->toolBox_statistics->updateStat("GT/translational_mean/", translational_mean, false);
ui_->toolBox_statistics->updateStat("GT/translational_median/", translational_median, false);
ui_->toolBox_statistics->updateStat("GT/translational_std/", translational_std, false);
ui_->toolBox_statistics->updateStat("GT/translational_min/", translational_min, false);
ui_->toolBox_statistics->updateStat("GT/translational_max/", translational_max, false);
ui_->toolBox_statistics->updateStat("GT/translational rmse/", translational_rmse, false);
ui_->toolBox_statistics->updateStat("GT/translational mean/", translational_mean, false);
ui_->toolBox_statistics->updateStat("GT/translational median/", translational_median, false);
ui_->toolBox_statistics->updateStat("GT/translational std/", translational_std, false);
ui_->toolBox_statistics->updateStat("GT/translational min/", translational_min, false);
ui_->toolBox_statistics->updateStat("GT/translational max/", translational_max, false);
ui_->toolBox_statistics->updateStat("GT/rotational_rmse/", rotational_rmse, false);
ui_->toolBox_statistics->updateStat("GT/rotational_mean/", rotational_mean, false);
ui_->toolBox_statistics->updateStat("GT/rotational_median/", rotational_median, false);
ui_->toolBox_statistics->updateStat("GT/rotational_std/", rotational_std, false);
ui_->toolBox_statistics->updateStat("GT/rotational_min/", rotational_min, false);
ui_->toolBox_statistics->updateStat("GT/rotational_max/", rotational_max, false);
ui_->toolBox_statistics->updateStat("GT/rotational rmse/", rotational_rmse, false);
ui_->toolBox_statistics->updateStat("GT/rotational mean/", rotational_mean, false);
ui_->toolBox_statistics->updateStat("GT/rotational median/", rotational_median, false);
ui_->toolBox_statistics->updateStat("GT/rotational std/", rotational_std, false);
ui_->toolBox_statistics->updateStat("GT/rotational min/", rotational_min, false);
ui_->toolBox_statistics->updateStat("GT/rotational max/", rotational_max, false);
}
}

2
guilib/src/GraphViewer.cpp
View File

@@ -673,7 +673,7 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior)
std::map<int,float>::const_iterator jter = posterior.find(iter.key());
if(jter != posterior.end())
{
UDEBUG("id=%d max=%f hyp=%f color = %f", iter.key(), max, jter->second, (1-jter->second/max)*240.0f/360.0f);
//UDEBUG("id=%d max=%f hyp=%f color = %f", iter.key(), max, jter->second, (1-jter->second/max)*240.0f/360.0f);
iter.value()->setColor(QColor::fromHsvF((1-jter->second/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
}
else

119
guilib/src/MainWindow.cpp
View File

@@ -3309,35 +3309,35 @@ Transform MainWindow::alignPosesToGroundTruth(
if((_preferencesDialog->isTimeUsedInFigures() && stamp > 0.0) || (refId && refId>=0))
{
_ui->statsToolBox->updateStat("GT/translational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_max, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_max, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_max, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_max, _preferencesDialog->isCacheSavedInFigures());
}
else
{
_ui->statsToolBox->updateStat("GT/translational_rmse/", translational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_mean/", translational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_median/", translational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_std/", translational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_min/", translational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_max/", translational_max, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational rmse/", translational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational mean/", translational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational median/", translational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational std/", translational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational min/", translational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational max/", translational_max, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_rmse/", rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_mean/", rotational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_median/", rotational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_std/", rotational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_min/", rotational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_max/", rotational_max, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational rmse/", rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational mean/", rotational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational median/", rotational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational std/", rotational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational min/", rotational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational max/", rotational_max, _preferencesDialog->isCacheSavedInFigures());
}
}
}
@@ -3348,43 +3348,58 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
{
UINFO("Update visibility %d", nodeId);
QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
if(_preferencesDialog->isCloudsShown(0))
Transform pose;
if(_currentGTPosesMap.size() &&
_ui->actionAnchor_clouds_to_ground_truth->isChecked() &&
_currentGTPosesMap.find(nodeId)!=_currentGTPosesMap.end())
{
std::string cloudName = uFormat("cloud%d", nodeId);
if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId) && _currentPosesMap.find(nodeId) != _currentPosesMap.end())
{
createAndAddCloudToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1));
}
else if(viewerClouds.contains(cloudName))
{
if(visible && _currentPosesMap.find(nodeId) != _currentPosesMap.end())
{
//make sure the transformation was done
_cloudViewer->updateCloudPose(cloudName, _currentPosesMap.find(nodeId)->second);
}
_cloudViewer->setCloudVisibility(cloudName, visible);
}
pose = _currentGTPosesMap.at(nodeId);
}
else if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
{
pose = _currentPosesMap.at(nodeId);
}
if(_preferencesDialog->isScansShown(0))
if(!pose.isNull() || !visible)
{
std::string scanName = uFormat("scan%d", nodeId);
if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId) && _currentPosesMap.find(nodeId) != _currentPosesMap.end())
if(_preferencesDialog->isCloudsShown(0))
{
createAndAddScanToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1));
}
else if(viewerClouds.contains(scanName))
{
if(visible && _currentPosesMap.find(nodeId) != _currentPosesMap.end())
std::string cloudName = uFormat("cloud%d", nodeId);
if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId))
{
//make sure the transformation was done
_cloudViewer->updateCloudPose(scanName, _currentPosesMap.find(nodeId)->second);
createAndAddCloudToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1));
}
else if(viewerClouds.contains(cloudName))
{
if(visible)
{
//make sure the transformation was done
_cloudViewer->updateCloudPose(cloudName, pose);
}
_cloudViewer->setCloudVisibility(cloudName, visible);
}
_cloudViewer->setCloudVisibility(scanName, visible);
}
}
_cloudViewer->update();
if(_preferencesDialog->isScansShown(0))
{
std::string scanName = uFormat("scan%d", nodeId);
if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId))
{
createAndAddScanToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1));
}
else if(viewerClouds.contains(scanName))
{
if(visible)
{
//make sure the transformation was done
_cloudViewer->updateCloudPose(scanName, pose);
}
_cloudViewer->setCloudVisibility(scanName, visible);
}
}
_cloudViewer->update();
}
}
void MainWindow::updateGraphView()