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

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

22
corelib/include/rtabmap/core/OccupancyGrid.h
View File

@@ -70,18 +70,8 @@ public:
cv::Mat & emptyCells,
cv::Point3f & viewPoint) const;
template<typename PointT>
void 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;
template<typename PointT>
void createLocalMap(
const typename pcl::PointCloud<PointT>::Ptr cloud, // in base_link frame
const pcl::IndicesPtr & indices,
const LaserScan & cloud,
const Transform & pose,
cv::Mat & groundCells,
cv::Mat & obstacleCells,
@@ -100,16 +90,6 @@ public:
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
private:
void createLocalMapImpl(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & groundCloud,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstaclesCloud,
const Transform & pose,
cv::Mat & groundCells,
cv::Mat & obstacleCells,
cv::Mat & emptyCells,
const cv::Point3f & viewPoint) const;
private:
ParametersMap parameters_;
int cloudDecimation_;

6
corelib/include/rtabmap/core/OctoMap.h
View File

@@ -210,6 +210,10 @@ public:
void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
void setMaxRange(float value) {rangeMax_ = value;}
void setRayTracing(bool enabled) {rayTracing_ = enabled;}
bool hasColor() const {return hasColor_;}
private:
void updateMinMax(const octomap::point3d & point);
@@ -223,6 +227,8 @@ private:
bool hasColor_;
bool fullUpdate_;
float updateError_;
float rangeMax_;
bool rayTracing_;
double minValues_[3];
double maxValues_[3];
};

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());
}
}

3
corelib/include/rtabmap/core/util3d.h
View File

@@ -199,8 +199,10 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud);
// return CV_32FC3 (x,y,z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform());
// return CV_32FC6 (x,y,z,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// return CV_32FC4 (x,y,z,rgb)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
@@ -211,6 +213,7 @@ cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI
// return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform());
// return CV_32FC7 (x,y,z,I,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform());

9
corelib/include/rtabmap/core/util3d_filtering.h
View File

@@ -620,11 +620,10 @@ pcl::IndicesPtr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative);
// PCL default lacks of pcl::PointNormal type support
//pcl::IndicesPtr RTABMAP_EXP extractIndices(
// const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
// const pcl::IndicesPtr & indices,
// bool negative);
pcl::IndicesPtr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative);
pcl::IndicesPtr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,

228
corelib/src/OccupancyGrid.cpp
View File

@@ -268,32 +268,22 @@ void OccupancyGrid::createLocalMap(
UDEBUG("3D laser scan");
const Transform & t = node.sensorData().laserScanRaw().localTransform();
LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
if(cloudMinDepth_ > 0.0f || cloudMaxDepth_ > 0.0f)
#ifdef RTABMAP_OCTOMAP
// clipping will be done in OctoMap
float maxRange = grid3D_&&rayTracing_?0.0f:cloudMaxDepth_;
#else
float maxRange = cloudMaxDepth_;
#endif
if(cloudMinDepth_ > 0.0f || maxRange > 0.0f)
{
scan = util3d::rangeFiltering(scan, cloudMinDepth_, cloudMaxDepth_);
scan = util3d::rangeFiltering(scan, cloudMinDepth_, maxRange);
}
// update viewpoint
viewPoint = cv::Point3f(t.x(), t.y(), t.z());
UDEBUG("scan format=%d", scan.format());
if(scan.hasNormals())
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, t);
pcl::io::savePCDFile("test.pcd", *cloud);
createLocalMap<pcl::PointXYZRGBNormal>(cloud, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
}
else if(scan.hasRGB())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, t);
createLocalMap<pcl::PointXYZRGB>(cloud, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, t);
createLocalMap<pcl::PointXYZ>(cloud, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
}
createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
}
else
{
@@ -311,7 +301,12 @@ void OccupancyGrid::createLocalMap(
cloud = util3d::cloudRGBFromSensorData(
node.sensorData(),
cloudDecimation_,
#ifdef RTABMAP_OCTOMAP
// clipping will be done in OctoMap
grid3D_&&rayTracing_?0.0f:cloudMaxDepth_,
#else
cloudMaxDepth_,
#endif
cloudMinDepth_,
indices.get(),
parameters_,
@@ -345,79 +340,168 @@ void OccupancyGrid::createLocalMap(
const Transform & t = node.sensorData().stereoCameraModel().localTransform();
viewPoint = cv::Point3f(t.x(), t.y(), t.z());
}
createLocalMap<pcl::PointXYZRGB>(cloud, indices, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f, LaserScan::kXYZRGB), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
}
}
}
void OccupancyGrid::createLocalMapImpl(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & groundCloud,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstaclesCloud,
void OccupancyGrid::createLocalMap(
const LaserScan & scan,
const Transform & pose,
cv::Mat & groundCells,
cv::Mat & obstacleCells,
cv::Mat & emptyCells,
const cv::Point3f & viewPoint) const
cv::Point3f & viewPointInOut) const
{
if(grid3D_)
if(projMapFrame_)
{
UDEBUG("ground=%d obstacles=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
if(groundIsObstacle_)
{
*obstaclesCloud += *groundCloud;
groundCloud->clear();
}
// transform back in base frame
//we should rotate viewPoint in /map frame
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();
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(rayTracing_)
if(scan.size())
{
pcl::IndicesPtr groundIndices(new std::vector<int>);
pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
cv::Mat groundCloud;
cv::Mat obstaclesCloud;
if(scan.hasRGB() && scan.hasNormals())
{
#ifdef RTABMAP_OCTOMAP
if(!groundCloud->empty() || !obstaclesCloud->empty())
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGBNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
if(grid3D_)
{
//create local octomap
OctoMap octomap(cellSize_);
octomap.addToCache(1, groundCloud, obstaclesCloud, pcl::PointXYZ(viewPoint.x, viewPoint.y, viewPoint.z));
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
octomap.update(poses);
pcl::IndicesPtr groundIndices(new std::vector<int>);
pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
pcl::IndicesPtr emptyIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
}
else
{
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGBNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
}
}
else if(scan.hasRGB())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGB>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
if(grid3D_)
{
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
}
else
{
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
}
}
else if(scan.hasNormals())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
if(grid3D_)
{
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
}
else
{
util3d::occupancy2DFromGroundObstacles<pcl::PointNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
}
}
else
#else
UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str());
}
#endif
{
groundCells = util3d::laserScanFromPointCloud(*groundCloud, tinv);
obstacleCells = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZ>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
if(grid3D_)
{
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
}
else
{
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
}
}
}
else
{
UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
// projection on the xy plane
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
groundCloud,
obstaclesCloud,
groundCells,
obstacleCells,
cellSize_);
if(grid3D_ && (!obstaclesCloud.empty() || !groundCloud.empty()))
{
UDEBUG("ground=%d obstacles=%d", groundCloud.cols, obstaclesCloud.cols);
if(groundIsObstacle_ && !groundCloud.empty())
{
if(obstaclesCloud.empty())
{
obstaclesCloud = groundCloud;
groundCloud = cv::Mat();
}
else
{
UASSERT(obstaclesCloud.type() == groundCloud.type());
cv::Mat merged(1,obstaclesCloud.cols+groundCloud.cols, obstaclesCloud.type());
obstaclesCloud.copyTo(merged(cv::Range::all(), cv::Range(0, obstaclesCloud.cols)));
groundCloud.copyTo(merged(cv::Range::all(), cv::Range(obstaclesCloud.cols, obstaclesCloud.cols+groundCloud.cols)));
}
}
if(rayTracing_)
// transform back in base frame
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();
if(rayTracing_)
{
#ifdef RTABMAP_OCTOMAP
if(!groundCloud.empty() || !obstaclesCloud.empty())
{
//create local octomap
OctoMap octomap(cellSize_);
octomap.setMaxRange(cloudMaxDepth_);
octomap.addToCache(1, groundCloud, obstaclesCloud, cv::Mat(), cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
octomap.update(poses);
pcl::IndicesPtr groundIndices(new std::vector<int>);
pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
pcl::IndicesPtr emptyIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
if(scan.hasRGB())
{
groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithRayTracing2(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloudWithRayTracing, *cloudWithRayTracing2);
groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv);
obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv);
emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv);
}
}
}
else
#else
UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str());
}
#endif
{
groundCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(groundCloud), tinv).data();
obstacleCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(obstaclesCloud), tinv).data();
}
}
else if(!grid3D_ && rayTracing_ && (!obstacleCells.empty() || !groundCells.empty()))
{
cv::Mat laserScan = obstacleCells;
cv::Mat laserScanNoHit = groundCells;
@@ -426,7 +510,7 @@ void OccupancyGrid::createLocalMapImpl(
util3d::occupancy2DFromLaserScan(
laserScan,
laserScanNoHit,
viewPoint,
viewPointInOut,
emptyCells,
obstacleCells,
cellSize_,
@@ -434,9 +518,9 @@ void OccupancyGrid::createLocalMapImpl(
0);
}
}
UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
}
void OccupancyGrid::clear()
{
cache_.clear();

225
corelib/src/OctoMap.cpp
View File

@@ -265,7 +265,9 @@ RtabmapColorOcTree::StaticMemberInitializer::StaticMemberInitializer() {
OctoMap::OctoMap(const ParametersMap & parameters) :
hasColor_(false),
fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
updateError_(Parameters::defaultGridGlobalUpdateError())
updateError_(Parameters::defaultGridGlobalUpdateError()),
rangeMax_(Parameters::defaultGridRangeMax()),
rayTracing_(Parameters::defaultGridRayTracing())
{
float cellSize = Parameters::defaultGridCellSize();
Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
@@ -281,13 +283,17 @@ OctoMap::OctoMap(const ParametersMap & parameters) :
octree_->setOccupancyThres(occupancyThr);
Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
}
OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate, float updateError) :
octree_(new RtabmapColorOcTree(cellSize)),
hasColor_(false),
fullUpdate_(fullUpdate),
updateError_(updateError)
updateError_(updateError),
rangeMax_(0.0f),
rayTracing_(true)
{
minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
@@ -511,6 +517,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
}
UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
float rangeMaxSqrd = rangeMax_*rangeMax_;
float cellSize = octree_->getResolution();
for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
{
std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
@@ -536,7 +544,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
}
bool computeRays = occupancyIter == cache_.end() || occupancyIter->second.second.empty();
bool computeRays = rayTracing_ && (occupancyIter == cache_.end() || occupancyIter->second.second.empty());
// instead of direct scan insertion, compute update to filter ground:
octomap::KeySet free_cells;
@@ -563,6 +571,56 @@ void OctoMap::update(const std::map<int, Transform> & poses)
pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
}
octomap::point3d point(pt.x, pt.y, pt.z);
bool ignoreOccupiedCell = false;
if(rangeMaxSqrd > 0.0f)
{
octomap::point3d v(pt.x - cellSize - sensorOrigin.x(), pt.y - cellSize - sensorOrigin.y(), pt.z - cellSize - sensorOrigin.z());
if(v.norm_sq() > rangeMaxSqrd)
{
// compute new point to max range
v.normalize();
v*=rangeMax_;
point = sensorOrigin + v;
ignoreOccupiedCell=true;
}
}
if(!ignoreOccupiedCell)
{
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
if(iter->first >0 && iter->first<lastId)
{
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
updateMinMax(point);
RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
n->setNodeRefId(iter->first);
n->setPointRef(point);
}
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeGround);
}
}
}
// only clear space (ground points)
if (computeRays &&
(iter->first < 0 || iter->first>lastId) &&
@@ -570,38 +628,6 @@ void OctoMap::update(const std::map<int, Transform> & poses)
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
if(iter->first >0 && iter->first<lastId)
{
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
updateMinMax(point);
RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
n->setNodeRefId(iter->first);
n->setPointRef(point);
}
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeGround);
}
}
}
UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size());
@@ -629,6 +655,56 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::point3d point(pt.x, pt.y, pt.z);
bool ignoreOccupiedCell = false;
if(rangeMaxSqrd > 0.0f)
{
octomap::point3d v(pt.x - cellSize - sensorOrigin.x(), pt.y - cellSize - sensorOrigin.y(), pt.z - cellSize - sensorOrigin.z());
if(v.norm_sq() > rangeMaxSqrd)
{
// compute new point to max range
v.normalize();
v*=rangeMax_;
point = sensorOrigin + v;
ignoreOccupiedCell=true;
}
}
if(!ignoreOccupiedCell)
{
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
if(iter->first >0 && iter->first<lastId)
{
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
updateMinMax(point);
RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
n->setNodeRefId(iter->first);
n->setPointRef(point);
}
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeObstacle);
}
}
}
// free cells
if (computeRays &&
(iter->first < 0 || iter->first>lastId) &&
@@ -636,38 +712,6 @@ void OctoMap::update(const std::map<int, Transform> & poses)
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
// occupied endpoint
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
if(iter->first >0 && iter->first<lastId)
{
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
updateMinMax(point);
RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
n->setNodeRefId(iter->first);
n->setPointRef(point);
}
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeObstacle);
}
}
}
UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (int)free_cells.size());
@@ -711,29 +755,42 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::point3d point(pt.x, pt.y, pt.z);
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
bool ignoreCell = false;
if(rangeMaxSqrd > 0.0f)
{
if(iter->first >0)
octomap::point3d v(pt.x - sensorOrigin.x(), pt.y - sensorOrigin.y(), pt.z - sensorOrigin.z());
if(v.norm_sq() > rangeMaxSqrd)
{
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
{
// The cell has been updated from current node or more recent node, don't update the cell
continue;
}
ignoreCell=true;
}
}
updateMinMax(point);
RtabmapColorOcTreeNode * n = octree_->updateNode(key, false);
if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
if(!ignoreCell)
{
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
if(iter->first > 0)
if(iter->first >0)
{
n->setNodeRefId(iter->first);
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
{
// The cell has been updated from current node or more recent node, don't update the cell
continue;
}
}
updateMinMax(point);
RtabmapColorOcTreeNode * n = octree_->updateNode(key, false);
if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
{
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
if(iter->first > 0)
{
n->setNodeRefId(iter->first);
}
}
}
}

205
corelib/src/util3d.cpp
View File

@@ -1487,55 +1487,120 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform)
{
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
if(indices.get())
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
laserScan = cv::Mat(1, (int)indices->size(), CV_32FC3);
for(unsigned int i=0; i<indices->size(); ++i)
{
pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
int index = indices->at(i);
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZ pt = pcl::transformPoint(cloud.at(index), transform3f);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
}
else
{
ptr[0] = cloud.at(index).x;
ptr[1] = cloud.at(index).y;
ptr[2] = cloud.at(index).z;
}
}
else
}
else
{
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC3);
for(unsigned int i=0; i<cloud.size(); ++i)
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
}
}
}
return laserScan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform)
{
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
for(unsigned int i=0; i<cloud.size(); ++i)
if(indices.get())
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(6));
for(unsigned int i=0; i<indices->size(); ++i)
{
pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
ptr[3] = pt.normal_x;
ptr[4] = pt.normal_y;
ptr[5] = pt.normal_z;
int index = indices->at(i);
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointNormal pt = util3d::transformPoint(cloud.at(index), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
ptr[3] = pt.normal_x;
ptr[4] = pt.normal_y;
ptr[5] = pt.normal_z;
}
else
{
ptr[0] = cloud.at(index).x;
ptr[1] = cloud.at(index).y;
ptr[2] = cloud.at(index).z;
ptr[3] = cloud.at(index).normal_x;
ptr[4] = cloud.at(index).normal_y;
ptr[5] = cloud.at(index).normal_z;
}
}
else
}
else
{
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
for(unsigned int i=0; i<cloud.size(); ++i)
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
ptr[3] = cloud.at(i).normal_x;
ptr[4] = cloud.at(i).normal_y;
ptr[5] = cloud.at(i).normal_z;
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
ptr[3] = pt.normal_x;
ptr[4] = pt.normal_y;
ptr[5] = pt.normal_z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
ptr[3] = cloud.at(i).normal_x;
ptr[4] = cloud.at(i).normal_y;
ptr[5] = cloud.at(i).normal_z;
}
}
}
return laserScan;
@@ -1739,32 +1804,70 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform)
{
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
for(unsigned int i=0; i<cloud.size(); ++i)
if(indices.get())
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(7));
for(unsigned int i=0; i<indices->size(); ++i)
{
pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(i), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
ptr[4] = pt.normal_x;
ptr[5] = pt.normal_y;
ptr[6] = pt.normal_z;
int index = indices->at(i);
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(index), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
ptr[4] = pt.normal_x;
ptr[5] = pt.normal_y;
ptr[6] = pt.normal_z;
}
else
{
ptr[0] = cloud.at(index).x;
ptr[1] = cloud.at(index).y;
ptr[2] = cloud.at(index).z;
ptr[4] = cloud.at(index).normal_x;
ptr[5] = cloud.at(index).normal_y;
ptr[6] = cloud.at(index).normal_z;
}
int * ptrInt = (int*)ptr;
ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
}
else
}
else
{
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7));
for(unsigned int i=0; i<cloud.size(); ++i)
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
ptr[4] = cloud.at(i).normal_x;
ptr[5] = cloud.at(i).normal_y;
ptr[6] = cloud.at(i).normal_z;
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(i), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
ptr[4] = pt.normal_x;
ptr[5] = pt.normal_y;
ptr[6] = pt.normal_z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
ptr[4] = cloud.at(i).normal_x;
ptr[5] = cloud.at(i).normal_y;
ptr[6] = cloud.at(i).normal_z;
}
int * ptrInt = (int*)ptr;
ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
}
int * ptrInt = (int*)ptr;
ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
}
return laserScan;
}

9
corelib/src/util3d_filtering.cpp
View File

@@ -1640,11 +1640,10 @@ pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud
{
return extractIndicesImpl<pcl::PointXYZ>(cloud, indices, negative);
}
// PCL default lacks of pcl::PointNormal type support
//pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
//{
// return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative);
//}
pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
{
return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative);
}
pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
{
return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative);