i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +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:40:08 -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
corelib/src/Graph.cpp
+1 -1
View File
@@ -1603,7 +1603,7 @@ int findNearestNode(
kdTree->nearestKSearch(pt, 1, ind, dist); kdTree->nearestKSearch(pt, 1, ind, dist);
if(ind.size() && dist.size() && ind[0] >= 0) if(ind.size() && dist.size() && ind[0] >= 0)
{ {
UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]); //UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
id = ids[ind[0]]; id = ids[ind[0]];
} }
} }
corelib/src/Memory.cpp
+43 -24
View File
@@ -2406,6 +2406,18 @@ Transform Memory::computeIcpTransformMulti(
UASSERT(uContains(poses, toId) && uContains(_signatures, toId)); UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
UDEBUG("Guess=%s", (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str()); UDEBUG("Guess=%s", (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
if(ULogger::level() == ULogger::kDebug)
{
std::string ids;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first != fromId)
{
ids += uNumber2Str(iter->first) + " ";
}
}
UDEBUG("%d vs %s", fromId, ids.c_str());
}
// make sure that all laser scans are loaded // make sure that all laser scans are loaded
std::list<Signature*> depthToLoad; std::list<Signature*> depthToLoad;
@@ -2438,6 +2450,7 @@ Transform Memory::computeIcpTransformMulti(
std::string msg; std::string msg;
int maxPoints = fromScan.cols; int maxPoints = fromScan.cols;
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
bool is2D = true; bool is2D = true;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{ {
@@ -2454,14 +2467,26 @@ Transform Memory::computeIcpTransformMulti(
{ {
is2D = false; is2D = false;
} }
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
scan, if(scan.channels() >= 5)
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second); {
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormal = util3d::laserScanToPointCloudNormal(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
*assembledToNormalClouds += *cloudNormal;
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
*assembledToClouds += *cloud;
}
if(scan.cols > maxPoints) if(scan.cols > maxPoints)
{ {
maxPoints = scan.cols; maxPoints = scan.cols;
} }
*assembledToClouds += *cloud;
} }
} }
else else
@@ -2470,28 +2495,22 @@ Transform Memory::computeIcpTransformMulti(
} }
} }
} }
if(assembledToClouds->size())
{
if(is2D)
{
assembledData.setLaserScanRaw(
util3d::laserScan2dFromPointCloud(*assembledToClouds),
LaserScanInfo(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
fromS->sensorData().laserScanInfo().maxRange(),
Transform::getIdentity())); // scans are in base frame
} cv::Mat assembledScan;
else if(assembledToNormalClouds->size())
{ {
assembledData.setLaserScanRaw( assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
util3d::laserScanFromPointCloud(*assembledToClouds),
LaserScanInfo(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
fromS->sensorData().laserScanInfo().maxRange(),
Transform::getIdentity())); // scans are in base frame
}
} }
else if(assembledToClouds->size())
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
}
// scans are in base frame but for 2d scans, set the height so that correspondences matching works
assembledData.setLaserScanRaw(assembledScan,
LaserScanInfo(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
fromS->sensorData().laserScanInfo().maxRange(),
is2D?Transform(0,0,fromS->sensorData().laserScanInfo().localTransform().z(),0,0,0):Transform::getIdentity()));
Transform guess = poses.at(fromId).inverse() * poses.at(toId); Transform guess = poses.at(fromId).inverse() * poses.at(toId);
std::vector<int> inliersV; std::vector<int> inliersV;
corelib/src/RegistrationIcp.cpp
+3 -3
View File
@@ -425,8 +425,8 @@ Transform RegistrationIcp::computeTransformationImpl(
if( _pointToPlane && if( _pointToPlane &&
_voxelSize == 0.0f && _voxelSize == 0.0f &&
fromScan.channels() >= 6 && fromScan.channels() >= 5 &&
toScan.channels() >= 6 && toScan.channels() >= 5 &&
!((fromScan.channels() == 5 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D) !((fromScan.channels() == 5 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D)
{ {
//special case if we have already normals computed and there is no filtering //special case if we have already normals computed and there is no filtering
@@ -454,8 +454,8 @@ Transform RegistrationIcp::computeTransformationImpl(
PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP); PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)"); UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
T = icp(data, ref); T = icp(data, ref);
UDEBUG("libpointmatcher icp...done!");
icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4)))); icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio(); float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
UDEBUG("match ratio: %f", matchRatio); UDEBUG("match ratio: %f", matchRatio);
corelib/src/Rtabmap.cpp
+7 -9
View File
@@ -1228,9 +1228,7 @@ bool Rtabmap::process(
//============================================================ //============================================================
if(_proximityByTime && if(_proximityByTime &&
rehearsedId == 0 && // don't do it if rehearsal happened rehearsedId == 0 && // don't do it if rehearsal happened
signature->getWords3().size() &&
_memory->isIncremental() && // don't do it in localization mode _memory->isIncremental() && // don't do it in localization mode
!signature->isBadSignature() &&
signature->getWeight()>=0) signature->getWeight()>=0)
{ {
const std::set<int> & stm = _memory->getStMem(); const std::set<int> & stm = _memory->getStMem();
@@ -1511,7 +1509,7 @@ bool Rtabmap::process(
++immunizedGlobally; ++immunizedGlobally;
} }
UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second); //UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
} }
neighbors.erase(iter++); neighbors.erase(iter++);
} }
@@ -1557,7 +1555,7 @@ bool Rtabmap::process(
{ {
++nbDirectNeighborsInDb; ++nbDirectNeighborsInDb;
} }
UDEBUG("nt=%d m=%d", iter->first, iter->second); //UDEBUG("nt=%d m=%d", iter->first, iter->second);
} }
neighbors.erase(iter++); neighbors.erase(iter++);
} }
@@ -1698,7 +1696,7 @@ bool Rtabmap::process(
{ {
++immunizedLocally; ++immunizedLocally;
} }
UDEBUG("local node %d on path immunized=1", iter->first); //UDEBUG("local node %d on path immunized=1", iter->first);
} }
} }
} }
@@ -1745,7 +1743,7 @@ bool Rtabmap::process(
{ {
++immunizedLocally; ++immunizedLocally;
} }
UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first); //UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
} }
} }
} }
@@ -2006,7 +2004,7 @@ bool Rtabmap::process(
//find the nearest pose on the path //find the nearest pose on the path
int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id())); int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
UASSERT(nearestId > 0); UASSERT(nearestId > 0);
UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId))); //UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
// nearest pose must be close and not linked to current location // nearest pose must be close and not linked to current location
if(!signature->hasLink(nearestId) && if(!signature->hasLink(nearestId) &&
@@ -2101,7 +2099,7 @@ bool Rtabmap::process(
} }
else else
{ {
UDEBUG("Path %d ignored", nearestId); //UDEBUG("Path %d ignored", nearestId);
} }
} }
} }
@@ -2996,7 +2994,7 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transfo
if(valid) if(valid)
{ {
UDEBUG("%d <- %d", nearestId, jter->first); //UDEBUG("%d <- %d", nearestId, jter->first);
path.insert(*jter); path.insert(*jter);
poses.erase(jter); poses.erase(jter);
} }
guilib/src/CloudViewer.cpp
+1 -1
View File
@@ -496,7 +496,7 @@ bool CloudViewer::updateCloudPose(
{ {
if(_addedClouds.contains(id)) 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; bool samePose = _addedClouds.find(id).value() == pose;
Eigen::Affine3f posef = pose.toEigen3f(); Eigen::Affine3f posef = pose.toEigen3f();
if(samePose || if(samePose ||
guilib/src/DatabaseViewer.cpp
+12 -12
View File
@@ -4020,19 +4020,19 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
UINFO("rotational_min=%f", rotational_min); UINFO("rotational_min=%f", rotational_min);
UINFO("rotational_max=%f", rotational_max); UINFO("rotational_max=%f", rotational_max);
ui_->toolBox_statistics->updateStat("GT/translational_rmse/", translational_rmse, 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 mean/", translational_mean, false);
ui_->toolBox_statistics->updateStat("GT/translational_median/", translational_median, 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 std/", translational_std, false);
ui_->toolBox_statistics->updateStat("GT/translational_min/", translational_min, 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 max/", translational_max, false);
ui_->toolBox_statistics->updateStat("GT/rotational_rmse/", rotational_rmse, 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 mean/", rotational_mean, false);
ui_->toolBox_statistics->updateStat("GT/rotational_median/", rotational_median, 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 std/", rotational_std, false);
ui_->toolBox_statistics->updateStat("GT/rotational_min/", rotational_min, 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 max/", rotational_max, false);
} }
} }
guilib/src/GraphViewer.cpp
+1 -1
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()); std::map<int,float>::const_iterator jter = posterior.find(iter.key());
if(jter != posterior.end()) 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 iter.value()->setColor(QColor::fromHsvF((1-jter->second/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
} }
else else
guilib/src/MainWindow.cpp
+67 -52
View File
@@ -3309,35 +3309,35 @@ Transform MainWindow::alignPosesToGroundTruth(
if((_preferencesDialog->isTimeUsedInFigures() && stamp > 0.0) || (refId && refId>=0)) 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 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 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 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 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 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 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 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 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 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 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 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 max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_max, _preferencesDialog->isCacheSavedInFigures());
} }
else else
{ {
_ui->statsToolBox->updateStat("GT/translational_rmse/", translational_rmse, _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 mean/", translational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_median/", translational_median, _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 std/", translational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_min/", translational_min, _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 max/", translational_max, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_rmse/", rotational_rmse, _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 mean/", rotational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_median/", rotational_median, _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 std/", rotational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_min/", rotational_min, _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 max/", rotational_max, _preferencesDialog->isCacheSavedInFigures());
} }
} }
} }
@@ -3348,43 +3348,58 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
{ {
UINFO("Update visibility %d", nodeId); UINFO("Update visibility %d", nodeId);
QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds(); 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); pose = _currentGTPosesMap.at(nodeId);
if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId) && _currentPosesMap.find(nodeId) != _currentPosesMap.end()) }
{ else if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
createAndAddCloudToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1)); {
} pose = _currentPosesMap.at(nodeId);
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);
}
} }
if(_preferencesDialog->isScansShown(0)) if(!pose.isNull() || !visible)
{ {
std::string scanName = uFormat("scan%d", nodeId); if(_preferencesDialog->isCloudsShown(0))
if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId) && _currentPosesMap.find(nodeId) != _currentPosesMap.end())
{ {
createAndAddScanToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1)); std::string cloudName = uFormat("cloud%d", nodeId);
} if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId))
else if(viewerClouds.contains(scanName))
{
if(visible && _currentPosesMap.find(nodeId) != _currentPosesMap.end())
{ {
//make sure the transformation was done createAndAddCloudToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1));
_cloudViewer->updateCloudPose(scanName, _currentPosesMap.find(nodeId)->second); }
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() void MainWindow::updateGraphView()