i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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::Mat & emptyCells,
cv::Point3f & viewPoint) const; cv::Point3f & viewPoint) const;
template<typename PointT>
void createLocalMap( void createLocalMap(
const typename pcl::PointCloud<PointT>::Ptr cloud, // in base_link frame const LaserScan & cloud,
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 Transform & pose, const Transform & pose,
cv::Mat & groundCells, cv::Mat & groundCells,
cv::Mat & obstacleCells, cv::Mat & obstacleCells,
@@ -100,16 +90,6 @@ public:
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;} const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;} 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: private:
ParametersMap parameters_; ParametersMap parameters_;
int cloudDecimation_; 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 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 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: private:
void updateMinMax(const octomap::point3d & point); void updateMinMax(const octomap::point3d & point);
@@ -223,6 +227,8 @@ private:
bool hasColor_; bool hasColor_;
bool fullUpdate_; bool fullUpdate_;
float updateError_; float updateError_;
float rangeMax_;
bool rayTracing_;
double minValues_[3]; double minValues_[3];
double maxValues_[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; 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); LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud);
// return CV_32FC3 (x,y,z) // 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 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) // 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 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()); 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) // return CV_32FC4 (x,y,z,rgb)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform()); 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) // 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::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 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) // 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::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()); 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::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
bool negative); bool negative);
// PCL default lacks of pcl::PointNormal type support pcl::IndicesPtr RTABMAP_EXP extractIndices(
//pcl::IndicesPtr RTABMAP_EXP extractIndices( const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
// const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices,
// const pcl::IndicesPtr & indices, bool negative);
// bool negative);
pcl::IndicesPtr RTABMAP_EXP extractIndices( pcl::IndicesPtr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,

176
corelib/src/OccupancyGrid.cpp
View File

@@ -268,32 +268,22 @@ void OccupancyGrid::createLocalMap(
UDEBUG("3D laser scan"); UDEBUG("3D laser scan");
const Transform & t = node.sensorData().laserScanRaw().localTransform(); const Transform & t = node.sensorData().laserScanRaw().localTransform();
LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_); LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
#ifdef RTABMAP_OCTOMAP
if(cloudMinDepth_ > 0.0f || cloudMaxDepth_ > 0.0f) // 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 // update viewpoint
viewPoint = cv::Point3f(t.x(), t.y(), t.z()); viewPoint = cv::Point3f(t.x(), t.y(), t.z());
UDEBUG("scan format=%d", scan.format()); UDEBUG("scan format=%d", scan.format());
if(scan.hasNormals()) createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
{
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);
}
} }
else else
{ {
@@ -311,7 +301,12 @@ void OccupancyGrid::createLocalMap(
cloud = util3d::cloudRGBFromSensorData( cloud = util3d::cloudRGBFromSensorData(
node.sensorData(), node.sensorData(),
cloudDecimation_, cloudDecimation_,
#ifdef RTABMAP_OCTOMAP
// clipping will be done in OctoMap
grid3D_&&rayTracing_?0.0f:cloudMaxDepth_,
#else
cloudMaxDepth_, cloudMaxDepth_,
#endif
cloudMinDepth_, cloudMinDepth_,
indices.get(), indices.get(),
parameters_, parameters_,
@@ -345,27 +340,115 @@ void OccupancyGrid::createLocalMap(
const Transform & t = node.sensorData().stereoCameraModel().localTransform(); const Transform & t = node.sensorData().stereoCameraModel().localTransform();
viewPoint = cv::Point3f(t.x(), t.y(), t.z()); 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( void OccupancyGrid::createLocalMap(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & groundCloud, const LaserScan & scan,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstaclesCloud,
const Transform & pose, const Transform & pose,
cv::Mat & groundCells, cv::Mat & groundCells,
cv::Mat & obstacleCells, cv::Mat & obstacleCells,
cv::Mat & emptyCells, cv::Mat & emptyCells,
const cv::Point3f & viewPoint) const 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(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())
{
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_) if(grid3D_)
{ {
UDEBUG("ground=%d obstacles=%d", (int)groundCloud->size(), (int)obstaclesCloud->size()); groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
if(groundIsObstacle_) obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
}
else
{ {
*obstaclesCloud += *groundCloud; util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGBNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
groundCloud->clear(); }
}
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
{
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_);
}
}
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)));
}
} }
// transform back in base frame // transform back in base frame
@@ -376,11 +459,12 @@ void OccupancyGrid::createLocalMapImpl(
if(rayTracing_) if(rayTracing_)
{ {
#ifdef RTABMAP_OCTOMAP #ifdef RTABMAP_OCTOMAP
if(!groundCloud->empty() || !obstaclesCloud->empty()) if(!groundCloud.empty() || !obstaclesCloud.empty())
{ {
//create local octomap //create local octomap
OctoMap octomap(cellSize_); OctoMap octomap(cellSize_);
octomap.addToCache(1, groundCloud, obstaclesCloud, pcl::PointXYZ(viewPoint.x, viewPoint.y, viewPoint.z)); octomap.setMaxRange(cloudMaxDepth_);
octomap.addToCache(1, groundCloud, obstaclesCloud, cv::Mat(), cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
std::map<int, Transform> poses; std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity())); poses.insert(std::make_pair(1, Transform::getIdentity()));
octomap.update(poses); octomap.update(poses);
@@ -390,10 +474,21 @@ void OccupancyGrid::createLocalMapImpl(
pcl::IndicesPtr emptyIndices(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()); 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()); 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); groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv); obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, 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
#else #else
@@ -401,23 +496,12 @@ void OccupancyGrid::createLocalMapImpl(
} }
#endif #endif
{ {
groundCells = util3d::laserScanFromPointCloud(*groundCloud, tinv); groundCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(groundCloud), tinv).data();
obstacleCells = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv); obstacleCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(obstaclesCloud), tinv).data();
} }
} }
else else if(!grid3D_ && rayTracing_ && (!obstacleCells.empty() || !groundCells.empty()))
{
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(rayTracing_)
{ {
cv::Mat laserScan = obstacleCells; cv::Mat laserScan = obstacleCells;
cv::Mat laserScanNoHit = groundCells; cv::Mat laserScanNoHit = groundCells;
@@ -426,7 +510,7 @@ void OccupancyGrid::createLocalMapImpl(
util3d::occupancy2DFromLaserScan( util3d::occupancy2DFromLaserScan(
laserScan, laserScan,
laserScanNoHit, laserScanNoHit,
viewPoint, viewPointInOut,
emptyCells, emptyCells,
obstacleCells, obstacleCells,
cellSize_, cellSize_,
@@ -434,9 +518,9 @@ void OccupancyGrid::createLocalMapImpl(
0); 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() void OccupancyGrid::clear()
{ {
cache_.clear(); cache_.clear();

87
corelib/src/OctoMap.cpp
View File

@@ -265,7 +265,9 @@ RtabmapColorOcTree::StaticMemberInitializer::StaticMemberInitializer() {
OctoMap::OctoMap(const ParametersMap & parameters) : OctoMap::OctoMap(const ParametersMap & parameters) :
hasColor_(false), hasColor_(false),
fullUpdate_(Parameters::defaultGridGlobalFullUpdate()), fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
updateError_(Parameters::defaultGridGlobalUpdateError()) updateError_(Parameters::defaultGridGlobalUpdateError()),
rangeMax_(Parameters::defaultGridRangeMax()),
rayTracing_(Parameters::defaultGridRayTracing())
{ {
float cellSize = Parameters::defaultGridCellSize(); float cellSize = Parameters::defaultGridCellSize();
Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize); Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
@@ -281,13 +283,17 @@ OctoMap::OctoMap(const ParametersMap & parameters) :
octree_->setOccupancyThres(occupancyThr); octree_->setOccupancyThres(occupancyThr);
Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_); Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_); 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) : OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate, float updateError) :
octree_(new RtabmapColorOcTree(cellSize)), octree_(new RtabmapColorOcTree(cellSize)),
hasColor_(false), hasColor_(false),
fullUpdate_(fullUpdate), fullUpdate_(fullUpdate),
updateError_(updateError) updateError_(updateError),
rangeMax_(0.0f),
rayTracing_(true)
{ {
minValues_[0] = minValues_[1] = minValues_[2] = 0.0; minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
maxValues_[0] = maxValues_[1] = maxValues_[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()); 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) 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; 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()); 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: // instead of direct scan insertion, compute update to filter ground:
octomap::KeySet free_cells; octomap::KeySet free_cells;
@@ -563,13 +571,22 @@ void OctoMap::update(const std::map<int, Transform> & poses)
pt = pcl::transformPoint(cloudIter->second.first->at(i), t); pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
} }
octomap::point3d point(pt.x, pt.y, pt.z); octomap::point3d point(pt.x, pt.y, pt.z);
// only clear space (ground points) bool ignoreOccupiedCell = false;
if (computeRays && if(rangeMaxSqrd > 0.0f)
(iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{ {
free_cells.insert(keyRay_.begin(), keyRay_.end()); 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 // occupied endpoint
octomap::OcTreeKey key; octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key)) if (octree_->coordToKeyChecked(point, key))
@@ -589,7 +606,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
if(n) if(n)
{ {
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0)) if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
{ {
hasColor_ = true; hasColor_ = true;
} }
@@ -603,6 +620,15 @@ void OctoMap::update(const std::map<int, Transform> & poses)
} }
} }
} }
// only clear space (ground points)
if (computeRays &&
(iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
}
UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size()); UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size());
// all other points: free on ray, occupied on endpoint: // all other points: free on ray, occupied on endpoint:
@@ -629,13 +655,22 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::point3d point(pt.x, pt.y, pt.z); octomap::point3d point(pt.x, pt.y, pt.z);
// free cells bool ignoreOccupiedCell = false;
if (computeRays && if(rangeMaxSqrd > 0.0f)
(iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{ {
free_cells.insert(keyRay_.begin(), keyRay_.end()); 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 // occupied endpoint
octomap::OcTreeKey key; octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key)) if (octree_->coordToKeyChecked(point, key))
@@ -655,7 +690,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
RtabmapColorOcTreeNode * n = octree_->updateNode(key, true); RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
if(n) if(n)
{ {
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0)) if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
{ {
hasColor_ = true; hasColor_ = true;
} }
@@ -669,6 +704,15 @@ void OctoMap::update(const std::map<int, Transform> & poses)
} }
} }
} }
// free cells
if (computeRays &&
(iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
}
UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (int)free_cells.size()); UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (int)free_cells.size());
@@ -711,6 +755,18 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::point3d point(pt.x, pt.y, pt.z); octomap::point3d point(pt.x, pt.y, pt.z);
bool ignoreCell = false;
if(rangeMaxSqrd > 0.0f)
{
octomap::point3d v(pt.x - sensorOrigin.x(), pt.y - sensorOrigin.y(), pt.z - sensorOrigin.z());
if(v.norm_sq() > rangeMaxSqrd)
{
ignoreCell=true;
}
}
if(!ignoreCell)
{
octomap::OcTreeKey key; octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key)) if (octree_->coordToKeyChecked(point, key))
{ {
@@ -739,6 +795,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
} }
} }
} }
}
// compress map // compress map
//octree_->prune(); //octree_->prune();

111
corelib/src/util3d.cpp
View File

@@ -1487,9 +1487,38 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform) 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(); bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f(); Eigen::Affine3f transform3f = transform.toEigen3f();
if(indices.get())
{
laserScan = cv::Mat(1, (int)indices->size(), CV_32FC3);
for(unsigned int i=0; i<indices->size(); ++i)
{
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
{
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC3);
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
float * ptr = laserScan.ptr<float>(0, i); float * ptr = laserScan.ptr<float>(0, i);
@@ -1506,15 +1535,50 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
ptr[1] = cloud.at(i).y; ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z; ptr[2] = cloud.at(i).z;
} }
}
} }
return laserScan; return laserScan;
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform) 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(); bool nullTransform = transform.isNull() || transform.isIdentity();
if(indices.get())
{
laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(6));
for(unsigned int i=0; i<indices->size(); ++i)
{
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
{
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
float * ptr = laserScan.ptr<float>(0, i); float * ptr = laserScan.ptr<float>(0, i);
@@ -1538,6 +1602,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud,
ptr[5] = cloud.at(i).normal_z; ptr[5] = cloud.at(i).normal_z;
} }
} }
}
return laserScan; return laserScan;
} }
@@ -1739,8 +1804,45 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform) 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(); bool nullTransform = transform.isNull() || transform.isIdentity();
if(indices.get())
{
laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(7));
for(unsigned int i=0; i<indices->size(); ++i)
{
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
{
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7));
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
float * ptr = laserScan.ptr<float>(0, i); float * ptr = laserScan.ptr<float>(0, i);
@@ -1766,6 +1868,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> &
int * ptrInt = (int*)ptr; int * ptrInt = (int*)ptr;
ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16); ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
} }
}
return laserScan; 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); 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)
//pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative) {
//{ return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative);
// return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative); }
//}
pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative) pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
{ {
return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative); return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative);

10
guilib/src/CloudViewer.cpp
View File

@@ -611,7 +611,15 @@ bool CloudViewer::addCloud(
{ {
_visualizer->updateColorHandlerIndex(id, previousColorIndex); _visualizer->updateColorHandlerIndex(id, previousColorIndex);
} }
else if(rgb || hasIntensity) else if(rgb)
{
_visualizer->updateColorHandlerIndex(id, 5);
}
else if(hasNormals)
{
_visualizer->updateColorHandlerIndex(id, hasIntensity?8:7);
}
else if(hasIntensity)
{ {
_visualizer->updateColorHandlerIndex(id, 5); _visualizer->updateColorHandlerIndex(id, 5);
} }

62
guilib/src/DatabaseViewer.cpp
View File

@@ -2402,7 +2402,7 @@ void DatabaseViewer::regenerateLocalMaps()
viewpoint = cv::Point3f(t.x(), t.y(), t.z()); viewpoint = cv::Point3f(t.x(), t.y(), t.z());
} }
grid.createLocalMap<pcl::PointXYZRGB>(cloud, s.getPose(), ground, obstacles, empty, viewpoint); grid.createLocalMap(LaserScan::backwardCompatibility(util3d::laserScanFromPointCloud(*cloud)), s.getPose(), ground, obstacles, empty, viewpoint);
} }
} }
else else
@@ -2524,7 +2524,7 @@ void DatabaseViewer::regenerateCurrentLocalMaps()
viewpoint = cv::Point3f(t.x(), t.y(), t.z()); viewpoint = cv::Point3f(t.x(), t.y(), t.z());
} }
grid.createLocalMap<pcl::PointXYZRGB>(cloud, s.getPose(), ground, obstacles, empty, viewpoint); grid.createLocalMap(LaserScan::backwardCompatibility(util3d::laserScanFromPointCloud(*cloud)), s.getPose(), ground, obstacles, empty, viewpoint);
} }
} }
else else
@@ -3308,6 +3308,8 @@ void DatabaseViewer::update(int value,
pcl::IndicesPtr empty(new std::vector<int>); pcl::IndicesPtr empty(new std::vector<int>);
pcl::IndicesPtr ground(new std::vector<int>); pcl::IndicesPtr ground(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
if(octomap->hasColor())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud); pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text())); cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
@@ -3317,6 +3319,19 @@ void DatabaseViewer::update(int value,
pcl::copyPointCloud(*cloud, *ground, *groundCloud); pcl::copyPointCloud(*cloud, *ground, *groundCloud);
cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text())); cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
cloudViewer_->setCloudPointSize("ground", 5); cloudViewer_->setCloudPointSize("ground", 5);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
cloudViewer_->setCloudPointSize("obstacles", 5);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *ground, *groundCloud);
cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
cloudViewer_->setCloudPointSize("ground", 5);
}
if(ui_->checkBox_grid_empty->isChecked()) if(ui_->checkBox_grid_empty->isChecked())
{ {
@@ -3336,14 +3351,25 @@ void DatabaseViewer::update(int value,
#endif #endif
{ {
// occupancy cloud // occupancy cloud
cloudViewer_->addCloud("ground", LaserScan scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.first);
util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localMaps.begin()->second.first.first)), if(scan.hasRGB())
pose, {
QColor(ui_->lineEdit_groundColor->text())); cloudViewer_->addCloud("ground", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
cloudViewer_->addCloud("obstacles", }
util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localMaps.begin()->second.first.second)), else
pose, {
QColor(ui_->lineEdit_obstacleColor->text())); cloudViewer_->addCloud("ground", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
}
scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.first);
if(scan.hasRGB())
{
cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
}
else
{
cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
}
cloudViewer_->setCloudPointSize("ground", 5); cloudViewer_->setCloudPointSize("ground", 5);
cloudViewer_->setCloudPointSize("obstacles", 5); cloudViewer_->setCloudPointSize("obstacles", 5);
@@ -5152,6 +5178,9 @@ void DatabaseViewer::updateOctomapView()
pcl::IndicesPtr empty(new std::vector<int>); pcl::IndicesPtr empty(new std::vector<int>);
pcl::IndicesPtr ground(new std::vector<int>); pcl::IndicesPtr ground(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
if(octomap_->hasColor())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud); pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), Qt::red); occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), Qt::red);
@@ -5161,6 +5190,19 @@ void DatabaseViewer::updateOctomapView()
pcl::copyPointCloud(*cloud, *ground, *groundCloud); pcl::copyPointCloud(*cloud, *ground, *groundCloud);
occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), Qt::green); occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), Qt::green);
occupancyGridViewer_->setCloudPointSize("octomap_ground", 5); occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), Qt::red);
occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *ground, *groundCloud);
occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), Qt::green);
occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
}
if(ui_->checkBox_grid_empty->isChecked()) if(ui_->checkBox_grid_empty->isChecked())
{ {