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

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

6
corelib/src/CameraThread.cpp
View File

@@ -406,9 +406,8 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
_scanRangeMin,
validIndices.get());
float maxPoints = (data.depthRaw().rows/_scanDownsampleStep)*(data.depthRaw().cols/_scanDownsampleStep);
cv::Mat scan;
LaserScan scan;
const Transform & baseToScan = data.cameraModels()[0].localTransform();
LaserScan::Format format = LaserScan::kXYZRGB;
if(validIndices->size())
{
if(_scanVoxelSize>0.0f)
@@ -433,7 +432,6 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
format = LaserScan::kXYZRGBNormal;
}
else
{
@@ -441,7 +439,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
}
}
}
data.setLaserScan(LaserScan(scan, (int)maxPoints, _scanRangeMax, format, baseToScan));
data.setLaserScan(LaserScan(scan, (int)maxPoints, _scanRangeMax, baseToScan));
if(info) info->timeScanFromDepth = timer.ticks();
}
else

147
corelib/src/LaserScan.cpp
View File

@@ -209,42 +209,60 @@ LaserScan::LaserScan() :
{
}
LaserScan::LaserScan(
const LaserScan & scan,
int maxPoints,
float maxRange,
const Transform & localTransform)
{
UASSERT(scan.empty() || scan.format() != kUnknown);
init(scan.data(), scan.format(), 0, maxRange, 0, 0, 0, maxPoints, localTransform);
}
LaserScan::LaserScan(
const LaserScan & scan,
int maxPoints,
float maxRange,
Format format,
const Transform & localTransform)
{
init(scan.data(), format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
}
LaserScan::LaserScan(
const cv::Mat & data,
int maxPoints,
float maxRange,
Format format,
const Transform & localTransform) :
data_(data),
format_(format),
maxPoints_(maxPoints),
rangeMin_(0),
rangeMax_(maxRange),
angleMin_(0),
angleMax_(0),
angleIncrement_(0),
localTransform_(localTransform)
const Transform & localTransform)
{
UASSERT(data.empty() || data.rows == 1);
UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6) || data.type() == CV_32FC(7));
UASSERT(!localTransform.isNull());
init(data, format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
}
if(!data.empty() && !isCompressed())
{
if(format == kUnknown)
{
*this = backwardCompatibility(data_, maxPoints_, rangeMax_, localTransform_);
}
else // verify that format corresponds to expected number of channels
{
UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
}
}
LaserScan::LaserScan(
const LaserScan & scan,
float minRange,
float maxRange,
float angleMin,
float angleMax,
float angleIncrement,
const Transform & localTransform)
{
UASSERT(scan.empty() || scan.format() != kUnknown);
init(scan.data(), scan.format(), minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
}
LaserScan::LaserScan(
const LaserScan & scan,
Format format,
float minRange,
float maxRange,
float angleMin,
float angleMax,
float angleIncrement,
const Transform & localTransform)
{
init(scan.data(), format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
}
LaserScan::LaserScan(
@@ -255,37 +273,78 @@ LaserScan::LaserScan(
float angleMin,
float angleMax,
float angleIncrement,
const Transform & localTransform) :
data_(data),
format_(format),
rangeMin_(minRange),
rangeMax_(maxRange),
angleMin_(angleMin),
angleMax_(angleMax),
angleIncrement_(angleIncrement),
localTransform_(localTransform)
const Transform & localTransform)
{
UASSERT(maxRange>minRange);
UASSERT(angleMax>angleMin);
UASSERT(angleIncrement != 0.0f);
maxPoints_ = std::ceil((angleMax - angleMin) / angleIncrement)+1;
init(data, format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
}
void LaserScan::init(
const cv::Mat & data,
Format format,
float rangeMin,
float rangeMax,
float angleMin,
float angleMax,
float angleIncrement,
int maxPoints,
const Transform & localTransform)
{
UASSERT(data.empty() || data.rows == 1);
UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6) || data.type() == CV_32FC(7));
UASSERT(!localTransform.isNull());
bool is2D = false;
if(angleIncrement != 0.0f)
{
// 2D scan
is2D = true;
UASSERT(rangeMax>rangeMin);
UASSERT(angleMax>angleMin);
UASSERT(angleIncrement != 0.0f);
maxPoints_ = std::ceil((angleMax - angleMin) / angleIncrement)+1;
}
else
{
// 3D scan
UASSERT(rangeMax>=rangeMin);
maxPoints_ = maxPoints;
}
data_ = data;
format_ = format;
rangeMin_ = rangeMin;
rangeMax_ = rangeMax;
angleMin_ = angleMin;
angleMax_ = angleMax;
angleIncrement_ = angleIncrement;
localTransform_ = localTransform;
if(!data.empty() && !isCompressed())
{
if(data_.cols > maxPoints_)
if(is2D && data_.cols > maxPoints_)
{
UWARN("The number of points (%d) in the scan is over the maximum "
"points (%d) defined by angle settings (min=%f max=%f inc=%f). "
"The scan info may be wrong!",
data_.cols, maxPoints_, angleMin_, angleMax_, angleIncrement_);
}
else if(!is2D && maxPoints_>0 && data_.cols > maxPoints_)
{
UWARN("The number of points (%d) in the scan is over the maximum "
"points (%d) defined by max points setting.",
data_.cols, maxPoints_);
}
if(format == kUnknown)
{
*this = backwardCompatibility(data_, rangeMin_, rangeMax_, angleMin_, angleMax_, angleIncrement_, localTransform_);
if(angleIncrement_ != 0)
{
*this = backwardCompatibility(data_, rangeMin_, rangeMax_, angleMin_, angleMax_, angleIncrement_, localTransform_);
}
else
{
*this = backwardCompatibility(data_, maxPoints_, rangeMax_, localTransform_);
}
}
else // verify that format corresponds to expected number of channels
{

5
corelib/src/Memory.cpp
View File

@@ -3173,7 +3173,7 @@ Transform Memory::computeIcpTransformMulti(
}
}
cv::Mat assembledScan;
LaserScan assembledScan;
if(assembledToNormalClouds->size())
{
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
@@ -3212,14 +3212,13 @@ Transform Memory::computeIcpTransformMulti(
assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
}
}
UDEBUG("assembledScan=%d points", assembledScan.cols);
UDEBUG("assembledScan=%d points", assembledScan.size());
// scans are in base frame but for 2d scans, set the height so that correspondences matching works
assembledData.setLaserScan(
LaserScan(assembledScan,
fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
fromScan.rangeMax(),
toScan.format(),
fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
t = _registrationIcpMulti->computeTransformation(fromS->sensorData(), assembledData, guess, info);

28
corelib/src/OccupancyGrid.cpp
View File

@@ -445,8 +445,8 @@ void OccupancyGrid::createLocalMap(
UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
if(grid3D_)
{
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
}
else
{
@@ -460,8 +460,8 @@ void OccupancyGrid::createLocalMap(
UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
if(grid3D_)
{
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
}
else
{
@@ -475,8 +475,8 @@ void OccupancyGrid::createLocalMap(
UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
if(grid3D_)
{
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
}
else
{
@@ -490,8 +490,8 @@ void OccupancyGrid::createLocalMap(
UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
if(grid3D_)
{
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
}
else
{
@@ -543,17 +543,17 @@ void OccupancyGrid::createLocalMap(
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);
groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv).data();
obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv).data();
emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv).data();
}
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);
groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv).data();
obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv).data();
emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv).data();
}
}
}

190
corelib/src/util3d.cpp
View File

@@ -1544,11 +1544,11 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0, format);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
{
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
@@ -1605,16 +1605,16 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZ);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
{
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
@@ -1688,12 +1688,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud,
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZNormal);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{
UASSERT(cloud.size() == normals.size());
cv::Mat laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
@@ -1734,17 +1734,17 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZNormal);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform, bool filterNaNs)
{
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
@@ -1805,17 +1805,17 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZRGB);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
{
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
@@ -1874,12 +1874,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZI);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{
UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
@@ -1922,16 +1922,16 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZRGBNormal);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform, bool filterNaNs)
{
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
@@ -2009,12 +2009,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> &
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZRGBNormal);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{
UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
@@ -2056,53 +2056,97 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZINormal);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
int oi = 0;
for(unsigned int i=0; i<cloud.size(); ++i)
if(indices.get())
{
if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
uIsFinite(cloud.at(i).normal_x) &&
uIsFinite(cloud.at(i).normal_y) &&
uIsFinite(cloud.at(i).normal_z)))
laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(7));
for(unsigned int i=0; i<indices->size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform)
int index = indices->at(i);
if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
uIsFinite(cloud.at(index).normal_x) &&
uIsFinite(cloud.at(index).normal_y) &&
uIsFinite(cloud.at(index).normal_z)))
{
pcl::PointXYZINormal 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;
float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform)
{
pcl::PointXYZINormal 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;
}
ptr[3] = cloud.at(i).intensity;
}
else
}
}
else
{
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7));
for(unsigned int i=0; i<cloud.size(); ++i)
{
if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
uIsFinite(cloud.at(i).normal_x) &&
uIsFinite(cloud.at(i).normal_y) &&
uIsFinite(cloud.at(i).normal_z)))
{
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, oi++);
if(!nullTransform)
{
pcl::PointXYZINormal 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;
}
ptr[3] = cloud.at(i).intensity;
}
ptr[3] = cloud.at(i).intensity;
}
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZINormal);
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
bool nullTransform = transform.isNull();
@@ -2129,12 +2173,12 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXY);
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
bool nullTransform = transform.isNull();
@@ -2163,12 +2207,12 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYI);
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
bool nullTransform = transform.isNull();
@@ -2203,12 +2247,12 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & clou
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYNormal);
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{
UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
@@ -2247,12 +2291,12 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYNormal);
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
bool nullTransform = transform.isNull();
@@ -2289,12 +2333,12 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> &
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYINormal);
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{
UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
@@ -2335,9 +2379,9 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
}
if(oi == 0)
{
return cv::Mat();
return LaserScan();
}
return laserScan(cv::Range::all(), cv::Range(0,oi));
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYINormal);
}
pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform)

68
corelib/src/util3d_filtering.cpp
View File

@@ -746,6 +746,14 @@ pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & clo
{
return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, indices, min, max, transform, negative);
}
pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
{
return cropBoxImpl<pcl::PointXYZI>(cloud, indices, min, max, transform, negative);
}
pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
{
return cropBoxImpl<pcl::PointXYZINormal>(cloud, indices, min, max, transform, negative);
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr cropBoxImpl(
@@ -782,6 +790,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cropBox(const pcl::PointCloud<pcl::PointX
{
return cropBoxImpl<pcl::PointXYZRGB>(cloud, min, max, transform, negative);
}
pcl::PointCloud<pcl::PointXYZI>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
{
return cropBoxImpl<pcl::PointXYZI>(cloud, min, max, transform, negative);
}
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
{
return cropBoxImpl<pcl::PointXYZINormal>(cloud, min, max, transform, negative);
@@ -999,6 +1011,14 @@ pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGBNormal>::P
{
return radiusFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
{
return radiusFilteringImpl<pcl::PointXYZI>(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
{
return radiusFilteringImpl<pcl::PointXYZINormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -1616,6 +1636,16 @@ pcl::IndicesPtr normalFiltering(
{
return normalFilteringImpl<pcl::PointXYZRGB>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint)
{
return normalFilteringImpl<pcl::PointXYZI>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
}
template<typename PointT>
pcl::IndicesPtr normalFilteringImpl(
@@ -1681,6 +1711,16 @@ pcl::IndicesPtr normalFiltering(
{
return normalFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, angleMax, normal);
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint)
{
return normalFilteringImpl<pcl::PointXYZINormal>(cloud, indices, angleMax, normal);
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -1794,6 +1834,26 @@ std::vector<pcl::IndicesPtr> extractClusters(
{
return extractClustersImpl<pcl::PointXYZRGBNormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
return extractClustersImpl<pcl::PointXYZI>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
return extractClustersImpl<pcl::PointXYZINormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
}
template<typename PointT>
pcl::IndicesPtr extractIndicesImpl(
@@ -1826,6 +1886,14 @@ pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Pt
{
return extractIndicesImpl<pcl::PointXYZRGBNormal>(cloud, indices, negative);
}
pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
{
return extractIndicesImpl<pcl::PointXYZI>(cloud, indices, negative);
}
pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
{
return extractIndicesImpl<pcl::PointXYZINormal>(cloud, indices, negative);
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr extractIndicesImpl(

21
corelib/src/util3d_mapping.cpp
View File

@@ -538,7 +538,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
std::map<int, std::pair<cv::Mat, cv::Mat> > scansCv;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter)
{
scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second), cv::Mat())));
scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second).data(), cv::Mat())));
}
return create2DMap(poses,
scansCv,
@@ -564,7 +564,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
std::map<int, std::pair<cv::Mat, cv::Mat> > scansCv;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter)
{
scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second), cv::Mat())));
scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second).data(), cv::Mat())));
}
return create2DMap(poses,
scansCv,
@@ -577,21 +577,6 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
scanMaxRange);
}
/**
* Create 2d Occupancy grid (CV_8S)
* -1 = unknown
* 0 = empty space
* 100 = obstacle
* @param poses
* @param scans
* @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 create2DMap(const std::map<int, Transform> & poses,
const std::map<int, std::pair<cv::Mat, cv::Mat> > & scans, // <id, <hit, no hit> >
const std::map<int, cv::Point3f > & viewpoints,
@@ -616,6 +601,8 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, std::pair<cv::Mat, cv::Mat> >::const_iterator jter=scans.find(iter->first);
UASSERT_MSG(jter->second.first.empty() || jter->second.first.type() == CV_32FC2, "Input scans should be 2D to avoid any confusion.");
UASSERT_MSG(jter->second.second.empty() || jter->second.second.type() == CV_32FC2, "Input scans should be 2D to avoid any confusion.");
if(jter!=scans.end() && (jter->second.first.cols || jter->second.second.cols))
{
UASSERT(!iter->second.isNull());