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

MainWindow: added timing stats on global map creation. Updated util3d::occupancy2DFromLaserScan() and util3d::create2DMap() interfaces (hit/noHit scans in opencv matrix format). Parameter default: Grid/ProjRayTracing=true, Grid/NormalK=true

Browse Source
This commit is contained in:
matlabbe
2017-04-06 12:27:12 -04:00
parent e415dd8504
commit 1d8952867d
13 changed files with 386 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/OccupancyGrid.cpp
View File

@@ -208,6 +208,7 @@ void OccupancyGrid::createLocalMap(
util3d::occupancy2DFromLaserScan(
util3d::transformLaserScan(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()),
cv::Mat(),
viewPoint,
ground,
obstacles,
@@ -345,9 +346,12 @@ void OccupancyGrid::createLocalMap(
if(projRayTracing_)
{
cv::Mat laserScan = obstacles;
cv::Mat laserScanNoHit = ground;
obstacles = cv::Mat();
ground = cv::Mat();
util3d::occupancy2DFromLaserScan(
laserScan,
laserScanNoHit,
viewPoint,
ground,
obstacles,

2
corelib/src/OctoMap.cpp
View File

@@ -107,7 +107,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
}
else
{
UWARN("Updated pose for node %d is not found, some points may not be copied.", jter->first);
UWARN("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
}
}
if(graphChanged)

38
corelib/src/util3d.cpp
View File

@@ -1424,6 +1424,44 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsig
return output;
}
void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max)
{
UASSERT(!laserScan.empty());
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
const float * ptr = laserScan.ptr<float>(0, 0);
min.x = max.x = ptr[0];
min.y = max.y = ptr[1];
min.z = max.z = laserScan.channels() >= 3?ptr[2]:0.0f;
for(int i=1; i<laserScan.cols; ++i)
{
ptr = laserScan.ptr<float>(0, i);
if(ptr[0] < min.x) min.x = ptr[0];
else if(ptr[0] > max.x) max.x = ptr[0];
if(ptr[1] < min.y) min.y = ptr[1];
else if(ptr[1] > max.y) max.y = ptr[1];
if(laserScan.channels() >= 3)
{
if(ptr[2] < min.z) min.z = ptr[2];
else if(ptr[2] > max.z) max.z = ptr[2];
}
}
}
void getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max)
{
cv::Point3f minCV, maxCV;
getMinMax3D(laserScan, minCV, maxCV);
min.x = minCV.x;
min.y = minCV.y;
min.z = minCV.z;
max.x = maxCV.x;
max.y = maxCV.y;
max.z = maxCV.z;
}
// inspired from ROS image_geometry/src/stereo_camera_model.cpp
cv::Point3f projectDisparityTo3D(
const cv::Point2f & pt,

174
corelib/src/util3d_mapping.cpp
View File

@@ -74,7 +74,20 @@ void occupancy2DFromLaserScan(
bool unknownSpaceFilled,
float scanMaxRange)
{
if(scan.empty())
occupancy2DFromLaserScan(scan, cv::Mat(), viewpoint, ground, obstacles, cellSize, unknownSpaceFilled, scanMaxRange);
}
void occupancy2DFromLaserScan(
const cv::Mat & scanHit,
const cv::Mat & scanNoHit,
const cv::Point3f & viewpoint,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize,
bool unknownSpaceFilled,
float scanMaxRange)
{
if(scanHit.empty() && scanNoHit.empty())
{
return;
}
@@ -82,11 +95,8 @@ void occupancy2DFromLaserScan(
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud = util3d::laserScanToPointCloud(scan);
//obstaclesCloud = util3d::voxelize<pcl::PointXYZ>(obstaclesCloud, cellSize);
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> scans;
scans.insert(std::make_pair(1, obstaclesCloud));
std::map<int, std::pair<cv::Mat, cv::Mat> > scans;
scans.insert(std::make_pair(1, std::make_pair(scanHit, scanNoHit)));
std::map<int, cv::Point3f> viewpoints;
viewpoints.insert(std::make_pair(1, viewpoint));
@@ -94,23 +104,6 @@ void occupancy2DFromLaserScan(
float xMin, yMin;
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)
{
for(int i=0; i<ground.cols; ++i)
{
cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
// cell
cv::Point2i cell((ptr[i][0]-xMin)/cellSize, (ptr[i][1]-yMin)/cellSize);
if(cell.x>=0 && cell.x<map8S.cols && cell.y >= 0 && cell.y < map8S.rows && map8S.at<char>(cell.y, cell.x) == -1)
{
map8S.at<char>(cell.y, cell.x) = 0;
}
}
}
// find ground cells
std::list<int> groundIndices;
for(unsigned int i=0; i< map8S.total(); ++i)
@@ -139,17 +132,7 @@ void occupancy2DFromLaserScan(
}
// copy directly obstacles precise positions
obstacles = cv::Mat();
if(obstaclesCloud->size())
{
obstacles = cv::Mat(1, (int)obstaclesCloud->size(), CV_32FC2);
for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
{
cv::Vec2f * ptr = obstacles.ptr<cv::Vec2f>();
ptr[i][0] = obstaclesCloud->at(i).x;
ptr[i][1] = obstaclesCloud->at(i).y;
}
}
obstacles = scanHit.clone();
}
/**
@@ -510,8 +493,39 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
float scanMaxRange)
{
std::map<int, cv::Point3f > viewpoints;
std::map<int, std::pair<cv::Mat, cv::Mat> > scansCv;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter)
{
scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second), cv::Mat())));
}
return create2DMap(poses,
scans,
scansCv,
viewpoints,
cellSize,
unknownSpaceFilled,
xMin,
yMin,
minMapSize,
scanMaxRange);
}
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)
{
std::map<int, std::pair<cv::Mat, cv::Mat> > scansCv;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter)
{
scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second), cv::Mat())));
}
return create2DMap(poses,
scansCv,
viewpoints,
cellSize,
unknownSpaceFilled,
@@ -537,7 +551,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
* @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, std::pair<cv::Mat, cv::Mat> > & scans, // <id, <hit, no hit> >
const std::map<int, cv::Point3f > & viewpoints,
float cellSize,
bool unknownSpaceFilled,
@@ -547,7 +561,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
float scanMaxRange)
{
UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
std::map<int, std::pair<cv::Mat, cv::Mat> > localScans;
pcl::PointCloud<pcl::PointXYZ> minMax;
if(minMapSize > 0.0f)
@@ -557,15 +571,25 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
}
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator jter=scans.find(iter->first);
if(jter!=scans.end() && jter->second->size())
std::map<int, std::pair<cv::Mat, cv::Mat> >::const_iterator jter=scans.find(iter->first);
if(jter!=scans.end() && (jter->second.first.cols || jter->second.second.cols))
{
UASSERT(!iter->second.isNull());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::transformPointCloud(jter->second, iter->second);
cv::Mat hit = util3d::transformLaserScan(jter->second.first, iter->second);
cv::Mat noHit = util3d::transformLaserScan(jter->second.second, iter->second);
pcl::PointXYZ min, max;
pcl::getMinMax3D(*cloud, min, max);
minMax.push_back(min);
minMax.push_back(max);
if(!hit.empty())
{
util3d::getMinMax3D(hit, min, max);
minMax.push_back(min);
minMax.push_back(max);
}
if(!noHit.empty())
{
util3d::getMinMax3D(noHit, 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);
@@ -574,7 +598,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
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));
localScans.insert(std::make_pair(iter->first, std::make_pair(hit, noHit)));
}
}
@@ -599,7 +623,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
int j=0;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{
const Transform & pose = poses.at(iter->first);
cv::Point3f viewpoint(0,0,0);
@@ -609,15 +633,48 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
viewpoint = kter->second;
}
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
for(unsigned int i=0; i<iter->second->size(); ++i)
// Set obstacles first
for(unsigned int i=0; i<iter->second.first.cols; ++i)
{
cv::Point2i end((iter->second->points[i].x-xMin)/cellSize, (iter->second->points[i].y-yMin)/cellSize);
const float * ptr = iter->second.first.ptr<float>(0, i);
cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
if(end!=start)
{
rayTrace(start, end, map, true); // trace free space
map.at<char>(end.y, end.x) = 100; // obstacle
}
}
// ray tracing for hits
for(unsigned int i=0; i<iter->second.first.cols; ++i)
{
const float * ptr = iter->second.first.ptr<float>(0, i);
cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
if(end!=start)
{
if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
{
rayTrace(start, end, map, true); // trace free space
}
}
}
// ray tracing for no hits
for(unsigned int i=0; i<iter->second.second.cols; ++i)
{
const float * ptr = iter->second.second.ptr<float>(0, i);
cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
if(end!=start)
{
if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
{
rayTrace(start, end, map, true); // trace free space
if(map.at<char>(end.y, end.x) == -1)
{
map.at<char>(end.y, end.x) = 0; // empty
}
}
}
}
++j;
}
UDEBUG("Ray trace known space=%fs", timer.ticks());
@@ -627,9 +684,9 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
{
j=0;
float a = CV_PI/256.0f; // angle increment
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{
if(iter->second->size() > 1)
if(iter->second.first.cols > 1)
{
if(scanMaxRange > cellSize)
{
@@ -650,19 +707,10 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
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)
//{
// swap to iterate counterclockwise
// pcl::PointXYZ tmp = ptLast;
// ptLast = ptFirst;
// ptFirst = tmp;
//}
endFirst.at<float>(0) = ptFirst.x;
endFirst.at<float>(1) = ptFirst.y;
endLast.at<float>(0) = ptLast.x;
endLast.at<float>(1) = ptLast.y;
endFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0];
endFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1];
endLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[0];
endLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[1];
//UWARN("origin = %f %f", origin.at<float>(0), origin.at<float>(1));
//UWARN("endFirst = %f %f", endFirst.at<float>(0), endFirst.at<float>(1));
//UWARN("endLast = %f %f", endLast.at<float>(0), endLast.at<float>(1));