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

3D Rendering: Added option to show graph's frustums, added option to filter floor and/or ceiling.

Browse Source
This commit is contained in:
matlabbe
2017-02-09 18:32:44 -05:00
parent e4629cc740
commit 0d0577a886
10 changed files with 1324 additions and 1048 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
guilib/include/rtabmap/gui/CloudViewer.h
View File

@@ -196,13 +196,15 @@ public:
void addOrUpdateFrustum(
const std::string & id,
const Transform & transform,
const Transform & localTransform,
double scale,
const QColor & color = QColor());
bool updateFrustumPose(
const std::string & id,
const Transform & pose);
void removeFrustum(const std::string & id);
void removeAllFrustums();
void removeAllFrustums(bool exceptCameraReference = false);
const QMap<std::string, Transform> & getAddedFrustums() const {return _frustums;}
void addOrUpdateGraph(
const std::string & id,
@@ -327,7 +329,7 @@ private:
std::set<std::string> _coordinates;
std::set<std::string> _texts;
std::set<std::string> _lines;
std::set<std::string> _frustums;
QMap<std::string, Transform> _frustums;
pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
unsigned int _maxTrajectorySize;
float _frustumScale;

1
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -296,6 +296,7 @@ private:
std::map<int, std::string> _currentLabels; // <nodeId, label>
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > _cachedClouds;
long _createdCloudsMemoryUsage;
std::set<int> _cachedEmptyClouds;
std::pair<int, std::pair<std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>, pcl::IndicesPtr> > _previousCloud; // used for subtraction
std::map<int, cv::Mat> _createdScans;

12
guilib/include/rtabmap/gui/PreferencesDialog.h
View File

@@ -155,10 +155,14 @@ public:
bool isGroundTruthAligned() const;
bool isGraphsShown() const;
bool isFrustumsShown() const;
bool isLabelsShown() const;
double getMapVoxel() const;
double getMapNoiseRadius() const;
int getMapNoiseMinNeighbors() const;
double getVoxel() const;
double getNoiseRadius() const;
int getNoiseMinNeighbors() const;
double getCeilingFilteringHeight() const;
double getFloorFilteringHeight() const;
int getNormalKSearch() const;
bool isCloudsShown(int index) const; // 0=map, 1=odom
bool isOctomapUpdated() const;
bool isOctomapShown() const;
@@ -190,8 +194,6 @@ public:
int getSubtractFilteringMinPts() const;
double getSubtractFilteringRadius() const;
double getSubtractFilteringAngle() const;
int getNormalKSearch() const;
bool gainCompensation() const;
bool getGridMapShown() const;
double getGridMapResolution() const;;

158
guilib/src/CloudViewer.cpp
View File

@@ -1237,6 +1237,7 @@ static const int frustum_indices[] = {
void CloudViewer::addOrUpdateFrustum(
const std::string & id,
const Transform & transform,
const Transform & localTransform,
double scale,
const QColor & color)
{
@@ -1246,46 +1247,55 @@ void CloudViewer::addOrUpdateFrustum(
return;
}
removeFrustum(id);
if(!transform.isNull())
{
_frustums.insert(id);
int frustumSize = sizeof(frustum_vertices)/sizeof(float);
UASSERT(frustumSize>0 && frustumSize % 3 == 0);
frustumSize/=3;
pcl::PointCloud<pcl::PointXYZ> frustumPoints;
frustumPoints.resize(frustumSize);
float scaleX = 0.5f * scale;
float scaleY = 0.4f * scale; //4x3 arbitrary ratio
float scaleZ = 0.3f * scale;
QColor c = Qt::gray;
if(color.isValid())
if(_frustums.find(id)==_frustums.end())
{
c = color;
}
Eigen::Affine3f t = transform.toEigen3f();
for(int i=0; i<frustumSize; ++i)
{
frustumPoints[i].x = frustum_vertices[i*3]*scaleX;
frustumPoints[i].y = frustum_vertices[i*3+1]*scaleY;
frustumPoints[i].z = frustum_vertices[i*3+2]*scaleZ;
frustumPoints[i] = pcl::transformPoint(frustumPoints[i], t);
}
_frustums.insert(id, Transform());
pcl::PolygonMesh mesh;
pcl::Vertices vertices;
vertices.vertices.resize(sizeof(frustum_indices)/sizeof(int));
for(unsigned int i=0; i<vertices.vertices.size(); ++i)
{
vertices.vertices[i] = frustum_indices[i];
}
pcl::toPCLPointCloud2(frustumPoints, mesh.cloud);
mesh.polygons.push_back(vertices);
_visualizer->addPolylineFromPolygonMesh(mesh, id);
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
int frustumSize = sizeof(frustum_vertices)/sizeof(float);
UASSERT(frustumSize>0 && frustumSize % 3 == 0);
frustumSize/=3;
pcl::PointCloud<pcl::PointXYZ> frustumPoints;
frustumPoints.resize(frustumSize);
float scaleX = 0.5f * scale;
float scaleY = 0.4f * scale; //4x3 arbitrary ratio
float scaleZ = 0.3f * scale;
QColor c = Qt::gray;
if(color.isValid())
{
c = color;
}
Transform opticalRotInv(0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0);
Eigen::Affine3f t = (localTransform*opticalRotInv).toEigen3f();
for(int i=0; i<frustumSize; ++i)
{
frustumPoints[i].x = frustum_vertices[i*3]*scaleX;
frustumPoints[i].y = frustum_vertices[i*3+1]*scaleY;
frustumPoints[i].z = frustum_vertices[i*3+2]*scaleZ;
frustumPoints[i] = pcl::transformPoint(frustumPoints[i], t);
}
pcl::PolygonMesh mesh;
pcl::Vertices vertices;
vertices.vertices.resize(sizeof(frustum_indices)/sizeof(int));
for(unsigned int i=0; i<vertices.vertices.size(); ++i)
{
vertices.vertices[i] = frustum_indices[i];
}
pcl::toPCLPointCloud2(frustumPoints, mesh.cloud);
mesh.polygons.push_back(vertices);
_visualizer->addPolylineFromPolygonMesh(mesh, id);
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
}
if(!this->updateFrustumPose(id, transform))
{
UERROR("Failed updating pose of frustum %s!?", id.c_str());
}
}
else
{
removeFrustum(id);
}
}
@@ -1293,10 +1303,37 @@ bool CloudViewer::updateFrustumPose(
const std::string & id,
const Transform & pose)
{
if(_frustums.find(id) != _frustums.end() && !pose.isNull())
QMap<std::string, Transform>::iterator iter=_frustums.find(id);
if(iter != _frustums.end() && !pose.isNull())
{
UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
return _visualizer->updateShapePose(id, pose.toEigen3f());
if(iter.value() == pose)
{
// same pose, just return
return true;
}
pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
vtkActor* actor;
if (am_it == _visualizer->getShapeActorMap()->end ())
return (false);
else
actor = vtkActor::SafeDownCast (am_it->second);
if (!actor)
return (false);
vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
pcl::visualization::PCLVisualizer::convertToVtkMatrix (pose.toEigen3f().matrix (), matrix);
actor->SetUserMatrix (matrix);
actor->Modified ();
iter.value() = pose;
return true;
}
return false;
}
@@ -1312,18 +1349,21 @@ void CloudViewer::removeFrustum(const std::string & id)
if(_frustums.find(id) != _frustums.end())
{
_visualizer->removeShape(id);
_frustums.erase(id);
_frustums.remove(id);
}
}
void CloudViewer::removeAllFrustums()
void CloudViewer::removeAllFrustums(bool exceptCameraReference)
{
std::set<std::string> frustums = _frustums;
for(std::set<std::string>::iterator iter = frustums.begin(); iter!=frustums.end(); ++iter)
QMap<std::string, Transform> frustums = _frustums;
for(QMap<std::string, Transform>::iterator iter = frustums.begin(); iter!=frustums.end(); ++iter)
{
this->removeFrustum(*iter);
if(!exceptCameraReference || !uStrContains(iter.key(), "reference_frustum"))
{
this->removeFrustum(iter.key());
}
}
UASSERT(_frustums.empty());
UASSERT(exceptCameraReference || _frustums.empty());
}
void CloudViewer::addOrUpdateGraph(
@@ -1486,12 +1526,12 @@ void CloudViewer::setFrustumShown(bool shown)
{
if(!shown)
{
std::set<std::string> frustumsCopy = _frustums;
for(std::set<std::string>::iterator iter=frustumsCopy.begin(); iter!=frustumsCopy.end(); ++iter)
QMap<std::string, Transform> frustumsCopy = _frustums;
for(QMap<std::string, Transform>::iterator iter=frustumsCopy.begin(); iter!=frustumsCopy.end(); ++iter)
{
if(uStrContains(*iter, "reference_frustum"))
if(uStrContains(iter.key(), "reference_frustum"))
{
this->removeFrustum(*iter);
this->removeFrustum(iter.key());
}
}
std::set<std::string> linesCopy = _lines;
@@ -1518,12 +1558,9 @@ void CloudViewer::setFrustumColor(QColor value)
{
value = Qt::gray;
}
for(std::set<std::string>::iterator iter=_frustums.begin(); iter!=_frustums.end(); ++iter)
for(QMap<std::string, Transform>::iterator iter=_frustums.begin(); iter!=_frustums.end(); ++iter)
{
if(uStrContains(*iter, "reference_frustum"))
{
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), *iter);
}
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), iter.key());
}
this->update();
_frustumColor = value;
@@ -1819,24 +1856,19 @@ void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector
{
if(!pose.isNull())
{
// commented: update pose is crashing...
/*if(_frustums.find("reference_frustum") != _frustums.end())
{
this->updateFrustumPose("reference_frustum", pose);
}
else */ if(_aShowFrustum->isChecked())
if(_aShowFrustum->isChecked())
{
Transform baseToCamera;
Transform opticalRot(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
for(unsigned int i=0; i<models.size(); ++i)
{
baseToCamera = Transform::getIdentity();
if(!models[i].localTransform().isNull() && !models[i].localTransform().isIdentity())
{
baseToCamera = models[i].localTransform()*opticalRot.inverse();
baseToCamera = models[i].localTransform();
}
this->addOrUpdateFrustum(uFormat("reference_frustum_%d", i), pose * baseToCamera, _frustumScale, _frustumColor);
std::string id = uFormat("reference_frustum_%d", i);
this->removeFrustum(id);
this->addOrUpdateFrustum(id, pose, baseToCamera, _frustumScale, _frustumColor);
if(!baseToCamera.isIdentity())
{
this->addOrUpdateLine(uFormat("reference_frustum_line_%d", i), pose, pose * baseToCamera, _frustumColor);

5
guilib/src/DepthCalibrationDialog.cpp
View File

@@ -355,11 +355,10 @@ void DepthCalibrationDialog::calibrate(
_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)...").arg(map->size()).arg(sequence.size()));
_progressDialog->incrementStep();
viewer->addCloud("map", map);
Transform opticalRot(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
for(std::map<int, SensorData>::iterator iter=sequence.begin(); iter!=sequence.end(); ++iter)
{
Transform baseToCamera = iter->second.cameraModels()[0].localTransform()*opticalRot.inverse();
viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first) * baseToCamera, 0.2);
Transform baseToCamera = iter->second.cameraModels()[0].localTransform();
viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2);
}
_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)... done.").arg(map->size()).arg(sequence.size()));

223
guilib/src/MainWindow.cpp
View File

@@ -1935,7 +1935,6 @@ void MainWindow::updateMapCloud(
if(update3dCloud)
{
// update cloud
std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud;
if(viewerClouds.contains(cloudName))
{
// Update only if the pose has changed
@@ -1952,9 +1951,11 @@ void MainWindow::updateMapCloud(
_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
}
else if(_cachedClouds.find(iter->first) == _cachedClouds.end() && _cachedSignatures.contains(iter->first))
else if(_cachedEmptyClouds.find(iter->first) == _cachedEmptyClouds.end() &&
_cachedClouds.find(iter->first) == _cachedClouds.end() &&
_cachedSignatures.contains(iter->first))
{
createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
if(_cloudViewer->getAddedClouds().contains(cloudName))
{
_cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
@@ -2145,36 +2146,100 @@ void MainWindow::updateMapCloud(
// update 3D graphes (show all poses)
_cloudViewer->removeAllGraphs();
_cloudViewer->removeCloud("graph_nodes");
if(_preferencesDialog->isGraphsShown() && _currentPosesMap.size())
if(!_preferencesDialog->isFrustumsShown())
{
_cloudViewer->removeAllFrustums(true);
}
if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown()) && _currentPosesMap.size())
{
UTimer timerGraph;
// Find all graphs
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
{
int mapId = uValue(_currentMapIds, iter->first, -1);
//edges
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
if(kter == graphs.end())
if(_preferencesDialog->isGraphsShown())
{
kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
//edges
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
if(kter == graphs.end())
{
kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
}
pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
kter->second->push_back(pt);
}
// get local transforms for frustums on the graph
if(_preferencesDialog->isFrustumsShown())
{
std::string frustumId = uFormat("f_%d_%d", mapId, iter->first);
if(_cloudViewer->getAddedFrustums().contains(frustumId))
{
_cloudViewer->updateFrustumPose(frustumId, iter->second);
}
else if(_cachedSignatures.contains(iter->first))
{
const Signature & s = _cachedSignatures.value(iter->first);
// Supporting only one frustum per node
if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
{
Transform t = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().localTransform():s.sensorData().cameraModels()[0].localTransform();
if(!t.isNull())
{
QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
_cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color);
}
}
}
}
pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
kter->second->push_back(pt);
}
//Ground truth graph?
for(std::map<int, Transform>::iterator iter=_currentGTPosesMap.begin(); iter!=_currentGTPosesMap.end(); ++iter)
{
int mapId = -100;
//edges
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
if(kter == graphs.end())
if(_preferencesDialog->isGraphsShown())
{
kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
//edges
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
if(kter == graphs.end())
{
kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
}
pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
kter->second->push_back(pt);
}
// get local transforms for frustums on the graph
if(_preferencesDialog->isFrustumsShown() && _cachedSignatures.contains(iter->first))
{
std::string frustumId = uFormat("f_gt_%d", iter->first);
QMap<std::string, Transform>::const_iterator jter=_cloudViewer->getAddedFrustums().find(frustumId);
if(jter != _cloudViewer->getAddedFrustums().end())
{
if(jter.value() != iter->second)
{
_cloudViewer->updateFrustumPose(frustumId, iter->second);
}
}
else if(_cachedSignatures.contains(iter->first))
{
const Signature & s = _cachedSignatures.value(iter->first);
// Supporting only one frustum per node
if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
{
Transform t = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().localTransform():s.sensorData().cameraModels()[0].localTransform();
if(!t.isNull())
{
QColor color = Qt::gray;
_cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color);
}
}
}
}
pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
kter->second->push_back(pt);
}
// add graphs
@@ -2187,6 +2252,8 @@ void MainWindow::updateMapCloud(
}
_cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
}
UDEBUG("timerGraph=%fs", timerGraph.ticks());
}
UDEBUG("labels.size()=%d", (int)labels.size());
@@ -2435,9 +2502,9 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
}
// filtering pipeline
if(indices->size() && _preferencesDialog->getMapVoxel() > 0.0)
if(indices->size() && _preferencesDialog->getVoxel() > 0.0)
{
cloud = util3d::voxelize(cloud, indices, _preferencesDialog->getMapVoxel());
cloud = util3d::voxelize(cloud, indices, _preferencesDialog->getVoxel());
//generate indices for all points (they are all valid)
indices->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
@@ -2446,23 +2513,36 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
}
}
// Do ceiling/floor filtering
if(indices->size() &&
(_preferencesDialog->getFloorFilteringHeight() != 0.0 ||
_preferencesDialog->getCeilingFilteringHeight() != 0.0))
{
// perform in /map frame
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
indices = rtabmap::util3d::passThrough(
cloudTransformed,
indices,
"z",
_preferencesDialog->getFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getFloorFilteringHeight(),
_preferencesDialog->getCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getCeilingFilteringHeight());
}
// Do radius filtering after voxel filtering ( a lot faster)
if(indices->size() &&
_preferencesDialog->getMapNoiseRadius() > 0.0 &&
_preferencesDialog->getMapNoiseMinNeighbors() > 0)
_preferencesDialog->getNoiseRadius() > 0.0 &&
_preferencesDialog->getNoiseMinNeighbors() > 0)
{
indices = rtabmap::util3d::radiusFiltering(
cloud,
indices,
_preferencesDialog->getMapNoiseRadius(),
_preferencesDialog->getMapNoiseMinNeighbors());
_preferencesDialog->getNoiseRadius(),
_preferencesDialog->getNoiseMinNeighbors());
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
GainCompensator compensator;
if((_preferencesDialog->isSubtractFiltering() &&
_preferencesDialog->getSubtractFilteringRadius() > 0.0) ||
_preferencesDialog->gainCompensation())
if(_preferencesDialog->isSubtractFiltering() &&
_preferencesDialog->getSubtractFilteringRadius() > 0.0)
{
pcl::IndicesPtr beforeFiltering = indices;
if( cloud->size() &&
@@ -2480,13 +2560,6 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
//pcl::io::savePCDFile("new.pcd", *cloud, *indices);
//pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second);
if(_preferencesDialog->gainCompensation())
{
compensator.feed(cloud, indices, _previousCloud.second.first.first, _previousCloud.second.second, t);
compensator.apply(0, cloud, indices);
UINFO("Time gain compensation = %fs", time.ticks());
}
if(_preferencesDialog->isSubtractFiltering())
{
if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f)
@@ -2598,10 +2671,6 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
tex_name >> mesh_material.tex_name;
mesh_material.tex_file = "";
if(_preferencesDialog->gainCompensation() && compensator.getIndex(0) >= 0)
{
compensator.apply(0, image);
}
textureMesh->tex_materials.push_back(mesh_material);
@@ -2682,10 +2751,18 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
_cachedClouds.insert(std::make_pair(nodeId, outputPair));
_createdCloudsMemoryUsage += (long)(output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int));
}
_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
}
else
{
_cachedEmptyClouds.insert(nodeId);
}
}
_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
else
{
_cachedEmptyClouds.insert(nodeId);
}
}
UDEBUG("");
@@ -2747,6 +2824,9 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
}
_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
}
}
else
@@ -2778,10 +2858,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
}
_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
}
}
_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
}
}
@@ -5298,6 +5379,7 @@ void MainWindow::clearTheCache()
_cachedMemoryUsage = 0;
_cachedClouds.clear();
_createdCloudsMemoryUsage = 0;
_cachedEmptyClouds.clear();
_previousCloud.first = 0;
_previousCloud.second.first.first.reset();
_previousCloud.second.first.second.reset();
@@ -5725,8 +5807,27 @@ void MainWindow::exportClouds()
return;
}
std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
// Use ground truth poses if current clouds are using them
if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
{
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
if(gtIter!=_currentGTPosesMap.end())
{
iter->second = gtIter->second;
}
else
{
UWARN("Not found ground truth pose for node %d", iter->first);
}
}
}
_exportCloudsDialog->exportClouds(
_ui->widget_mapVisibility->getVisiblePoses(),
poses,
_currentLinksMap,
_currentMapIds,
_cachedSignatures,
@@ -5742,8 +5843,27 @@ void MainWindow::viewClouds()
return;
}
std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
// Use ground truth poses if current clouds are using them
if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
{
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
if(gtIter!=_currentGTPosesMap.end())
{
iter->second = gtIter->second;
}
else
{
UWARN("Not found ground truth pose for node %d", iter->first);
}
}
}
_exportCloudsDialog->viewClouds(
_ui->widget_mapVisibility->getVisiblePoses(),
poses,
_currentLinksMap,
_currentMapIds,
_cachedSignatures,
@@ -5962,6 +6082,23 @@ void MainWindow::exportBundlerFormat()
{
std::map<int, Transform> posesIn = _ui->widget_mapVisibility->getVisiblePoses();
// Use ground truth poses if current clouds are using them
if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
{
for(std::map<int, Transform>::iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
{
std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
if(gtIter!=_currentGTPosesMap.end())
{
iter->second = gtIter->second;
}
else
{
UWARN("Not found ground truth pose for node %d", iter->first);
}
}
}
std::map<int, Transform> poses;
for(std::map<int, Transform>::iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
{

53
guilib/src/PreferencesDialog.cpp
View File

@@ -379,8 +379,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_voxel, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_noiseRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->spinBox_noiseMinNeighbors, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_ceilingFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_floorFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_showGraphs, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_showFrustums, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_showLabels, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->radioButton_noFiltering, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -391,8 +395,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->spinBox_subtractFilteringMinPts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_subtractFilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_subtractFilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_gainCompensation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1257,11 +1259,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->doubleSpinBox_noiseRadius->setValue(0);
_ui->spinBox_noiseMinNeighbors->setValue(5);
_ui->doubleSpinBox_ceilingFilterHeight->setValue(0);
_ui->doubleSpinBox_floorFilterHeight->setValue(0);
_ui->checkBox_showGraphs->setChecked(true);
_ui->checkBox_showFrustums->setChecked(false);
_ui->checkBox_showLabels->setChecked(false);
_ui->spinBox_normalKSearch->setValue(10);
_ui->checkBox_gainCompensation->setChecked(false);
_ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0);
_ui->groupBox_organized->setChecked(false);
@@ -1647,8 +1652,12 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->doubleSpinBox_voxel->setValue(settings.value("cloudVoxel", _ui->doubleSpinBox_voxel->value()).toDouble());
_ui->doubleSpinBox_noiseRadius->setValue(settings.value("cloudNoiseRadius", _ui->doubleSpinBox_noiseRadius->value()).toDouble());
_ui->spinBox_noiseMinNeighbors->setValue(settings.value("cloudNoiseMinNeighbors", _ui->spinBox_noiseMinNeighbors->value()).toInt());
_ui->doubleSpinBox_ceilingFilterHeight->setValue(settings.value("cloudCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight->value()).toDouble());
_ui->doubleSpinBox_floorFilterHeight->setValue(settings.value("cloudFloorHeight", _ui->doubleSpinBox_floorFilterHeight->value()).toDouble());
_ui->spinBox_normalKSearch->setValue(settings.value("normalKSearch", _ui->spinBox_normalKSearch->value()).toInt());
_ui->checkBox_showGraphs->setChecked(settings.value("showGraphs", _ui->checkBox_showGraphs->isChecked()).toBool());
_ui->checkBox_showFrustums->setChecked(settings.value("showFrustums", _ui->checkBox_showFrustums->isChecked()).toBool());
_ui->checkBox_showLabels->setChecked(settings.value("showLabels", _ui->checkBox_showLabels->isChecked()).toBool());
_ui->radioButton_noFiltering->setChecked(settings.value("noFiltering", _ui->radioButton_noFiltering->isChecked()).toBool());
@@ -1659,8 +1668,6 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->spinBox_subtractFilteringMinPts->setValue(settings.value("subtractFilteringMinPts", _ui->spinBox_subtractFilteringMinPts->value()).toInt());
_ui->doubleSpinBox_subtractFilteringRadius->setValue(settings.value("subtractFilteringRadius", _ui->doubleSpinBox_subtractFilteringRadius->value()).toDouble());
_ui->doubleSpinBox_subtractFilteringAngle->setValue(settings.value("subtractFilteringAngle", _ui->doubleSpinBox_subtractFilteringAngle->value()).toDouble());
_ui->spinBox_normalKSearch->setValue(settings.value("normalKSearch", _ui->spinBox_normalKSearch->value()).toInt());
_ui->checkBox_gainCompensation->setChecked(settings.value("gainCompensation", _ui->checkBox_gainCompensation->isChecked()).toBool());
_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
@@ -2030,8 +2037,12 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue("cloudVoxel", _ui->doubleSpinBox_voxel->value());
settings.setValue("cloudNoiseRadius", _ui->doubleSpinBox_noiseRadius->value());
settings.setValue("cloudNoiseMinNeighbors", _ui->spinBox_noiseMinNeighbors->value());
settings.setValue("cloudCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight->value());
settings.setValue("cloudFloorHeight", _ui->doubleSpinBox_floorFilterHeight->value());
settings.setValue("normalKSearch", _ui->spinBox_normalKSearch->value());
settings.setValue("showGraphs", _ui->checkBox_showGraphs->isChecked());
settings.setValue("showFrustums", _ui->checkBox_showFrustums->isChecked());
settings.setValue("showLabels", _ui->checkBox_showLabels->isChecked());
settings.setValue("noFiltering", _ui->radioButton_noFiltering->isChecked());
@@ -2042,8 +2053,6 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue("subtractFilteringMinPts", _ui->spinBox_subtractFilteringMinPts->value());
settings.setValue("subtractFilteringRadius", _ui->doubleSpinBox_subtractFilteringRadius->value());
settings.setValue("subtractFilteringAngle", _ui->doubleSpinBox_subtractFilteringAngle->value());
settings.setValue("normalKSearch", _ui->spinBox_normalKSearch->value());
settings.setValue("gainCompensation", _ui->checkBox_gainCompensation->isChecked());
settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked());
settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value());
@@ -3938,23 +3947,39 @@ double PreferencesDialog::getOctomapOccupancyThr() const
return _ui->doubleSpinBox_octomap_occupancyThr->value();
}
double PreferencesDialog::getMapVoxel() const
double PreferencesDialog::getVoxel() const
{
return _ui->doubleSpinBox_voxel->value();
}
double PreferencesDialog::getMapNoiseRadius() const
double PreferencesDialog::getNoiseRadius() const
{
return _ui->doubleSpinBox_noiseRadius->value();
}
int PreferencesDialog::getMapNoiseMinNeighbors() const
int PreferencesDialog::getNoiseMinNeighbors() const
{
return _ui->spinBox_noiseMinNeighbors->value();
}
double PreferencesDialog::getCeilingFilteringHeight() const
{
return _ui->doubleSpinBox_ceilingFilterHeight->value();
}
double PreferencesDialog::getFloorFilteringHeight() const
{
return _ui->doubleSpinBox_floorFilterHeight->value();
}
int PreferencesDialog::getNormalKSearch() const
{
return _ui->spinBox_normalKSearch->value();
}
bool PreferencesDialog::isGraphsShown() const
{
return _ui->checkBox_showGraphs->isChecked();
}
bool PreferencesDialog::isFrustumsShown() const
{
return _ui->checkBox_showFrustums->isChecked();
}
bool PreferencesDialog::isLabelsShown() const
{
return _ui->checkBox_showLabels->isChecked();
@@ -4088,14 +4113,6 @@ double PreferencesDialog::getSubtractFilteringAngle() const
{
return _ui->doubleSpinBox_subtractFilteringAngle->value()*M_PI/180.0;
}
int PreferencesDialog::getNormalKSearch() const
{
return _ui->spinBox_normalKSearch->value();
}
bool PreferencesDialog::gainCompensation() const
{
return _ui->checkBox_gainCompensation->isChecked();
}
bool PreferencesDialog::getGridMapShown() const
{
return _ui->checkBox_map_shown->isChecked();

1886
guilib/src/ui/preferencesDialog.ui
View File

File diff suppressed because it is too large Load Diff