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

util3d: Refactored laserScanFromPointCloud() functions to return LaserScan with correct format instead of cv::Mat.

Browse Source
This commit is contained in:
matlabbe
2021-02-07 17:27:55 -05:00
parent c42a4e3d7e
commit 481a140f84
14 changed files with 464 additions and 182 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
corelib/include/rtabmap/core/LaserScan.h
View File

@@ -71,11 +71,35 @@ public:
public:
LaserScan();
LaserScan(const LaserScan & data,
int maxPoints,
float maxRange,
const Transform & localTransform = Transform::getIdentity());
RTABMAP_DEPRECATED(LaserScan(const LaserScan & data,
int maxPoints,
float maxRange,
Format format,
const Transform & localTransform = Transform::getIdentity()), "Use version without \"format\" argument.");
LaserScan(const cv::Mat & data,
int maxPoints,
float maxRange,
Format format,
const Transform & localTransform = Transform::getIdentity());
RTABMAP_DEPRECATED(LaserScan(const LaserScan & data,
Format format,
float minRange,
float maxRange,
float angleMin,
float angleMax,
float angleIncrement,
const Transform & localTransform = Transform::getIdentity()), "Use version without \"format\" argument.");
LaserScan(const LaserScan & data,
float minRange,
float maxRange,
float angleMin,
float angleMax,
float angleIncrement,
const Transform & localTransform = Transform::getIdentity());
LaserScan(const cv::Mat & data,
Format format,
float minRange,
@@ -114,6 +138,17 @@ public:
void clear() {data_ = cv::Mat();}
private:
void init(const cv::Mat & data,
Format format,
float minRange,
float maxRange,
float angleMin,
float angleMax,
float angleIncrement,
int maxPoints,
const Transform & localTransform = Transform::getIdentity());
private:
cv::Mat data_;
Format format_;

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

@@ -198,35 +198,36 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs = true, bool is2D = false, const Transform & transform = Transform());
// return CV_32FC3 (x,y,z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
// 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(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC4 (x,y,z,rgb)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC4 (x,y,z,I)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
// 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(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
// 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(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC2 (x,y)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC3 (x,y,I)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC5 (x,y,normal_x, normal_y, normal_z)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC6 (x,y,I,normal_x, normal_y, normal_z)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform = Transform());
// For 2d laserScan, z is set to null.

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

@@ -279,6 +279,20 @@ pcl::IndicesPtr RTABMAP_EXP cropBox(
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
pcl::IndicesPtr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
pcl::IndicesPtr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Eigen::Vector4f & min,
@@ -297,6 +311,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cropBox(
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const Eigen::Vector4f & min,
@@ -408,6 +428,16 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius);
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius);
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius);
/**
* For convenience.
@@ -590,6 +620,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint);
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);
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -604,6 +641,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint);
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);
/**
* For convenience.
@@ -661,6 +705,20 @@ std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
int minClusterSize,
int maxClusterSize = std::numeric_limits<int>::max(),
int * biggestClusterIndex = 0);
std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize = std::numeric_limits<int>::max(),
int * biggestClusterIndex = 0);
std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize = std::numeric_limits<int>::max(),
int * biggestClusterIndex = 0);
pcl::IndicesPtr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -678,6 +736,14 @@ pcl::IndicesPtr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative);
pcl::IndicesPtr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative);
pcl::IndicesPtr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -700,6 +766,16 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP extractIndices(
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized);
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized);
pcl::IndicesPtr extractPlane(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

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

@@ -99,6 +99,21 @@ RTABMAP_DEPRECATED(cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform
float minMapSize = 0.0f,
float scanMaxRange = 0.0f), "Use interface with cv::Mat scans.");
/**
* Create 2d Occupancy grid (CV_8S)
* -1 = unknown
* 0 = empty space
* 100 = obstacle
* @param poses
* @param scans, should be CV_32FC2 type!
* @param viewpoints
* @param cellSize m
* @param unknownSpaceFilled if false no fill, otherwise a virtual laser sweeps the unknown space from each pose (stopping on detected obstacle)
* @param xMin
* @param yMin
* @param minMapSize minimum map size in meters
* @param scanMaxRange laser scan maximum range, would be set if unknownSpaceFilled=true
*/
cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
const std::map<int, std::pair<cv::Mat, cv::Mat> > & scans, // <id, <hit, no hit> >, in /base_link frame
const std::map<int, cv::Point3f > & viewpoints, // /base_link -> /base_scan