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

Added viewpoint to laser scan 2D ray tracing

Browse Source
This commit is contained in:
matlabbe
2017-03-06 12:07:59 -05:00
parent 9f65ef199b
commit d1e9525052
5 changed files with 134 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/src/OccupancyGrid.cpp
View File

@@ -201,8 +201,14 @@ void OccupancyGrid::createLocalMap(
{
UDEBUG("2D laser scan");
//2D
viewPoint = cv::Point3f(
node.sensorData().laserScanInfo().localTransform().x(),
node.sensorData().laserScanInfo().localTransform().y(),
node.sensorData().laserScanInfo().localTransform().z());
util3d::occupancy2DFromLaserScan(
util3d::transformLaserScan(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()),
viewPoint,
ground,
obstacles,
cellSize_,
@@ -331,6 +337,7 @@ void OccupancyGrid::createLocalMap(
obstacles = cv::Mat();
util3d::occupancy2DFromLaserScan(
laserScan,
viewPoint,
ground,
obstacles,
cellSize_,

95
corelib/src/util3d_mapping.cpp
View File

@@ -45,6 +45,7 @@ namespace rtabmap
namespace util3d
{
void occupancy2DFromLaserScan(
const cv::Mat & scan,
cv::Mat & ground,
@@ -52,6 +53,26 @@ void occupancy2DFromLaserScan(
float cellSize,
bool unknownSpaceFilled,
float scanMaxRange)
{
cv::Point3f viewpoint(0,0,0);
occupancy2DFromLaserScan(
scan,
viewpoint,
ground,
obstacles,
cellSize,
unknownSpaceFilled,
scanMaxRange);
}
void occupancy2DFromLaserScan(
const cv::Mat & scan,
const cv::Point3f & viewpoint,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize,
bool unknownSpaceFilled,
float scanMaxRange)
{
if(scan.empty())
{
@@ -67,8 +88,11 @@ void occupancy2DFromLaserScan(
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> scans;
scans.insert(std::make_pair(1, obstaclesCloud));
std::map<int, cv::Point3f> viewpoints;
viewpoints.insert(std::make_pair(1, viewpoint));
float xMin, yMin;
cv::Mat map8S = create2DMap(poses, scans, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
cv::Mat map8S = create2DMap(poses, scans, viewpoints, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
// If input ground has already values, add them to map
if(ground.rows == 1 && ground.cols>0 && ground.type() == CV_32FC2)
@@ -482,6 +506,43 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
float & yMin,
float minMapSize,
float scanMaxRange)
{
std::map<int, cv::Point3f > viewpoints;
return create2DMap(poses,
scans,
viewpoints,
cellSize,
unknownSpaceFilled,
xMin,
yMin,
minMapSize,
scanMaxRange);
}
/**
* Create 2d Occupancy grid (CV_8S)
* -1 = unknown
* 0 = empty space
* 100 = obstacle
* @param poses
* @param scans
* @param viewpoints
* @param cellSize m
* @param unknownSpaceFilled if false no fill, otherwise a virtual laser sweeps the unknown space from each pose (stopping on detected obstacle)
* @param xMin
* @param yMin
* @param minMapSize minimum map size in meters
* @param scanMaxRange laser scan maximum range, would be set if unknownSpaceFilled=true
*/
cv::Mat create2DMap(const std::map<int, Transform> & poses,
const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
const std::map<int, cv::Point3f > & viewpoints,
float cellSize,
bool unknownSpaceFilled,
float & xMin,
float & yMin,
float minMapSize,
float scanMaxRange)
{
UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
@@ -494,15 +555,23 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
}
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(uContains(scans, iter->first) && scans.at(iter->first)->size())
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator jter=scans.find(iter->first);
if(jter!=scans.end() && jter->second->size())
{
UASSERT(!iter->second.isNull());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::transformPointCloud(scans.at(iter->first), iter->second);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::transformPointCloud(jter->second, iter->second);
pcl::PointXYZ min, max;
pcl::getMinMax3D(*cloud, min, max);
minMax.push_back(min);
minMax.push_back(max);
minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
if(kter!=viewpoints.end())
{
minMax.push_back(pcl::PointXYZ(iter->second.x()+kter->second.x, iter->second.y()+kter->second.y, iter->second.z()+kter->second.z));
}
localScans.insert(std::make_pair(iter->first, cloud));
}
}
@@ -531,7 +600,13 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{
const Transform & pose = poses.at(iter->first);
cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
cv::Point3f viewpoint(0,0,0);
std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
if(kter!=viewpoints.end())
{
viewpoint = kter->second;
}
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize + 0.5f, ((pose.y()+viewpoint.y)-yMin)/cellSize + 0.5f);
for(unsigned int i=0; i<iter->second->size(); ++i)
{
cv::Point2i end((iter->second->points[i].x-xMin)/cellSize, (iter->second->points[i].y-yMin)/cellSize);
@@ -557,7 +632,13 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
if(scanMaxRange > cellSize)
{
const Transform & pose = poses.at(iter->first);
cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
cv::Point3f viewpoint(0,0,0);
std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
if(kter!=viewpoints.end())
{
viewpoint = kter->second;
}
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize + 0.5f, ((pose.y()+viewpoint.y)-yMin)/cellSize + 0.5f);
//UWARN("maxLength = %f", maxLength);
//rotate counterclockwise from the first point until we pass the last point
@@ -565,8 +646,8 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(a), -sin(a),
sin(a), cos(a));
cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
origin.at<float>(0) = pose.x();
origin.at<float>(1) = pose.y();
origin.at<float>(0) = pose.x()+viewpoint.x;
origin.at<float>(1) = pose.y()+viewpoint.y;
pcl::PointXYZ ptFirst = iter->second->points[0];
pcl::PointXYZ ptLast = iter->second->points[iter->second->points.size()-1];
//if(ptFirst.y > ptLast.y)