i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Parameters: added Grid/FullUpdate (default true). Updated how occupancy grid is updated after loop closure. OctoMap: added tree depth argument when creating 2d map, added full update argument on constructor (default false). MainWindow: using OccupancyGrid object instead of keeping in cache local grids (we can have actual time to update the global grid).

Browse Source
This commit is contained in:
matlabbe
2017-04-07 18:31:11 -04:00
parent 6bfce63060
commit b7dbf27931
13 changed files with 831 additions and 676 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/OccupancyGrid.h
+3
View File
@@ -45,6 +45,8 @@ public:
void setCellSize(float cellSize);
float getCellSize() const {return cellSize_;}
bool isGridFromDepth() const {return occupancyFromCloud_;}
bool isFullUpdate() const {return fullUpdate_;}
const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr segmentCloud(
@@ -104,6 +106,7 @@ private:
bool scan2dUnknownSpaceFilled_;
double scan2dMaxUnknownSpaceFilledRange_;
bool projRayTracing_;
bool fullUpdate_;
std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
cv::Mat map_;
corelib/include/rtabmap/core/OctoMap.h
+4 -2
View File
@@ -57,7 +57,7 @@ public:
class RTABMAP_EXP OctoMap {
public:
OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f);
OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false);
const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
void addToCache(int nodeId,
@@ -81,7 +81,8 @@ public:
float & xMin,
float & yMin,
float & gridCellSize,
float minGridSize);
float minGridSize = 0.0f,
unsigned int treeDepth = 0);
bool writeBinary(const std::string & path);
@@ -97,6 +98,7 @@ private:
std::map<int, Transform> addedNodes_;
octomap::KeyRay keyRay_;
bool hasColor_;
bool fullUpdate_;
};
} /* namespace rtabmap */
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -505,6 +505,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled, bool, false, "Unknown space filled. Only used with 2D laser scans.");
RTABMAP_PARAM(Grid, Scan2dMaxFilledRange, float, 4.0, "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
RTABMAP_PARAM(Grid, ProjRayTracing, bool, true, uFormat("[%s=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.", kGrid3D().c_str()));
RTABMAP_PARAM(Grid, FullUpdate, bool, true, "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.");
public:
virtual ~Parameters();
corelib/include/rtabmap/core/util3d_mapping.h
+2
View File
@@ -116,6 +116,8 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
cv::Mat RTABMAP_EXP erodeMap(const cv::Mat & map);
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
const typename pcl::PointCloud<PointT> & cloud);
corelib/src/OccupancyGrid.cpp
+209 -106
View File
@@ -65,6 +65,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
projRayTracing_(Parameters::defaultGridProjRayTracing()),
fullUpdate_(Parameters::defaultGridFullUpdate()),
xMin_(0.0f),
yMin_(0.0f)
{
@@ -131,6 +132,7 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
Parameters::parse(parameters, Parameters::kGridProjRayTracing(), projRayTracing_);
Parameters::parse(parameters, Parameters::kGridFullUpdate(), fullUpdate_);
// convert ROI from string to vector
ParametersMap::const_iterator iter;
@@ -383,7 +385,7 @@ void OccupancyGrid::addToCache(
const cv::Mat & obstacles)
{
UDEBUG("nodeId=%d", nodeId);
cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
}
void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMapSize, float footprintRadius)
@@ -391,7 +393,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
UTimer timer;
UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size());
float margin = cellSize_*10.0f+footprintRadius;
float margin = cellSize_*10.0f+(footprintRadius>cellSize_*1.5f?float(int(footprintRadius/cellSize_)+1):0.0f)*cellSize_;
float minX=-minMapSize/2.0f;
float minY=-minMapSize/2.0f;
@@ -401,7 +403,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
std::map<int, cv::Mat> emptyLocalMaps;
std::map<int, cv::Mat> occupiedLocalMaps;
// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
// First, check of the graph has changed. If so, re-create the map by moving all occupied nodes.
bool graphChanged = false;
std::map<int, Transform> transforms;
for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
@@ -448,75 +450,78 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
if(graphChanged && !map_.empty())
{
UINFO("Graph changed!");
// 1) recreate all local maps
UASSERT(map_.cols == mapInfo_.cols &&
map_.rows == mapInfo_.rows);
std::map<int, std::pair<int, int> > tmpIndices;
for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
if(!fullUpdate_)
{
if(iter->second.first)
// 1) recreate all local maps
UASSERT(map_.cols == mapInfo_.cols &&
map_.rows == mapInfo_.rows);
std::map<int, std::pair<int, int> > tmpIndices;
for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
{
emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
}
if(iter->second.second)
{
occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
}
tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
}
for(int y=1; y<map_.rows-1; ++y)
{
for(int x=1; x<map_.cols-1; ++x)
{
float * info = mapInfo_.ptr<float>(y,x);
int nodeId = (int)info[0];
if(nodeId > 0 && map_.at<char>(y,x) >= 0)
if(iter->second.first)
{
std::map<int, Transform>::iterator tter = transforms.find(nodeId);
if(tter != transforms.end() && !uContains(cache_, nodeId))
emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
}
if(iter->second.second)
{
occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
}
tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
}
for(int y=1; y<map_.rows-1; ++y)
{
for(int x=1; x<map_.cols-1; ++x)
{
float * info = mapInfo_.ptr<float>(y,x);
int nodeId = (int)info[0];
if(nodeId > 0 && map_.at<char>(y,x) >= 0)
{
cv::Point3f pt(info[1], info[2], 0.0f);
pt = util3d::transformPoint(pt, tter->second);
if(minX > pt.x)
minX = pt.x;
else if(maxX < pt.x)
maxX = pt.x;
if(minY > pt.y)
minY = pt.y;
else if(maxY < pt.y)
maxY = pt.y;
std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
if(map_.at<char>(y, x) == 0)
std::map<int, Transform>::iterator tter = transforms.find(nodeId);
if(tter != transforms.end() && !uContains(cache_, nodeId))
{
// ground
std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
UASSERT(iter != emptyLocalMaps.end());
UASSERT(jter->second.first < iter->second.cols);
float * ptf = iter->second.ptr<float>(0,jter->second.first++);
ptf[0] = pt.x;
ptf[1] = pt.y;
}
else
{
// obstacle
std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
UASSERT(iter != occupiedLocalMaps.end());
UASSERT(iter!=occupiedLocalMaps.end());
UASSERT(jter->second.second < iter->second.cols);
float * ptf = iter->second.ptr<float>(0,jter->second.second++);
ptf[0] = pt.x;
ptf[1] = pt.y;
cv::Point3f pt(info[1], info[2], 0.0f);
pt = util3d::transformPoint(pt, tter->second);
if(minX > pt.x)
minX = pt.x;
else if(maxX < pt.x)
maxX = pt.x;
if(minY > pt.y)
minY = pt.y;
else if(maxY < pt.y)
maxY = pt.y;
std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
if(map_.at<char>(y, x) == 0)
{
// ground
std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
UASSERT(iter != emptyLocalMaps.end());
UASSERT(jter->second.first < iter->second.cols);
float * ptf = iter->second.ptr<float>(0,jter->second.first++);
ptf[0] = pt.x;
ptf[1] = pt.y;
}
else
{
// obstacle
std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
UASSERT(iter != occupiedLocalMaps.end());
UASSERT(iter!=occupiedLocalMaps.end());
UASSERT(jter->second.second < iter->second.cols);
float * ptf = iter->second.ptr<float>(0,jter->second.second++);
ptf[0] = pt.x;
ptf[1] = pt.y;
}
}
}
}
}
UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
}
UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
addedNodes_.clear();
map_ = cv::Mat();
mapInfo_ = cv::Mat();
@@ -527,25 +532,36 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
else if(!map_.empty())
{
// update
minX=xMin_+margin;
minY=yMin_+margin;
minX=xMin_+margin+cellSize_/2.0f;
minY=yMin_+margin+cellSize_/2.0f;
maxX=xMin_+float(map_.cols)*cellSize_ - margin;
maxY=yMin_+float(map_.rows)*cellSize_ - margin;
undefinedSize = false;
}
bool incrementalGraphUpdate = graphChanged && !fullUpdate_;
std::list<std::pair<int, Transform> > poses;
// place negative poses at the end
for(std::map<int, Transform>::const_reverse_iterator iter = posesIn.rbegin(); iter!=posesIn.rend(); ++iter)
int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
UDEBUG("Last id = %d", lastId);
if(lastId >= 0)
{
if(iter->first>0)
{
poses.push_front(*iter);
}
else
for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(lastId); iter!=posesIn.end(); ++iter)
{
poses.push_back(*iter);
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
{
if(iter->first < 0)
{
poses.push_back(*iter);
}
else
{
break;
}
}
}
for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
@@ -662,9 +678,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
{
//Get map size
float xMin = minX-margin;
xMin -= cellSize_/2.0f;
float yMin = minY-margin;
yMin -= cellSize_/2.0f;
float xMax = maxX+margin;
float yMax = maxY+margin;
if(fabs((yMax - yMin) / cellSize_) > 99999 ||
fabs((xMax - xMin) / cellSize_) > 99999)
{
@@ -675,7 +694,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
else
{
UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, xMin_, yMin_, xMax, yMax);
cv::Size newMapSize((xMax - xMin) / cellSize_ + 0.5f, (yMax - yMin) / cellSize_ + 0.5f);
cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
if(map_.empty())
{
UDEBUG("Map empty!");
@@ -695,6 +714,11 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
}
else
{
UASSERT(xMin <= xMin_);
UASSERT(yMin <= yMin_);
UASSERT(xMax >= xMin_+float(map_.cols)*cellSize_);
UASSERT(yMax >= yMin_+float(map_.rows)*cellSize_);
UDEBUG("Copy map");
// copy the old map in the new map
// make sure the translation is cellSize
@@ -711,8 +735,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
yMin = yMin_-float(deltaY)*cellSize_;
}
UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
newMapSize.width = (xMax - xMin) / cellSize_ + 0.5f;
newMapSize.height = (yMax - yMin) / cellSize_ + 0.5f;
newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height);
UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows);
map = cv::Mat::ones(newMapSize, CV_8S)*-1;
mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
@@ -743,12 +769,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
for(int i=0; i<iter->second.cols; ++i)
{
float * ptf = iter->second.ptr<float>(0,i);
cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
if(value != -2 && (!incrementalGraphUpdate || value==-1))
{
float * info = mapInfo.ptr<float>(pt.y, pt.x);
int nodeId = (int)info[0];
@@ -792,8 +818,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
if(footprintRadius >= cellSize_*1.5f)
{
// place free space under the footprint of the robot
cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()-footprintRadius-yMin)/cellSize_ + 0.5f);
cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()+footprintRadius-yMin)/cellSize_ + 0.5f);
cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize_, (kter->second.y()-footprintRadius-yMin)/cellSize_);
cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize_, (kter->second.y()+footprintRadius-yMin)/cellSize_);
if(ptBegin.x < 0)
ptBegin.x = 0;
if(ptEnd.x >= map.cols)
@@ -839,8 +865,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = float(i) * cellSize_ + xMin_ + 0.5f;
info[2] = float(j) * cellSize_ + yMin_ + 0.5f;
info[1] = float(i) * cellSize_ + xMin;
info[2] = float(j) * cellSize_ + yMin;
cter->second.first+=1;
}
value = -2; // free space (footprint)
@@ -853,7 +879,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
for(int i=0; i<jter->second.cols; ++i)
{
float * ptf = jter->second.ptr<float>(0,i);
cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
@@ -900,36 +926,110 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
}
}
// fill holes and put footprint values to empty (0)
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//cloud->resize(map.rows*map.cols);
//int oi=0;
for(int i=1; i<map.rows-1; ++i)
if(footprintRadius >= cellSize_*1.5f || incrementalGraphUpdate)
{
for(int j=1; j<map.cols-1; ++j)
for(int i=1; i<map.rows-1; ++i)
{
char & value = map.at<char>(i, j);
if(value == -2)
for(int j=1; j<map.cols-1; ++j)
{
value = 0;
}
char & value = map.at<char>(i, j);
if(value == -2)
{
value = 0;
}
char sum = (map.at<char>(i+1, j) != -1?1:0) +
(map.at<char>(i-1, j) != -1?1:0) +
(map.at<char>(i, j+1) != -1?1:0) +
(map.at<char>(i, j-1) != -1?1:0);
if(value == -1 && sum >=3)
{
value = 0;
}
if(incrementalGraphUpdate && value == -1)
{
float * info = mapInfo.ptr<float>(i, j);
//float * info = mapInfo.ptr<float>(i,j);
//if(info[0] > 0)
//{
// cloud->at(oi).x = info[1];
// cloud->at(oi).y = info[2];
// oi++;
//}
// fill obstacle
if(map.at<char>(i+1, j) == 100 && map.at<char>(i-1, j) == 100)
{
value = 100;
// associate with the nearest pose
if(mapInfo.ptr<float>(i+1, j)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i+1, j)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.second+=1;
}
else if(mapInfo.ptr<float>(i-1, j)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i-1, j)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.second+=1;
}
}
else if(map.at<char>(i, j+1) == 100 && map.at<char>(i, j-1) == 100)
{
value = 100;
// associate with the nearest pose
if(mapInfo.ptr<float>(i, j+1)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i, j+1)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.second+=1;
}
else if(mapInfo.ptr<float>(i, j-1)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i, j-1)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.second+=1;
}
}
else
{
// fill empty
char sum = (map.at<char>(i+1, j) == 0?1:0) +
(map.at<char>(i-1, j) == 0?1:0) +
(map.at<char>(i, j+1) == 0?1:0) +
(map.at<char>(i, j-1) == 0?1:0);
if(sum >=3)
{
value = 0;
// associate with the nearest pose, only check two cases (as 3 are required)
if(map.at<char>(i+1, j) != -1 && mapInfo.ptr<float>(i+1, j)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i+1, j)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.first+=1;
}
else if(map.at<char>(i-1, j) != -1 && mapInfo.ptr<float>(i-1, j)[0]>0.0f)
{
info[0] = mapInfo.ptr<float>(i-1, j)[0];
info[1] = float(j) * cellSize_ + xMin;
info[2] = float(i) * cellSize_ + yMin;
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
UASSERT(cter!=cellCount_.end());
cter->second.first+=1;
}
}
}
}
//float * info = mapInfo.ptr<float>(i,j);
//if(info[0] > 0)
//{
// cloud->at(oi).x = info[1];
// cloud->at(oi).y = info[2];
// oi++;
//}
}
}
}
//if(graphChanged)
@@ -960,7 +1060,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
}
}
cache_.clear();
if(!fullUpdate_)
{
cache_.clear();
}
UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
}
corelib/src/OctoMap.cpp
+234 -211
View File
@@ -35,9 +35,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
OctoMap::OctoMap(float voxelSize, float occupancyThr) :
OctoMap::OctoMap(float voxelSize, float occupancyThr, bool fullUpdate) :
octree_(new octomap::ColorOcTree(voxelSize)),
hasColor_(false)
hasColor_(false),
fullUpdate_(fullUpdate)
{
octree_->setOccupancyThres(occupancyThr);
UASSERT(voxelSize>0.0f);
@@ -67,8 +68,8 @@ void OctoMap::addToCache(int nodeId,
const pcl::PointXYZ & viewPoint)
{
UDEBUG("nodeId=%d", nodeId);
cacheClouds_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
cacheViewPoints_.insert(std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
uInsert(cacheClouds_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
uInsert(cacheViewPoints_, std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
}
void OctoMap::addToCache(int nodeId,
const cv::Mat & ground,
@@ -78,8 +79,8 @@ void OctoMap::addToCache(int nodeId,
UASSERT(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6));
UASSERT(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6));
UDEBUG("nodeId=%d", nodeId);
cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
cacheViewPoints_.insert(std::make_pair(nodeId, viewPoint));
uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
uInsert(cacheViewPoints_, std::make_pair(nodeId, viewPoint));
}
void OctoMap::update(const std::map<int, Transform> & poses)
@@ -113,65 +114,77 @@ void OctoMap::update(const std::map<int, Transform> & poses)
if(graphChanged)
{
UINFO("Graph changed!");
octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
int copied=0;
for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
iter!=occupiedCells_.end();
++iter)
if(fullUpdate_)
{
std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
if(jter != transforms.end())
// clear all but keep cache
octree_->clear();
occupiedCells_.clear();
addedNodes_.clear();
keyRay_ = octomap::KeyRay();
hasColor_ = false;
}
else
{
octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
int copied=0;
for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
iter!=occupiedCells_.end();
++iter)
{
octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
UASSERT(pter != addedNodes_.end());
cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
cvPt = util3d::transformPoint(cvPt, jter->second);
octomap::OcTreeKey key;
if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
if(jter != transforms.end())
{
octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
if(n)
octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
UASSERT(pter != addedNodes_.end());
cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
cvPt = util3d::transformPoint(cvPt, jter->second);
octomap::OcTreeKey key;
if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
{
++copied;
uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
if(n)
{
++copied;
uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
}
else
{
UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
}
}
else
{
UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
}
}
else
else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
{
UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
}
}
else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
{
UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
}
}
UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
delete octree_;
octree_ = newOcTree;
occupiedCells_ = newOccupiedCells;
UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
delete octree_;
octree_ = newOcTree;
occupiedCells_ = newOccupiedCells;
//update added poses
addedNodes_ = updatedAddedNodes;
//update added poses
addedNodes_ = updatedAddedNodes;
}
}
// Original version from A. Hornung:
// https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
//
std::list<std::pair<int, Transform> > orderedPoses;
int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
UDEBUG("Last id = %d", lastId);
if(lastId >= 0)
{
std::list<std::pair<int, Transform> > orderedPoses;
for(std::map<int, Transform>::const_iterator iter=poses.upper_bound(lastId); iter!=poses.end(); ++iter)
{
orderedPoses.push_back(*iter);
@@ -188,173 +201,176 @@ void OctoMap::update(const std::map<int, Transform> & poses)
break;
}
}
}
UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
{
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
std::map<int, cv::Point3f>::iterator viewPointIter;
cloudIter = cacheClouds_.find(iter->first);
occupancyIter = cache_.find(iter->first);
viewPointIter = cacheViewPoints_.find(iter->first);
if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
{
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
std::map<int, cv::Point3f>::iterator viewPointIter;
cloudIter = cacheClouds_.find(iter->first);
occupancyIter = cache_.find(iter->first);
viewPointIter = cacheViewPoints_.find(iter->first);
if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
UASSERT(viewPointIter != cacheViewPoints_.end());
octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
octomap::OcTreeKey tmpKey;
if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
{
UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
UASSERT(viewPointIter != cacheViewPoints_.end());
octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
octomap::OcTreeKey tmpKey;
if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
{
UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
}
// instead of direct scan insertion, compute update to filter ground:
octomap::KeySet free_cells, occupied_cells, ground_cells;
// insert ground points only as free:
unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
Eigen::Affine3f t = iter->second.toEigen3f();
for (unsigned int i=0; i<maxGroundPts; ++i)
{
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
pt = pcl::transformPoint(pt, t);
}
else
{
pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
}
octomap::point3d point(pt.x, pt.y, pt.z);
// only clear space (ground points)
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
ground_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
}
else
{
occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
}
}
}
}
UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
// all other points: free on ray, occupied on endpoint:
unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
for (unsigned int i=0; i<maxObstaclePts; ++i)
{
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
pt = pcl::transformPoint(pt, t);
}
else
{
pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
}
octomap::point3d point(pt.x, pt.y, pt.z);
// free cells
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
occupied_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
}
else
{
occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
}
}
}
}
UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
// mark free cells only if not seen occupied in this cloud
for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
{
if (occupied_cells.find(*it) == occupied_cells.end() &&
ground_cells.find(*it) == ground_cells.end())
{
octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
if(n)
{
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
gter = occupiedCells_.find(n);
if(gter != occupiedCells_.end() && gter->second.isObstacle_)
{
occupiedCells_.erase(gter);
}
}
}
}
// compress map
//octree_->prune();
// ignore negative ids as they are temporary clouds
if(iter->first > 0)
{
addedNodes_.insert(*iter);
}
UDEBUG("%d: end", iter->first);
UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
}
else
// instead of direct scan insertion, compute update to filter ground:
octomap::KeySet free_cells, occupied_cells, ground_cells;
// insert ground points only as free:
unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
Eigen::Affine3f t = iter->second.toEigen3f();
for (unsigned int i=0; i<maxGroundPts; ++i)
{
UDEBUG("Did not find %d in cache", iter->first);
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
pt = pcl::transformPoint(pt, t);
}
else
{
pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
}
octomap::point3d point(pt.x, pt.y, pt.z);
// only clear space (ground points)
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
ground_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
}
else
{
occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
}
}
}
}
UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
// all other points: free on ray, occupied on endpoint:
unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
for (unsigned int i=0; i<maxObstaclePts; ++i)
{
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
pt = pcl::transformPoint(pt, t);
}
else
{
pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
}
octomap::point3d point(pt.x, pt.y, pt.z);
// free cells
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
occupied_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
}
else
{
occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
}
}
}
}
UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
// mark free cells only if not seen occupied in this cloud
for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
{
if (occupied_cells.find(*it) == occupied_cells.end() &&
ground_cells.find(*it) == ground_cells.end())
{
octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
if(n)
{
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
gter = occupiedCells_.find(n);
if(gter != occupiedCells_.end() && gter->second.isObstacle_)
{
occupiedCells_.erase(gter);
}
}
}
}
// compress map
//octree_->prune();
// ignore negative ids as they are temporary clouds
if(iter->first > 0)
{
addedNodes_.insert(*iter);
}
UDEBUG("%d: end", iter->first);
}
else
{
UDEBUG("Did not find %d in cache", iter->first);
}
}
cache_.clear();
cacheClouds_.clear();
cacheViewPoints_.clear();
if(!fullUpdate_)
{
cache_.clear();
cacheClouds_.clear();
cacheViewPoints_.clear();
}
}
void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
@@ -494,9 +510,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
return cloud;
}
cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize)
cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize, unsigned int treeDepth)
{
gridCellSize = octree_->getResolution();
UASSERT(treeDepth <= octree_->getTreeDepth());
if(treeDepth == 0)
{
treeDepth = octree_->getTreeDepth();
}
gridCellSize = octree_->getNodeSize(treeDepth);
pcl::PointCloud<pcl::PointXYZ>::Ptr ground(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>);
@@ -505,16 +527,15 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
obstacles->resize(octree_->size());
int gi=0;
int oi=0;
for (octomap::ColorOcTree::iterator it = octree_->begin(octree_->getTreeDepth()); it != octree_->end(); ++it)
for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(octree_->isNodeOccupied(*it))
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*obstacles)[oi++] = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
}
else
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*ground)[gi++] = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
}
}
@@ -531,17 +552,19 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
}
cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);
cv::Vec2f * ptr = obstaclesMat.ptr<cv::Vec2f>(0,0);
for(unsigned int i=0;i<obstacles->size(); ++i)
{
obstaclesMat.at<cv::Vec2f>(i)[0] = obstacles->at(i).x;
obstaclesMat.at<cv::Vec2f>(i)[1] = obstacles->at(i).y;
ptr[i][0] = obstacles->at(i).x;
ptr[i][1] = obstacles->at(i).y;
}
cv::Mat groundMat = cv::Mat(1, (int)ground->size(), CV_32FC2);
ptr = groundMat.ptr<cv::Vec2f>(0,0);
for(unsigned int i=0;i<ground->size(); ++i)
{
groundMat.at<cv::Vec2f>(i)[0] = ground->at(i).x;
groundMat.at<cv::Vec2f>(i)[1] = ground->at(i).y;
ptr[i][0] = ground->at(i).x;
ptr[i][1] = ground->at(i).y;
}
std::map<int, Transform> poses;
corelib/src/util3d_mapping.cpp
+29
View File
@@ -877,6 +877,35 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
return map8U;
}
cv::Mat erodeMap(const cv::Mat & map)
{
UASSERT(map.type() == CV_8SC1);
cv::Mat erodedMap = map.clone();
for(int i=0; i<map.rows; ++i)
{
for(int j=0; j<map.cols; ++j)
{
if(map.at<char>(i, j) == 100)
{
// remove obstacles which touch at least 3 empty cells but not unknown cells
int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
(map.at<char>(i-1, j) == 0?1:0) +
(map.at<char>(i, j+1) == 0?1:0) +
(map.at<char>(i, j-1) == 0?1:0);
if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
map.at<char>(i-1, j) != -1 &&
map.at<char>(i, j+1) != -1 &&
map.at<char>(i, j-1) != -1)
{
erodedMap.at<char>(i, j) = 0; // empty
}
}
}
}
return erodedMap;
}
}
}
guilib/include/rtabmap/gui/MainWindow.h
-3
View File
@@ -301,9 +301,6 @@ private:
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;
std::map<int, std::pair<cv::Mat, cv::Mat> > _gridLocalMaps; // <ground, obstacles>
std::map<int, cv::Point3f> _gridViewPoints;
long _cachedGridsMemoryUsage;
rtabmap::OccupancyGrid * _occupancyGrid;
rtabmap::OctoMap * _octomap;
guilib/include/rtabmap/gui/PreferencesDialog.h
+1 -2
View File
@@ -169,7 +169,7 @@ public:
bool isOctomapCubeRendering() const;
bool isOctomap2dGrid() const;
int getOctomapTreeDepth() const;
bool isOctomapGroundAnObstacle() const;
bool isOctomapFullUpdate() const;
double getOctomapOccupancyThr() const;
int getOctomapPointSize() const;
int getCloudDecimation(int index) const; // 0=map, 1=odom
@@ -199,7 +199,6 @@ public:
bool getGridMapShown() const;
double getGridMapResolution() const;;
bool isGridMapEroded() const;
bool isGridMapIncremental() const;
double getGridMapFootprintRadius() const;
bool isGridMapFrom3DCloud() const;
bool projMapFrame() const;
guilib/src/DatabaseViewer.cpp
+5 -1
View File
@@ -2673,6 +2673,10 @@ void DatabaseViewer::update(int value,
localMaps,
ui_->doubleSpinBox_gridCellSize->value(),
xMin, yMin);
//OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
//grid.addToCache(data.id(), localMaps.begin()->second.first, localMaps.begin()->second.second);
//grid.update(poses);
//map8S = grid.getMap(xMin, yMin);
}
if(!map8S.empty())
{
@@ -3892,7 +3896,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
#ifdef RTABMAP_OCTOMAP
if(ui_->checkBox_octomap->isChecked())
{
map = octomap_->createProjectionMap(xMin, yMin, cell, 0);
map = octomap_->createProjectionMap(xMin, yMin, cell, 0, ui_->spinBox_grid_depth->value());
}
else
#endif
guilib/src/MainWindow.cpp
+51 -80
View File
@@ -154,7 +154,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_waypointsIndex(0),
_cachedMemoryUsage(0),
_createdCloudsMemoryUsage(0),
_cachedGridsMemoryUsage(0),
_occupancyGrid(0),
_octomap(0),
_odometryCorrection(Transform::getIdentity()),
@@ -246,7 +245,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_occupancyGrid = new OccupancyGrid(_preferencesDialog->getAllParameters());
#ifdef RTABMAP_OCTOMAP
_octomap = new OctoMap(_preferencesDialog->getGridMapResolution(), _preferencesDialog->getOctomapOccupancyThr());
_octomap = new OctoMap(
_preferencesDialog->getGridMapResolution(),
_preferencesDialog->getOctomapOccupancyThr(),
_preferencesDialog->isOctomapFullUpdate());
#endif
// Timer
@@ -596,7 +598,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->statsToolBox->updateStat("GUI/Refresh stats/ms", false);
_ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", false);
_ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", false);
_ui->statsToolBox->updateStat("GUI/Cache Grids Size/MB", false);
#ifdef RTABMAP_OCTOMAP
_ui->statsToolBox->updateStat("GUI/Octomap Size/MB", false);
#endif
@@ -1856,7 +1857,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
_ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _createdCloudsMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GUI/Cache Grids Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedGridsMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
#ifdef RTABMAP_OCTOMAP
_ui->statsToolBox->updateStat("GUI/Octomap Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _octomap->octree()->memoryUsage()/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
#endif
@@ -2052,16 +2052,10 @@ void MainWindow::updateMapCloud(
}
// occupancy grids
if(iter->first < 0)
{
_gridLocalMaps.erase(iter->first);
_gridViewPoints.erase(iter->first);
}
bool updateGridMap =
((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) ||
(_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) &&
_gridLocalMaps.find(iter->first) == _gridLocalMaps.end();
_occupancyGrid->addedNodes().find(iter->first) == _occupancyGrid->addedNodes().end();
bool updateOctomap = false;
#ifdef RTABMAP_OCTOMAP
updateOctomap =
@@ -2072,41 +2066,27 @@ void MainWindow::updateMapCloud(
if(updateGridMap || updateOctomap)
{
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
if(jter!=_cachedSignatures.end())
if(jter!=_cachedSignatures.end() && jter->sensorData().gridCellSize() > 0.0f)
{
if(_gridLocalMaps.find(iter->first) == _gridLocalMaps.end())
{
cv::Mat ground;
cv::Mat obstacles;
if (jter->sensorData().gridCellSize() > 0.0f)
{
jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
_gridLocalMaps.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
_gridViewPoints.insert(std::make_pair(iter->first, jter->sensorData().gridViewPoint()));
_cachedGridsMemoryUsage += (long)(ground.total()*ground.elemSize() + obstacles.total()*obstacles.elemSize());
cv::Mat ground;
cv::Mat obstacles;
jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
_occupancyGrid->addToCache(iter->first, ground, obstacles);
if (ground.cols || obstacles.cols)
{
_occupancyGrid->addToCache(iter->first, ground, obstacles);
}
}
}
#ifdef RTABMAP_OCTOMAP
if(updateOctomap)
{
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mter = _gridLocalMaps.find(iter->first);
std::map<int, cv::Point3f>::iterator pter = _gridViewPoints.find(iter->first);
if(mter != _gridLocalMaps.end() && pter!=_gridViewPoints.end())
if((ground.empty() || ground.channels() > 2) &&
(obstacles.empty() || obstacles.channels() > 2))
{
if((mter->second.first.empty() || mter->second.first.channels() > 2) &&
(mter->second.second.empty() || mter->second.second.channels() > 2))
{
_octomap->addToCache(iter->first, mter->second.first, mter->second.second, pter->second);
}
else if(!mter->second.first.empty() && !mter->second.second.empty())
{
UWARN("Node %d: Cannot update octomap with 2D occupancy grids.", iter->first);
}
cv::Point3f viewpoint = jter->sensorData().gridViewPoint();
_octomap->addToCache(iter->first, ground, obstacles, viewpoint);
}
else if(!ground.empty() || !obstacles.empty())
{
UWARN("Node %d: Cannot update octomap with 2D occupancy grids.", iter->first);
}
}
#endif
@@ -2384,8 +2364,7 @@ void MainWindow::updateMapCloud(
_ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
}
cv::Mat map8U;
if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) &&
_gridLocalMaps.size())
if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()))
{
float xMin, yMin;
float resolution = _preferencesDialog->getGridMapResolution();
@@ -2393,35 +2372,26 @@ void MainWindow::updateMapCloud(
#ifdef RTABMAP_OCTOMAP
if(_preferencesDialog->isOctomap2dGrid())
{
map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0);
map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0, _preferencesDialog->getOctomapTreeDepth());
}
else
#endif
{
if(_preferencesDialog->isGridMapIncremental())
_occupancyGrid->update(poses, 0, _preferencesDialog->getGridMapFootprintRadius());
if(stats)
{
_occupancyGrid->update(poses, 0, _preferencesDialog->getGridMapFootprintRadius());
if(stats)
{
stats->insert(std::make_pair("GUI/Grid Update/ms", (float)timer.restart()*1000.0f));
}
map8S = _occupancyGrid->getMap(xMin, yMin);
}
else
{
map8S = util3d::create2DMapFromOccupancyLocalMaps(
poses,
_gridLocalMaps,
resolution,
xMin, yMin,
0,
_preferencesDialog->isGridMapEroded(),
_preferencesDialog->getGridMapFootprintRadius());
stats->insert(std::make_pair("GUI/Grid Update/ms", (float)timer.restart()*1000.0f));
}
map8S = _occupancyGrid->getMap(xMin, yMin);
}
if(!map8S.empty())
{
if(_preferencesDialog->isGridMapEroded())
{
map8S = util3d::erodeMap(map8S);
}
//convert to gray scaled map
map8U = util3d::convertMap2Image8U(map8S);
@@ -2516,7 +2486,7 @@ void MainWindow::updateMapCloud(
_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
_ui->actionView_scans->setEnabled(!_createdScans.empty());
#ifdef RTABMAP_OCTOMAP
_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -4477,17 +4447,18 @@ void MainWindow::startDetection()
"progress will not be shown in the GUI."));
}
_occupancyGrid->clear();
_occupancyGrid->parseParameters(parameters);
#ifdef RTABMAP_OCTOMAP
UASSERT(_octomap != 0);
delete _octomap;
_octomap = new OctoMap(
_preferencesDialog->getGridMapResolution(),
_preferencesDialog->getOctomapOccupancyThr());
_preferencesDialog->getOctomapOccupancyThr(),
_preferencesDialog->isOctomapFullUpdate());
#endif
_occupancyGrid->clear();
_occupancyGrid->parseParameters(parameters);
// clear odometry visual stuff
_cloudViewer->removeCloud("cloudOdom");
_cloudViewer->removeCloud("scanOdom");
@@ -5614,8 +5585,6 @@ void MainWindow::clearTheCache()
_previousCloud.second.first.second.reset();
_previousCloud.second.second.reset();
_createdScans.clear();
_gridLocalMaps.clear();
_cachedGridsMemoryUsage = 0;
_createdFeatures.clear();
_cloudViewer->clear();
_cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
@@ -5664,7 +5633,10 @@ void MainWindow::clearTheCache()
// re-create one if the resolution has changed
UASSERT(_octomap != 0);
delete _octomap;
_octomap = new OctoMap(_preferencesDialog->getGridMapResolution(), _preferencesDialog->getOctomapOccupancyThr());
_octomap = new OctoMap(
_preferencesDialog->getGridMapResolution(),
_preferencesDialog->getOctomapOccupancyThr(),
_preferencesDialog->isOctomapFullUpdate());
#endif
_occupancyGrid->clear();
}
@@ -5944,17 +5916,16 @@ void MainWindow::exportGridMap()
else
#endif
{
pixels = util3d::create2DMapFromOccupancyLocalMaps(
poses,
_gridLocalMaps,
gridCellSize,
xMin, yMin,
0,
_preferencesDialog->isGridMapEroded());
pixels = _occupancyGrid->getMap(xMin, yMin);
}
if(!pixels.empty())
{
if(_preferencesDialog->isGridMapEroded())
{
pixels = util3d::erodeMap(pixels);
}
cv::Mat map8U(pixels.rows, pixels.cols, CV_8U);
//convert to gray scaled map
for (int i = 0; i < pixels.rows; ++i)
@@ -6617,7 +6588,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
_ui->actionView_scans->setEnabled(!_createdScans.empty());
#ifdef RTABMAP_OCTOMAP
_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -6679,7 +6650,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
_ui->actionView_scans->setEnabled(!_createdScans.empty());
#ifdef RTABMAP_OCTOMAP
_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -6805,7 +6776,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
_ui->actionView_scans->setEnabled(!_createdScans.empty());
#ifdef RTABMAP_OCTOMAP
_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -6872,7 +6843,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
_ui->actionView_scans->setEnabled(!_createdScans.empty());
#ifdef RTABMAP_OCTOMAP
_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
guilib/src/PreferencesDialog.cpp
+8 -10
View File
@@ -409,6 +409,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->groupBox_octomap, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->spinBox_octomap_treeDepth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_octomap_fullUpdate, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_octomap_2dgrid, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_octomap_show3dMap, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_octomap_cubeRendering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -842,6 +843,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->checkBox_grid_flatObstaclesDetected->setObjectName(Parameters::kGridFlatObstacleDetected().c_str());
_ui->groupBox_grid_fromDepthImage->setObjectName(Parameters::kGridFromDepth().c_str());
_ui->checkBox_grid_projMapFrame->setObjectName(Parameters::kGridMapFrameProjection().c_str());
_ui->checkBox_grid_fullUpdate->setObjectName(Parameters::kGridFullUpdate().c_str());
_ui->doubleSpinBox_grid_maxGroundAngle->setObjectName(Parameters::kGridMaxGroundAngle().c_str());
_ui->spinBox_grid_normalK->setObjectName(Parameters::kGridNormalK().c_str());
_ui->doubleSpinBox_grid_maxGroundHeight->setObjectName(Parameters::kGridMaxGroundHeight().c_str());
@@ -1306,15 +1308,15 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->checkBox_map_shown->setChecked(false);
_ui->doubleSpinBox_map_resolution->setValue(0.05);
_ui->checkBox_map_erode->setChecked(false);
_ui->checkBox_map_incremental->setChecked(false);
_ui->doubleSpinBox_map_footprintRadius->setValue(0);
_ui->doubleSpinBox_map_opacity->setValue(0.75);
_ui->groupBox_octomap->setChecked(false);
_ui->spinBox_octomap_treeDepth->setValue(16);
_ui->checkBox_octomap_fullUpdate->setChecked(false);
_ui->checkBox_octomap_2dgrid->setChecked(true);
_ui->checkBox_octomap_show3dMap->setChecked(true);
_ui->checkBox_octomap_cubeRendering->setChecked(true);
_ui->checkBox_octomap_cubeRendering->setChecked(false);
_ui->spinBox_octomap_pointSize->setValue(5);
_ui->doubleSpinBox_octomap_occupancyThr->setValue(0.5);
}
@@ -1685,12 +1687,12 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_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());
_ui->checkBox_map_erode->setChecked(settings.value("gridMapEroded", _ui->checkBox_map_erode->isChecked()).toBool());
_ui->checkBox_map_incremental->setChecked(settings.value("gridMapIncremental", _ui->checkBox_map_incremental->isChecked()).toBool());
_ui->doubleSpinBox_map_footprintRadius->setValue(settings.value("gridMapFootprintRadius", _ui->doubleSpinBox_map_footprintRadius->value()).toDouble());
_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
_ui->groupBox_octomap->setChecked(settings.value("octomap", _ui->groupBox_octomap->isChecked()).toBool());
_ui->spinBox_octomap_treeDepth->setValue(settings.value("octomap_depth", _ui->spinBox_octomap_treeDepth->value()).toInt());
_ui->checkBox_octomap_fullUpdate->setChecked(settings.value("octomap_full_update", _ui->checkBox_octomap_fullUpdate->isChecked()).toBool());
_ui->checkBox_octomap_2dgrid->setChecked(settings.value("octomap_2dgrid", _ui->checkBox_octomap_2dgrid->isChecked()).toBool());
_ui->checkBox_octomap_show3dMap->setChecked(settings.value("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked()).toBool());
_ui->checkBox_octomap_cubeRendering->setChecked(settings.value("octomap_cube", _ui->checkBox_octomap_cubeRendering->isChecked()).toBool());
@@ -2071,12 +2073,12 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked());
settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value());
settings.setValue("gridMapEroded", _ui->checkBox_map_erode->isChecked());
settings.setValue("gridMapIncremental", _ui->checkBox_map_incremental->isChecked());
settings.setValue("gridMapFootprintRadius", _ui->doubleSpinBox_map_footprintRadius->value());
settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
settings.setValue("octomap", _ui->groupBox_octomap->isChecked());
settings.setValue("octomap_depth", _ui->spinBox_octomap_treeDepth->value());
settings.setValue("octomap_full_update", _ui->checkBox_octomap_fullUpdate->isChecked());
settings.setValue("octomap_2dgrid", _ui->checkBox_octomap_2dgrid->isChecked());
settings.setValue("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked());
settings.setValue("octomap_cube", _ui->checkBox_octomap_cubeRendering->isChecked());
@@ -4064,9 +4066,9 @@ int PreferencesDialog::getOctomapTreeDepth() const
{
return _ui->spinBox_octomap_treeDepth->value();
}
bool PreferencesDialog::isOctomapGroundAnObstacle() const
bool PreferencesDialog::isOctomapFullUpdate() const
{
return _ui->checkBox_grid_groundObstacle->isChecked();
return _ui->checkBox_octomap_fullUpdate->isChecked();
}
double PreferencesDialog::getOctomapOccupancyThr() const
{
@@ -4255,10 +4257,6 @@ bool PreferencesDialog::isGridMapEroded() const
{
return _ui->checkBox_map_erode->isChecked();
}
bool PreferencesDialog::isGridMapIncremental() const
{
return _ui->checkBox_map_incremental->isChecked();
}
double PreferencesDialog::getGridMapFootprintRadius() const
{
return _ui->doubleSpinBox_map_footprintRadius->value();
guilib/src/ui/preferencesDialog.ui
+284 -261
View File
@@ -65,7 +65,7 @@
<x>0</x>
<y>0</y>
<width>673</width>
<height>2718</height>
<height>2735</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -1794,6 +1794,26 @@ Show a yellow background when the number of odometry inliers goes under this thr
</item>
<item>
<layout class="QGridLayout" name="gridLayout_20" columnstretch="0,1">
<item row="1" column="1">
<widget class="QLabel" name="label_159">
<property name="text">
<string>Resolution (cell size).</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_170">
<property name="text">
<string>Opacity.</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_map_opacity">
<property name="suffix">
@@ -1849,30 +1869,10 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_159">
<property name="text">
<string>Resolution (cell size).</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_170">
<property name="text">
<string>Opacity.</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_224">
<property name="text">
<string>Erode. Not used if incremental.</string>
<string>Erode.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -1924,29 +1924,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_332">
<property name="text">
<string>Incremental.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="checkBox_map_incremental">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -1958,7 +1935,17 @@ Show a yellow background when the number of odometry inliers goes under this thr
<bool>true</bool>
</property>
<layout class="QGridLayout" name="gridLayout_72" columnstretch="0,1">
<item row="5" column="1">
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_octomap_show3dMap">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_octomap_treeDepth_4">
<property name="text">
<string>Occupancy threshold.</string>
@@ -1971,20 +1958,20 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_octomap_treeDepth_5">
<property name="text">
<string>Cube rendering. Disable to show as a point cloud (a lot less GPU power required).</string>
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_octomap_occupancyThr">
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="wordWrap">
<bool>true</bool>
<property name="singleStep">
<double>0.050000000000000</double>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
<property name="value">
<double>0.500000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<item row="5" column="1">
<widget class="QLabel" name="label_octomap_treeDepth">
<property name="text">
<string>Octomap maximum tree depth (max 16). The highest depth means the smallest resolution of the map (cell size). At smallest resolution the octomap shows RGB colors. Other resolutions produce z-axis gradient colored octomap.</string>
@@ -1997,7 +1984,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="3" column="0">
<item row="5" column="0">
<widget class="QSpinBox" name="spinBox_octomap_treeDepth">
<property name="minimum">
<number>1</number>
@@ -2010,8 +1997,34 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="spinBox_octomap_pointSize">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>99</number>
</property>
<property name="value">
<number>5</number>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_octomap_treeDepth_7">
<property name="text">
<string>Full update. When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="checkBox_octomap_2dgrid">
<widget class="QCheckBox" name="checkBox_octomap_fullUpdate">
<property name="text">
<string/>
</property>
@@ -2020,10 +2033,10 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_octomap_treeDepth_2">
<item row="2" column="1">
<widget class="QLabel" name="label_octomap_treeDepth_5">
<property name="text">
<string>Show 2D occupancy grid map from OctoMap projection.</string>
<string>Cube rendering. Warning: this requires significant more GPU power.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -2046,30 +2059,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_octomap_show3dMap">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_octomap_occupancyThr">
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.050000000000000</double>
</property>
<property name="value">
<double>0.500000000000000</double>
</property>
</widget>
</item>
<item row="1" column="0">
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_octomap_cubeRendering">
<property name="text">
<string/>
@@ -2079,7 +2069,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_octomap_treeDepth_6">
<property name="text">
<string>Point size. When cube rendering is disabled.</string>
@@ -2092,16 +2082,26 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="spinBox_octomap_pointSize">
<property name="minimum">
<number>1</number>
<item row="1" column="1">
<widget class="QLabel" name="label_octomap_treeDepth_8">
<property name="text">
<string>2D grid map created from OctoMap.</string>
</property>
<property name="maximum">
<number>99</number>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="value">
<number>5</number>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="checkBox_octomap_2dgrid">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
@@ -8421,43 +8421,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<layout class="QVBoxLayout" name="verticalLayout_110">
<item>
<layout class="QGridLayout" name="gridLayout_75" columnstretch="0,1">
<item row="3" column="1">
<widget class="QLabel" name="label_327">
<property name="text">
<string>Minimum ground height (0=disabled).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_326">
<property name="text">
<string>Resolution (cell size).</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_335">
<property name="text">
<string>Footprint filtering height (0=disabled). Footprint length and width should be set.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="0">
<item row="9" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_noiseRadius">
<property name="suffix">
<string> m</string>
@@ -8476,111 +8440,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_maxObstacleHeight">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>9999.000000000000000</double>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_maxGroundHeight">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_321">
<property name="text">
<string>Noise filtering min neighbors.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_325">
<property name="text">
<string>Noise filtering radius (0=disabled). Done after segmentation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_grid_projMapFrame">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_311">
<property name="text">
<string>Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_310">
<property name="text">
<string>Maximum ground height (0=disabled). Should be set if Normals Segmentation Approach is checked below.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_309">
<property name="text">
<string>Maximum obstacles height (0=disabled).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_minGroundHeight">
<property name="suffix">
<string> m</string>
@@ -8602,7 +8462,88 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="13" column="1">
<widget class="QLabel" name="label_335">
<property name="text">
<string>Footprint filtering height (0=disabled). Footprint length and width should be set.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_maxObstacleHeight">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>9999.000000000000000</double>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_maxGroundHeight">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_321">
<property name="text">
<string>Noise filtering min neighbors.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_310">
<property name="text">
<string>Maximum ground height (0=disabled). Should be set if Normals Segmentation Approach is checked below.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_309">
<property name="text">
<string>Maximum obstacles height (0=disabled).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QSpinBox" name="spinBox_grid_noiseMinNeighbors">
<property name="minimum">
<number>1</number>
@@ -8615,7 +8556,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="0">
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_resolution">
<property name="suffix">
<string> m</string>
@@ -8631,7 +8572,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="11" column="1">
<item row="14" column="1">
<widget class="QLabel" name="label_331">
<property name="text">
<string>Laser scan decimation.</string>
@@ -8644,7 +8585,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="11" column="0">
<item row="14" column="0">
<widget class="QSpinBox" name="spinBox_grid_scanDecimation">
<property name="minimum">
<number>1</number>
@@ -8657,7 +8598,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="1">
<item row="11" column="1">
<widget class="QLabel" name="label_333">
<property name="text">
<string>Footprint filtering length (0=disabled).</string>
@@ -8670,7 +8611,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="1">
<item row="12" column="1">
<widget class="QLabel" name="label_334">
<property name="text">
<string>Footprint filtering width (0=disabled). Footprint length should be set.</string>
@@ -8683,26 +8624,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_footprintLength">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>10.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="9" column="0">
<item row="12" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_footprintWidth">
<property name="suffix">
<string> m</string>
@@ -8721,7 +8643,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="10" column="0">
<item row="13" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_footprintHeight">
<property name="suffix">
<string> m</string>
@@ -8740,6 +8662,74 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_332">
<property name="text">
<string>Global occupancy grid full update. When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_327">
<property name="text">
<string>Minimum ground height (0=disabled).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_326">
<property name="text">
<string>Resolution (cell size).</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_325">
<property name="text">
<string>Noise filtering radius (0=disabled). Done after segmentation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_footprintLength">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>10.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="checkbox_rgbd_createOccupancyGrid">
<property name="text">
@@ -8763,6 +8753,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="checkBox_grid_fullUpdate">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_311">
<property name="text">
<string>Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_grid_projMapFrame">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>