i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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,