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

updated util3d::create2dMap() with filling unknown space option (default true)

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1453 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-03 20:16:39 +00:00
parent 99fbca5ffe
commit 87d1e4b52a
8 changed files with 290 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -178,7 +178,6 @@ private:
void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose);
void createAndAddCloudToMap(int nodeId, const Transform & pose);
void createAndAddScanToMap(int nodeId, const Transform & pose);
std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses) const;
void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
void setupMainLayout(bool vertical);
void updateSelectSourceImageMenu(int type);

36
guilib/src/GraphViewer.cpp
View File

@@ -135,7 +135,9 @@ GraphViewer::GraphViewer(QWidget * parent) :
_nodeRadius(0.01),
_linkWidth(0),
_gridMap(0),
_gridCellSize(0.05f)
_lastReferential(0),
_gridCellSize(0.05f),
_gridUnknownSpaceFilled(true)
{
Q_ASSERT(_gridCellSize > 0);
@@ -154,6 +156,19 @@ GraphViewer::GraphViewer(QWidget * parent) :
item->setZValue(100);
item->setParentItem(_root);
// current pose
_lastReferential = new QGraphicsItemGroup();
this->scene()->addItem(_lastReferential);
item = this->scene()->addLine(0,0,0,-0.5, QPen(QBrush(Qt::red), _linkWidth));
item->setZValue(100);
item->setParentItem(_root);
_lastReferential->addToGroup(item);
item = this->scene()->addLine(0,0,-0.5,0, QPen(QBrush(Qt::green), _linkWidth));
item->setZValue(100);
item->setParentItem(_root);
_lastReferential->addToGroup(item);
_gridMap = this->scene()->addPixmap(QPixmap());
_gridMap->scale(_gridCellSize, -_gridCellSize);
_gridMap->setRotation(90);
@@ -312,7 +327,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
if(scanClouds.size())
{
float xMin=0.0f, yMin=0.0f;
cv::Mat map8S = util3d::create2DMap(poses, scanClouds, _gridCellSize, xMin, yMin);
cv::Mat map8S = util3d::create2DMap(poses, scanClouds, _gridCellSize, _gridUnknownSpaceFilled, xMin, yMin);
cv::Mat map8U(map8S.rows, map8S.cols, CV_8U);
//convert to gray scaled map
for (int i = 0; i < map8S.rows; ++i)
@@ -346,7 +361,13 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
if(_nodeItems.size())
{
(--_nodeItems.end()).value()->setColor(Qt::gray);
(--_nodeItems.end()).value()->setColor(Qt::green);
}
if(poses.size())
{
Transform t = poses.rbegin()->second;
QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24(), -t.o14());
_lastReferential->setTransform(qt);
}
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
@@ -362,6 +383,7 @@ void GraphViewer::clearGraph()
qDeleteAll(_loopLinkItems);
_loopLinkItems.clear();
_gridMap->setPixmap(QPixmap());
_lastReferential->resetTransform();
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
}
@@ -391,6 +413,9 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
QAction * aSetLinkSize = menu.addAction(tr("Set link width..."));
menu.addSeparator();
QAction * aSetGridCellSize = menu.addAction(tr("Set grid cell size..."));
QAction * aSetGridUnknownSpaceFilled = menu.addAction(tr("Unknown grid space filled"));
aSetGridUnknownSpaceFilled->setCheckable(true);
aSetGridUnknownSpaceFilled->setChecked(_gridUnknownSpaceFilled);
QAction * aShowHideGridMap;
if(_gridMap->isVisible())
{
@@ -557,9 +582,12 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
if(ok)
{
_gridCellSize = value;
}
}
else if(r == aSetGridUnknownSpaceFilled)
{
_gridUnknownSpaceFilled = aSetGridUnknownSpaceFilled->isChecked();
}
else if(r == aShowHideGridMap)
{
_gridMap->setVisible(!_gridMap->isVisible());

3
guilib/src/GraphViewer.h
View File

@@ -15,6 +15,7 @@
class QGraphicsItem;
class QGraphicsPixmapItem;
class QGraphicsItemGroup;
namespace rtabmap {
@@ -50,7 +51,9 @@ private:
float _nodeRadius;
float _linkWidth;
QGraphicsPixmapItem * _gridMap;
QGraphicsItemGroup * _lastReferential;
float _gridCellSize;
bool _gridUnknownSpaceFilled;
};
} /* namespace rtabmap */

107
guilib/src/MainWindow.cpp
View File

@@ -942,7 +942,21 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
// update some widgets
if(_ui->graphicsView_graphView->isVisible())
{
_ui->graphicsView_graphView->updateGraph(stat.poses(), stat.constraints(), _depths2DMap);
std::map<int, Transform> poses;
if(_preferencesDialog->isCloudFiltering() && stat.poses().size())
{
float radius = _preferencesDialog->getCloudFilteringRadius();
float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
poses = util3d::radiusPosesFiltering(stat.poses(), radius, angle);
// make sure the last is here
poses.insert(*stat.poses().rbegin());
}
else
{
poses = stat.poses();
}
_ui->graphicsView_graphView->updateGraph(poses, stat.constraints(), _depths2DMap);
}
_odometryReceived = false;
@@ -1036,9 +1050,13 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
// filter duplicated poses
std::map<int, Transform> poses;
if(_preferencesDialog->isCloudFiltering())
if(_preferencesDialog->isCloudFiltering() && posesIn.size())
{
poses = radiusPosesFiltering(posesIn);
float radius = _preferencesDialog->getCloudFilteringRadius();
float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
poses = util3d::radiusPosesFiltering(posesIn, radius, angle);
// make sure the last is here
poses.insert(*posesIn.rbegin());
}
else
{
@@ -1308,89 +1326,6 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
_ui->widget_cloudViewer->render();
}
std::map<int, Transform> MainWindow::radiusPosesFiltering(const std::map<int, Transform> & poses) const
{
float radius = _preferencesDialog->getCloudFilteringRadius();
float angle = _preferencesDialog->getCloudFilteringAngle()*3.14159265359/180.0; // convert to rad
if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(poses.size());
int i=0;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
}
// radius filtering
std::vector<int> names = uKeys(poses);
std::vector<Transform> transforms = uValues(poses);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
tree->setInputCloud(cloud);
std::set<int> indicesChecked;
std::set<int> indicesKept;
for(unsigned int i=0; i<cloud->size(); ++i)
{
// ignore scans
if(indicesChecked.find(i) == indicesChecked.end())
{
std::vector<int> kIndices;
std::vector<float> kDistances;
tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
std::set<int> cloudIndices;
const Transform & currentT = transforms.at(i);
Eigen::Vector3f vA = util3d::transformToEigen3f(currentT).rotation()*Eigen::Vector3f(1,0,0);
for(unsigned int j=0; j<kIndices.size(); ++j)
{
if(indicesChecked.find(kIndices[j]) == indicesChecked.end())
{
const Transform & checkT = transforms.at(kIndices[j]);
// same orientation?
Eigen::Vector3f vB = util3d::transformToEigen3f(checkT).rotation()*Eigen::Vector3f(1,0,0);
double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
if(a <= angle)
{
cloudIndices.insert(kIndices[j]);
}
}
}
bool firstAdded = false;
for(std::set<int>::iterator iter = cloudIndices.begin(); iter!=cloudIndices.end(); ++iter)
{
if(!firstAdded)
{
indicesKept.insert(*iter);
firstAdded = true;
}
indicesChecked.insert(*iter);
}
}
}
//pcl::IndicesPtr indicesOut(new std::vector<int>);
//indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
UINFO("Cloud filtered In = %d, Out = %d", cloud->size(), indicesKept.size());
//pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
//pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);
std::map<int, Transform> keptPoses;
for(std::set<int>::iterator iter = indicesKept.begin(); iter!=indicesKept.end(); ++iter)
{
keptPoses.insert(std::make_pair(names.at(*iter), transforms.at(*iter)));
}
return keptPoses;
}
else
{
return poses;
}
}
void MainWindow::processRtabmapEventInit(int status, const QString & info)
{
if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing)