i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-12 21:09:50 +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
corelib/src/LaserScan.cpp
+103 -44
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
{