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

Improved performance of create2DMap() and fixed unknown space filled rotation when laser scans are inverted. Added fill unknown space option to DatabaseViewer.

Browse Source
This commit is contained in:
matlabbe
2015-09-28 15:30:01 -04:00
parent 7ea730bb0a
commit 3fa6124ad0
3 changed files with 83 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
corelib/src/util3d_mapping.cpp
View File

@@ -405,7 +405,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans; std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
// For computation issue, the maximum scan range allowed is 6 meters // For computation issue, the maximum scan range allowed is 6 meters
if(scanMaxRange > 6.0f) if(scanMaxRange > 6.0f || scanMaxRange <= 0.0f)
{ {
scanMaxRange = 6.0f; scanMaxRange = 6.0f;
} }
@@ -438,20 +438,19 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
pcl::PointXYZ min, max; pcl::PointXYZ min, max;
pcl::getMinMax3D(minMax, min, max); pcl::getMinMax3D(minMax, min, max);
// Added X2 to make sure that all points are inside the map (when rounded to integer) // Added margin to make sure that all points are inside the map (when rounded to integer)
float margin = cellSize*10.0f + (scanMaxRange>0.0f?scanMaxRange/cellSize:0.0f); float margin = cellSize*10.0f;
xMin = min.x-margin; xMin = (scanMaxRange > 0 && -scanMaxRange < min.x?-scanMaxRange:min.x) - margin;
yMin = min.y-margin; yMin = (scanMaxRange > 0 && -scanMaxRange < min.y?-scanMaxRange:min.y) - margin;
float xMax = max.x+margin; float xMax = (scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + margin;
float yMax = max.y+margin; float yMax = (scanMaxRange > 0 && scanMaxRange > max.y?scanMaxRange:max.y) + margin;
UDEBUG("map min=(%f, %f) max=(%f,%f) (margin=%f, cellSize=%f, scan range=%f, min=[%f,%f] max=[%f,%f])", //UWARN("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y); // xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
UTimer timer; UTimer timer;
map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1; map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
std::vector<float> maxSquaredLength(localScans.size(), scanMaxRange*scanMaxRange);
int j=0; int j=0;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{ {
@@ -459,19 +458,11 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f); cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
for(unsigned int i=0; i<iter->second->size(); ++i) for(unsigned int i=0; i<iter->second->size(); ++i)
{ {
cv::Point2i end((iter->second->points[i].x-xMin)/cellSize + 0.5f, (iter->second->points[i].y-yMin)/cellSize + 0.5f); cv::Point2i end((iter->second->points[i].x-xMin)/cellSize, (iter->second->points[i].y-yMin)/cellSize);
map.at<char>(end.y, end.x) = 100; // obstacle if(end!=start)
rayTrace(start, end, map, true); // trace free space
if(unknownSpaceFilled)
{ {
float dx = iter->second->points[i].x - pose.x(); rayTrace(start, end, map, true); // trace free space
float dy = iter->second->points[i].y - pose.y(); map.at<char>(end.y, end.x) = 100; // obstacle
float l = dx*dx + dy*dy;
if(l > maxSquaredLength[j])
{
maxSquaredLength[j] = l;
}
} }
} }
++j; ++j;
@@ -479,36 +470,45 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
UDEBUG("Ray trace known space=%fs", timer.ticks()); UDEBUG("Ray trace known space=%fs", timer.ticks());
// now fill unknown spaces // now fill unknown spaces
if(unknownSpaceFilled) if(unknownSpaceFilled && scanMaxRange > 0)
{ {
j=0; j=0;
float a = CV_PI/256.0f; // angle increment float a = CV_PI/256.0f; // angle increment
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{ {
if(iter->second->size() > 1 && maxSquaredLength[j] > 0.0f) if(iter->second->size() > 1)
{ {
float maxLength = sqrt(maxSquaredLength[j]); if(scanMaxRange > cellSize)
if(maxLength > cellSize)
{ {
const Transform & pose = poses.at(iter->first); const Transform & pose = poses.at(iter->first);
cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f); cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
//UWARN("maxLength = %f", maxLength); //UWARN("maxLength = %f", maxLength);
//rotate counterclockwise from the first point until we pass the last point //rotate counterclockwise from the first point until we pass the last point
// Note: assuming that first laser scan is negative y
cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(a), -sin(a), cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(a), -sin(a),
sin(a), cos(a)); sin(a), cos(a));
cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F); cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
origin.at<float>(0) = pose.x(); origin.at<float>(0) = pose.x();
origin.at<float>(1) = pose.y(); origin.at<float>(1) = pose.y();
endFirst.at<float>(0) = iter->second->points[0].x; pcl::PointXYZ ptFirst = iter->second->points[0];
endFirst.at<float>(1) = iter->second->points[0].y; pcl::PointXYZ ptLast = iter->second->points[iter->second->points.size()-1];
endLast.at<float>(0) = iter->second->points[iter->second->points.size()-1].x; if(ptFirst.y > ptLast.y)
endLast.at<float>(1) = iter->second->points[iter->second->points.size()-1].y; {
// swap to iterate counterclockwise
pcl::PointXYZ tmp = ptLast;
ptLast = ptFirst;
ptFirst = tmp;
}
endFirst.at<float>(0) = ptFirst.x;
endFirst.at<float>(1) = ptFirst.y;
endLast.at<float>(0) = ptLast.x;
endLast.at<float>(1) = ptLast.y;
//UWARN("origin = %f %f", origin.at<float>(0), origin.at<float>(1)); //UWARN("origin = %f %f", origin.at<float>(0), origin.at<float>(1));
//UWARN("endFirst = %f %f", endFirst.at<float>(0), endFirst.at<float>(1)); //UWARN("endFirst = %f %f", endFirst.at<float>(0), endFirst.at<float>(1));
//UWARN("endLast = %f %f", endLast.at<float>(0), endLast.at<float>(1)); //UWARN("endLast = %f %f", endLast.at<float>(0), endLast.at<float>(1));
cv::Mat tmp = (endFirst - origin); cv::Mat tmp = (endFirst - origin);
cv::Mat endRotated = rotation*((tmp/cv::norm(tmp))*maxLength) + origin; cv::Mat endRotated = rotation*((tmp/cv::norm(tmp))*scanMaxRange) + origin;
cv::Mat endLastVector(3,1,CV_32F), endRotatedVector(3,1,CV_32F); cv::Mat endLastVector(3,1,CV_32F), endRotatedVector(3,1,CV_32F);
endLastVector.at<float>(0) = endLast.at<float>(0) - origin.at<float>(0); endLastVector.at<float>(0) = endLast.at<float>(0) - origin.at<float>(0);
endLastVector.at<float>(1) = endLast.at<float>(1) - origin.at<float>(1); endLastVector.at<float>(1) = endLast.at<float>(1) - origin.at<float>(1);
@@ -517,10 +517,11 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1); endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
endRotatedVector.at<float>(2) = 0.0f; endRotatedVector.at<float>(2) = 0.0f;
//UWARN("endRotated = %f %f", endRotated.at<float>(0), endRotated.at<float>(1)); //UWARN("endRotated = %f %f", endRotated.at<float>(0), endRotated.at<float>(1));
float CV_PI_2 = CV_PI/2.0f;
float normEndRotatedVector = cv::norm(endRotatedVector); float normEndRotatedVector = cv::norm(endRotatedVector);
endLastVector = endLastVector / cv::norm(endLastVector); endLastVector = endLastVector / cv::norm(endLastVector);
while(acos((endRotatedVector/normEndRotatedVector).dot(endLastVector)) > CV_PI_2 || endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f) float angle = (endRotatedVector/normEndRotatedVector).dot(endLastVector);
angle = angle<-1.0f?-1.0f:angle>1.0f?1.0f:angle;
while(acos(angle) > M_PI_4 || endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f)
{ {
cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize + 0.5f, (endRotated.at<float>(1)-yMin)/cellSize + 0.5f); cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize + 0.5f, (endRotated.at<float>(1)-yMin)/cellSize + 0.5f);
//end must be inside the grid //end must be inside the grid
@@ -533,13 +534,22 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
endRotated = rotation*(endRotated - origin) + origin; endRotated = rotation*(endRotated - origin) + origin;
endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0); endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1); endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
//UWARN("endRotated = %f %f", endRotated.at<float>(0), endRotated.at<float>(1)); angle = (endRotatedVector/normEndRotatedVector).dot(endLastVector);
angle = angle<-1.0f?-1.0f:angle>1.0f?1.0f:angle;
//UWARN("endRotated = %f %f (%f %f %f)",
// endRotated.at<float>(0), endRotated.at<float>(1),
// acos(angle),
// angle,
// endRotatedVector.cross(endLastVector).at<float>(2));
} }
} }
} }
++j; ++j;
} }
UDEBUG("Fill empty space=%fs", timer.ticks()); UDEBUG("Fill empty space=%fs", timer.ticks());
//cv::imwrite("map.png", util3d::convertMap2Image8U(map));
//UWARN("saved map.png");
} }
} }
return map; return map;

30
guilib/src/DatabaseViewer.cpp
View File

@@ -224,7 +224,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView())); connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
connect(ui_->checkBox_gridFillUnkownSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView())); connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
@@ -267,6 +268,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(int)), this, SLOT(configModified())); connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_gridFillUnkownSpace, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(configModified())); connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified())); connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->spinBox_projDecimation, SIGNAL(valueChanged(int)), this, SLOT(configModified())); connect(ui_->spinBox_projDecimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
@@ -387,6 +389,7 @@ void DatabaseViewer::readSettings()
ui_->checkBox_2dslam->setChecked(settings.value("slam2d", ui_->checkBox_2dslam->isChecked()).toBool()); ui_->checkBox_2dslam->setChecked(settings.value("slam2d", ui_->checkBox_2dslam->isChecked()).toBool());
ui_->spinBox_optimizationDepth->setValue(settings.value("depth", ui_->spinBox_optimizationDepth->value()).toInt()); ui_->spinBox_optimizationDepth->setValue(settings.value("depth", ui_->spinBox_optimizationDepth->value()).toInt());
ui_->checkBox_gridErode->setChecked(settings.value("erode", ui_->checkBox_gridErode->isChecked()).toBool()); ui_->checkBox_gridErode->setChecked(settings.value("erode", ui_->checkBox_gridErode->isChecked()).toBool());
ui_->checkBox_gridFillUnkownSpace->setChecked(settings.value("unknownSpaceFilled", ui_->checkBox_gridFillUnkownSpace->isChecked()).toBool());
settings.endGroup(); settings.endGroup();
settings.beginGroup("grid"); settings.beginGroup("grid");
@@ -481,6 +484,7 @@ void DatabaseViewer::writeSettings()
settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked()); settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
settings.setValue("depth", ui_->spinBox_optimizationDepth->value()); settings.setValue("depth", ui_->spinBox_optimizationDepth->value());
settings.setValue("erode", ui_->checkBox_gridErode->isChecked()); settings.setValue("erode", ui_->checkBox_gridErode->isChecked());
settings.setValue("unknownSpaceFilled", ui_->checkBox_gridFillUnkownSpace->isChecked());
settings.endGroup(); settings.endGroup();
// save Grid settings // save Grid settings
@@ -2516,18 +2520,20 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
{ {
if(memory_ && value >=0 && value < (int)graphes_.size()) if(memory_ && value >=0 && value < (int)graphes_.size())
{ {
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
if(ui_->dockWidget_graphView->isVisible() && localMaps_.size() == 0) if(ui_->dockWidget_graphView->isVisible() && localMaps_.size() == 0)
{ {
//update scans //update scans
UINFO("Update local maps list..."); UINFO("Update local maps list...");
for(int i=0; i<ids_.size(); ++i) std::vector<int> ids = uKeys(graph);
for(unsigned int i=0; i<ids.size(); ++i)
{ {
UTimer time; UTimer time;
bool added = false; bool added = false;
if(ui_->groupBox_gridFromProjection->isChecked()) if(ui_->groupBox_gridFromProjection->isChecked())
{ {
SensorData data = memory_->getNodeData(ids_.at(i), true); SensorData data = memory_->getNodeData(ids.at(i), true);
if(!data.depthOrRightRaw().empty()) if(!data.depthOrRightRaw().empty())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
@@ -2553,7 +2559,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
if(!ground.empty() || !obstacles.empty()) if(!ground.empty() || !obstacles.empty())
{ {
localMaps_.insert(std::make_pair(ids_.at(i), std::make_pair(ground, obstacles))); localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
added = true; added = true;
} }
} }
@@ -2561,26 +2567,32 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
} }
else else
{ {
SensorData data = memory_->getNodeData(ids_.at(i), false); SensorData data = memory_->getNodeData(ids.at(i), false);
if(!data.laserScanCompressed().empty()) if(!data.laserScanCompressed().empty())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat laserScan; cv::Mat laserScan;
data.uncompressDataConst(0, 0, &laserScan); data.uncompressDataConst(0, 0, &laserScan);
cv::Mat ground, obstacles; cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(laserScan, ground, obstacles, ui_->doubleSpinBox_gridCellSize->value()); util3d::occupancy2DFromLaserScan(
localMaps_.insert(std::make_pair(ids_.at(i), std::make_pair(ground, obstacles))); laserScan,
ground,
obstacles,
ui_->doubleSpinBox_gridCellSize->value(),
ui_->checkBox_gridFillUnkownSpace->isChecked(),
data.laserScanMaxRange());
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
added = true; added = true;
} }
} }
if(added) if(added)
{ {
UINFO("Processed grid map %d/%d (%fs)", i+1, (int)ids_.size(), time.ticks()); UINFO("Processed grid map %d/%d (%fs)", i+1, (int)ids.size(), time.ticks());
} }
} }
UINFO("Update local maps list... done"); UINFO("Update local maps list... done");
} }
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_); ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
if(graph.size() && localMaps_.size() && ui_->graphViewer->isGridMapVisible()) if(graph.size() && localMaps_.size() && ui_->graphViewer->isGridMapVisible())
{ {

22
guilib/src/ui/DatabaseViewer.ui
View File

@@ -829,7 +829,7 @@
<item> <item>
<widget class="QToolBox" name="toolBox"> <widget class="QToolBox" name="toolBox">
<property name="currentIndex"> <property name="currentIndex">
<number>2</number> <number>3</number>
</property> </property>
<widget class="QWidget" name="page"> <widget class="QWidget" name="page">
<property name="geometry"> <property name="geometry">
@@ -1345,7 +1345,7 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-26</y> <y>0</y>
<width>333</width> <width>333</width>
<height>333</height> <height>333</height>
</rect> </rect>
@@ -1585,8 +1585,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>243</width> <width>320</width>
<height>284</height> <height>311</height>
</rect> </rect>
</property> </property>
<attribute name="label"> <attribute name="label">
@@ -1635,6 +1635,20 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="2" column="1">
<widget class="QLabel" name="label_64">
<property name="text">
<string>Fill unknown space</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_gridFillUnkownSpace">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout> </layout>
</item> </item>
<item> <item>