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

DBViewer: fixed grid cell size in 3D View. Memory/Grid, using Icp/PointToPlaneGroundNormalsUp parameter when normals are computed. Grid: fixed 2D noHit ray tracing.

Browse Source
This commit is contained in:
matlabbe
2022-01-25 18:10:43 -05:00
parent 2dc7b59b05
commit d56692640e
11 changed files with 91 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/Memory.h
View File

@@ -320,6 +320,7 @@ private:
float _laserScanVoxelSize;
int _laserScanNormalK;
float _laserScanNormalRadius;
float _laserScanGroundNormalsUp;
bool _reextractLoopClosureFeatures;
bool _localBundleOnLoopClosure;
float _rehearsalMaxDistance;

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

@@ -122,6 +122,7 @@ private:
bool projMapFrame_;
float maxObstacleHeight_;
int normalKSearch_;
float groundNormalsUp_;
float maxGroundAngle_;
float clusterRadius_;
int minClusterSize_;

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

@@ -126,6 +126,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
UDEBUG("Cluster radius=%f", clusterRadius_);
UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
UDEBUG("groundNormalsUp=%f", groundNormalsUp_);
util3d::segmentObstaclesFromGround<PointT>(
cloud,
indices,
@@ -138,7 +139,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
flatObstaclesDetected_,
maxGroundHeight_,
flatObstacles,
Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1));
Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1),
groundNormalsUp_);
UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
//UWARN("Saving ground.pcd and obstacles.pcd");
//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);

12
corelib/include/rtabmap/core/impl/util3d_mapping.hpp
View File

@@ -63,7 +63,8 @@ void segmentObstaclesFromGround(
bool segmentFlatObstacles,
float maxGroundHeight,
pcl::IndicesPtr * flatObstacles,
const Eigen::Vector4f & viewPoint)
const Eigen::Vector4f & viewPoint,
float groundNormalsUp)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
@@ -81,7 +82,8 @@ void segmentObstaclesFromGround(
groundNormalAngle,
Eigen::Vector4f(0,0,1,0),
normalKSearch,
viewPoint);
viewPoint,
groundNormalsUp);
if(segmentFlatObstacles && flatSurfaces->size())
{
@@ -205,7 +207,8 @@ void segmentObstaclesFromGround(
bool segmentFlatObstacles,
float maxGroundHeight,
pcl::IndicesPtr * flatObstacles,
const Eigen::Vector4f & viewPoint)
const Eigen::Vector4f & viewPoint,
float groundNormalsUp)
{
pcl::IndicesPtr indices(new std::vector<int>);
segmentObstaclesFromGround<PointT>(
@@ -220,7 +223,8 @@ void segmentObstaclesFromGround(
segmentFlatObstacles,
maxGroundHeight,
flatObstacles,
viewPoint);
viewPoint,
groundNormalsUp);
}
template<typename PointT>

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

@@ -697,13 +697,15 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint);
const Eigen::Vector4f & viewpoint,
float groundNormalsUp = 0.0f);
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint);
const Eigen::Vector4f & viewpoint,
float groundNormalsUp = 0.0f);
/**
* @brief Given a normal and a maximum angle error, keep all points of the cloud
@@ -727,42 +729,48 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint);
const Eigen::Vector4f & viewpoint,
float groundNormalsUp = 0.0f);
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint);
const Eigen::Vector4f & viewpoint,
float groundNormalsUp = 0.0f);
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint);
const Eigen::Vector4f & viewpoint,
float groundNormalsUp = 0.0f);
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint);
const Eigen::Vector4f & viewpoint,
float groundNormalsUp = 0.0f);
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint);
const Eigen::Vector4f & viewpoint,
float groundNormalsUp = 0.0f);
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint);
const Eigen::Vector4f & viewpoint,
float groundNormalsUp = 0.0f);
/**
* For convenience.

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

@@ -152,7 +152,8 @@ void segmentObstaclesFromGround(
bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f,
pcl::IndicesPtr * flatObstacles = 0,
const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
float groundNormalsUp = 0);
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -165,7 +166,8 @@ void segmentObstaclesFromGround(
bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f,
pcl::IndicesPtr * flatObstacles = 0,
const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
float groundNormalsUp = 0);
template<typename PointT>
void occupancy2DFromGroundObstacles(