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

Fixed occupancy grid created from PointNormal with RGB

Browse Source
This commit is contained in:
matlabbe
2018-02-18 15:10:25 -05:00
parent d181bedbfc
commit 07244a8a73
11 changed files with 546 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
corelib/include/rtabmap/core/impl/OccupancyGrid.hpp
View File

@@ -186,74 +186,6 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
return cloud;
}
template<typename PointT>
void OccupancyGrid::createLocalMap(
const typename pcl::PointCloud<PointT>::Ptr cloud, // in base_link frame
const Transform & pose,
cv::Mat & groundCells,
cv::Mat & obstacleCells,
cv::Mat & emptyCells,
cv::Point3f & viewPointInOut) const
{
pcl::IndicesPtr indices(new std::vector<int>);
createLocalMap<PointT>(cloud, indices, pose, groundCells, obstacleCells, emptyCells, viewPointInOut);
}
template<typename PointT>
void OccupancyGrid::createLocalMap(
const typename pcl::PointCloud<PointT>::Ptr cloud, // in base_link frame
const pcl::IndicesPtr & indices,
const Transform & pose,
cv::Mat & groundCells,
cv::Mat & obstacleCells,
cv::Mat & emptyCells,
cv::Point3f & viewPointInOut) const
{
if(projMapFrame_)
{
//we should rotate viewPoint in /map frame
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0);
viewPointInOut.x = viewpointRotated.x();
viewPointInOut.y = viewpointRotated.y();
viewPointInOut.z = viewpointRotated.z();
}
if((cloud->is_dense && cloud->size()) ||
(!cloud->is_dense && indices->size()))
{
pcl::IndicesPtr groundIndices(new std::vector<int>);
pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
typename pcl::PointCloud<PointT>::Ptr cloudSegmented = segmentCloud<PointT>(
cloud,
indices,
pose,
viewPointInOut,
groundIndices,
obstaclesIndices);
if(!groundIndices->empty() || !obstaclesIndices->empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(groundIndices->size())
{
pcl::copyPointCloud(*cloudSegmented, *groundIndices, *groundCloud);
}
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloudSegmented, *obstaclesIndices, *obstaclesCloud);
}
createLocalMapImpl(groundCloud, obstaclesCloud, pose, groundCells, obstacleCells, emptyCells, viewPointInOut);
}
}
UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
}
}