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

Added notification message box when a global path is computed. Refactoring occupancy grid.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-05 17:38:38 -04:00
parent b2bfa91153
commit f32319f0dc
9 changed files with 117 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
corelib/src/util3d.cpp
View File

@@ -2595,12 +2595,32 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
cv::Mat affineTransform(2,3,CV_32FC1);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
if(undefinedSize)
{
minX = maxX = x;
minY = maxY = y;
undefinedSize = false;
}
else
{
if(minX > x)
minX = x;
else if(maxX < x)
maxX = x;
if(minY > y)
minY = y;
else if(maxY < y)
maxY = y;
}
if(uContains(occupancy, iter->first))
{
UASSERT(!iter->second.isNull());
const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
cosT = cos(yaw);
sinT = sin(yaw);
affineTransform.at<float>(0,0) = cosT;
@@ -2610,25 +2630,6 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
affineTransform.at<float>(0,2) = x;
affineTransform.at<float>(1,2) = y;
if(undefinedSize)
{
minX = maxX = x;
minY = maxY = y;
undefinedSize = false;
}
else
{
if(minX > x)
minX = x;
else if(maxX < x)
maxX = x;
if(minY > y)
minY = y;
else if(maxY < y)
maxY = y;
}
//ground
if(pair.first.rows)
{