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

2d ray tracing: Fixed assert when viewpoint is farther than cellSize*10 from the pose.

Browse Source
This commit is contained in:
matlabbe
2025-02-25 16:09:32 -08:00
parent ec4e076a08
commit 35c3ca20ad
1 changed files with 6 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
corelib/src/util3d_mapping.cpp
View File

@@ -703,10 +703,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
if(end!=start)
{
if(localScans.size() > 1 || map.at<signed char>(end.y, end.x) != 0)
{
rayTrace(start, end, map, true); // trace free space
}
rayTrace(start, end, map, true); // trace free space
}
}
// ray tracing for no hits
@@ -730,14 +727,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
if(end!=start)
{
if(localScans.size() > 1 || map.at<signed char>(end.y, end.x) != 0)
{
rayTrace(start, end, map, true); // trace free space
if(map.at<signed char>(end.y, end.x) == -1)
{
map.at<signed char>(end.y, end.x) = 0; // empty
}
}
rayTrace(start, end, map, true); // trace free space
}
}
}
@@ -823,8 +813,6 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
{
UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
cv::Point2i ptA, ptB;
ptA = start;
@@ -832,6 +820,10 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
// clip end point
ptB.x = std::min(std::max(ptB.x, 0), grid.cols-1);
ptB.y = std::min(std::max(ptB.y, 0), grid.rows-1);
bool swapped = false;
if(slope<-1.0f || slope>1.0f)
{