From 502804076acf052700e24c740adac69f4b30c517 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 14 Nov 2014 15:45:10 +0000 Subject: [PATCH] demo_robot_mapping.launch: fixed "util3d.cpp:3038::rayTrace() Condition (end.x >= 0 && end.x < grid.cols) not met! [end.x=84 grid.cols=84]" fatal error git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1997 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- corelib/src/util3d.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index b0cd7aca..ad4bced7 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -2834,7 +2834,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps( float yMax = maxY+margin; UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax); - map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1; + map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1; for(std::map::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter) { std::map::iterator iter = emptyLocalMaps.find(kter->first); @@ -2930,9 +2930,11 @@ cv::Mat create2DMap(const std::map & poses, float xMax = max.x+cellSize; float yMax = max.y+cellSize; + UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax); + //UTimer timer; - map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1; + map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1; std::vector maxSquaredLength(localScans.size(), 0.0f); int j=0; for(std::map::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)