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

create2DMap() added minMapSize parameter

Browse Source 
dbViewer: fixed refine Visual

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1962 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-11-03 21:40:54 +00:00
parent 91a6799a40
commit c98db0396b
3 changed files with 16 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/util3d.h
+4 -2
View File
@@ -505,14 +505,16 @@ cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
float cellSize, float cellSize,
float & xMin, float & xMin,
float & yMin, float & yMin,
int fillEmptyRadius = 0); int fillEmptyRadius = 0,
float minMapSize = 0.0f);
cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses, cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans, const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
float cellSize, float cellSize,
bool unknownSpaceFilled, bool unknownSpaceFilled,
float & xMin, float & xMin,
float & yMin); float & yMin,
float minMapSize = 0.0f);
void RTABMAP_EXP rayTrace(const cv::Point2i & start, void RTABMAP_EXP rayTrace(const cv::Point2i & start,
const cv::Point2i & end, const cv::Point2i & end,
corelib/src/util3d.cpp
+11 -3
View File
@@ -2725,16 +2725,18 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
float cellSize, float cellSize,
float & xMin, float & xMin,
float & yMin, float & yMin,
int fillEmptyRadius) int fillEmptyRadius,
float minMapSize)
{ {
UASSERT(fillEmptyRadius >= 0); UASSERT(fillEmptyRadius >= 0);
UASSERT(minMapSize >= 0.0f);
UDEBUG(""); UDEBUG("");
UTimer timer; UTimer timer;
std::map<int, cv::Mat> emptyLocalMaps; std::map<int, cv::Mat> emptyLocalMaps;
std::map<int, cv::Mat> occupiedLocalMaps; std::map<int, cv::Mat> occupiedLocalMaps;
float minX=0.0f, minY=0.0f, maxX=0.0f, maxY=0.0f; float minX=-minMapSize, minY=-minMapSize, maxX=minMapSize, maxY=minMapSize;
bool undefinedSize = true; bool undefinedSize = true;
float x,y,z,toll,pitch,yaw,cosT,sinT; float x,y,z,toll,pitch,yaw,cosT,sinT;
cv::Mat affineTransform(2,3,CV_32FC1); cv::Mat affineTransform(2,3,CV_32FC1);
@@ -2887,12 +2889,18 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
float cellSize, float cellSize,
bool unknownSpaceFilled, bool unknownSpaceFilled,
float & xMin, float & xMin,
float & yMin) float & yMin,
float minMapSize)
{ {
UDEBUG("poses=%d, scans = %d", poses.size(), scans.size()); UDEBUG("poses=%d, scans = %d", poses.size(), scans.size());
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans; std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
pcl::PointCloud<pcl::PointXYZ> minMax; pcl::PointCloud<pcl::PointXYZ> minMax;
if(minMapSize > 0.0f)
{
minMax.push_back(pcl::PointXYZ(-minMapSize, -minMapSize, 0));
minMax.push_back(pcl::PointXYZ(minMapSize, minMapSize, 0));
}
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{ {
if(uContains(scans, iter->first) && scans.at(iter->first)->size()) if(uContains(scans, iter->first) && scans.at(iter->first)->size())
guilib/src/DatabaseViewer.cpp
+1 -1
View File
@@ -1814,7 +1814,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to)
if(!t.isNull()) if(!t.isNull())
{ {
Link newLink(currentLink.from(), currentLink.to(), t*currentLink.transform(), currentLink.type()); Link newLink(currentLink.from(), currentLink.to(), t, currentLink.type());
bool updated = false; bool updated = false;
std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from()); std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());