mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-03 16:47:47 +08:00
util3d_filtering.h: started doc and test
This commit is contained in:
matlabbe
@@ -43,10 +43,36 @@ namespace util3d
|
||||
{
|
||||
|
||||
/**
|
||||
* Do some filtering approaches and try to
|
||||
* avoid converting between pcl and opencv and to avoid not needed
|
||||
* operations like computing normals while the scan has already
|
||||
* normals and voxel filtering is not used.
|
||||
* @brief Applies a common set of filters to a LaserScan, including downsampling, range limits, voxel grid filtering, and normal estimation.
|
||||
*
|
||||
* This function performs a sequence of optional preprocessing steps on the input LaserScan:
|
||||
* - Downsampling: Reduces the number of points by selecting every N-th point.
|
||||
* - Range filtering: Removes points outside the specified minimum and maximum range.
|
||||
* - Voxel grid filtering: Reduces point density using a voxel grid.
|
||||
* - Normal estimation: Computes surface normals using k-nearest neighbors or radius search.
|
||||
* - Normal reorientation: Optionally orients normals upward if this condition is fulfilled: for each normal,
|
||||
* if normal.z < `-groundNormalsUp` and the corresponding point is below viewpoint, it is flipped upward.
|
||||
*
|
||||
* The function supports both 2D and 3D scans and adapts behavior based on whether the scan contains RGB or intensity data.
|
||||
*
|
||||
* Depending on the filtering options used, the output point cloud may be dense if the input is organized.
|
||||
*
|
||||
* @param scanIn Input LaserScan to be filtered.
|
||||
* @param downsamplingStep Step size for downsampling. A value >1 will reduce the scan resolution. For organized
|
||||
* scans, only the largest dimension is downsampled and the result is dense. For example,
|
||||
* if the input organized scan is 16x1024, the resulting scan will be 1x8192 (if all values are valid).
|
||||
* @param rangeMin Minimum range to keep points from the scan viewpoint. Points closer than this value will be discarded. Output point cloud will be dense. Set to 0 to disable.
|
||||
* @param rangeMax Maximum range to keep points from the scan viewpoint. Points farther than this value will be discarded. Output point cloud will be dense. Set to 0 to disable.
|
||||
* @param voxelSize Size of the voxel grid in meters. A value >0 enables voxel filtering. Output point cloud will be dense.
|
||||
* @param normalK Number of nearest neighbors to use for normal estimation. Set to 0 to disable.
|
||||
* @param normalRadius Radius used for normal estimation. Set to 0 to disable.
|
||||
* @param groundNormalsUp If >0, normal vectors close to -Z axis will be oriented upward (+Z). Expected value
|
||||
* is around `0.8`.
|
||||
*
|
||||
* @return A new LaserScan instance with the applied filters and potential normals.
|
||||
* @see adjustNormalsToViewPoint()
|
||||
*
|
||||
*
|
||||
*/
|
||||
LaserScan RTABMAP_CORE_EXPORT commonFiltering(
|
||||
const LaserScan & scan,
|
||||
@@ -57,7 +83,10 @@ LaserScan RTABMAP_CORE_EXPORT commonFiltering(
|
||||
int normalK = 0,
|
||||
float normalRadius = 0.0f,
|
||||
float groundNormalsUp = 0.0f);
|
||||
// Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp=0.8, otherwise set groundNormalsUp=0.0.
|
||||
/**
|
||||
* @brief Applies a common set of filters to a LaserScan, including downsampling, range limits, voxel grid filtering, and normal estimation.
|
||||
* @deprecated Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUpAngle=0.8, otherwise set groundNormalsUpAngle=0.0.
|
||||
*/
|
||||
RTABMAP_DEPRECATED LaserScan RTABMAP_CORE_EXPORT commonFiltering(
|
||||
const LaserScan & scan,
|
||||
int downsamplingStep,
|
||||
|
||||
@@ -87,7 +87,10 @@ LaserScan commonFiltering(
|
||||
if(!scan.isEmpty())
|
||||
{
|
||||
// combined downsampling and range filtering step
|
||||
if(downsamplingStep<=1 || scan.size() <= downsamplingStep)
|
||||
if(downsamplingStep<=1 ||
|
||||
scan.size() <= downsamplingStep ||
|
||||
(scan.data().rows > scan.data().cols && scan.data().rows <= downsamplingStep) ||
|
||||
(scan.data().cols > scan.data().rows && scan.data().cols <= downsamplingStep))
|
||||
{
|
||||
downsamplingStep = 1;
|
||||
}
|
||||
@@ -325,6 +328,7 @@ LaserScan commonFiltering(
|
||||
|
||||
if(scan.size() && !scan.is2d() && scan.hasNormals() && groundNormalsUp>0.0f)
|
||||
{
|
||||
// FIXME: the viewpoint should be 0,0,0 here as we don't apply the local transform
|
||||
scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,10), groundNormalsUp);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -14,4 +14,9 @@ gtest_discover_tests(test_util3d)
|
||||
#util3d_transforms.h
|
||||
add_executable(test_util3d_transforms test_util3d_transforms.cpp)
|
||||
target_link_libraries(test_util3d_transforms gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_util3d_transforms)
|
||||
gtest_discover_tests(test_util3d_transforms)
|
||||
|
||||
#util3d_filtering.h
|
||||
add_executable(test_util3d_filtering test_util3d_filtering.cpp)
|
||||
target_link_libraries(test_util3d_filtering gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_util3d_filtering)
|
||||
@@ -0,0 +1,419 @@
|
||||
#include "gtest/gtest.h"
|
||||
#include "rtabmap/core/util3d_filtering.h"
|
||||
#include "rtabmap/utilite/UException.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
TEST(Util3dFiltering, commonFilteringEmpty) {
|
||||
ASSERT_TRUE(util3d::commonFiltering(LaserScan(), 1).isEmpty());
|
||||
}
|
||||
|
||||
TEST(Util3dFiltering, commonFilteringDownsample) {
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
// Dense
|
||||
cv::Mat data = cv::Mat::zeros(1, i+1, CV_32FC3);
|
||||
LaserScan scan(data, data.total(), 0, LaserScan::kXYZ);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 0).size(), i+1);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1).size(), i+1);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 2).size(), (i+1) <= 2 ? (i+1) : (i+1)/2);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 3).size(), (i+1) <= 3 ? (i+1) : (i+1)/3);
|
||||
|
||||
// Organized (cols>rows)
|
||||
data = cv::Mat::zeros(i+1, (i+1)*2+i, CV_32FC3);
|
||||
SCOPED_TRACE(uFormat("Testing Organized dim %dx%d (i=%d)", i+1, (i+1)*2+i, i));
|
||||
scan = LaserScan(data, data.total(), 0, LaserScan::kXYZ);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 0).size(), (i+1)*((i+1)*2+i));
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1).size(), (i+1)*((i+1)*2+i));
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 2).size(), (i+1)*2+i <= 2 ? (i+1)*((i+1)*2+i) : (i+1)*(((i+1)*2+i)/2));
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 3).size(), (i+1)*2+i <= 3 ? (i+1)*((i+1)*2+i) : (i+1)*(((i+1)*2+i)/3));
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 2).data().rows, 1);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 3).data().rows, 1);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 2).data().cols, (i+1)*2+i <= 2 ? (i+1)*((i+1)*2+i) : (i+1)*(((i+1)*2+i)/2));
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 3).data().cols, (i+1)*2+i <= 3 ? (i+1)*((i+1)*2+i) : (i+1)*(((i+1)*2+i)/3));
|
||||
|
||||
// Organized (cols<rows)
|
||||
data = cv::Mat::zeros((i+1)*2+i, (i+1), CV_32FC3);
|
||||
SCOPED_TRACE(uFormat("Testing Organized dim %dx%d (i=%d)", (i+1)*2+i, (i+1), i));
|
||||
scan = LaserScan(data, data.total(), 0, LaserScan::kXYZ);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 0).size(), (i+1)*((i+1)*2+i));
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1).size(), (i+1)*((i+1)*2+i));
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 2).size(), (i+1)*2+i <= 2 ? (i+1)*((i+1)*2+i) : (i+1)*(((i+1)*2+i)/2));
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 3).size(), (i+1)*2+i <= 3 ? (i+1)*((i+1)*2+i) : (i+1)*(((i+1)*2+i)/3));
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 2).data().rows, (i+1)*2+i <= 2 ? (i+1)*2+i : 1);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 3).data().rows, (i+1)*2+i <= 3 ? (i+1)*2+i : 1);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 2).data().cols, (i+1)*2+i <= 2 ? (i+1) : (i+1)*(((i+1)*2+i)/2));
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 3).data().cols, (i+1)*2+i <= 3 ? (i+1) : (i+1)*(((i+1)*2+i)/3));
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Util3dFiltering, commonFilteringRange) {
|
||||
cv::Mat data = cv::Mat::zeros(1, 5, CV_32FC3);
|
||||
for(int i=0; i<(int)data.total(); ++i)
|
||||
{
|
||||
data.at<cv::Vec3f>(0, i)[0] = i-1; // -1, 0, 1, 2, 3
|
||||
}
|
||||
LaserScan scan(data, data.total(), 0, LaserScan::kXYZ);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1, 0.0f, 0.0f).size(), 5); // rangeMin<=0 ignored
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1, -0.1f, 0.0f).size(), 5); // rangeMin<=0 ignored
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1, 0.5f, 0.0f).size(), 4); // rangeMax<=0 ignored, -1 is 1 meter away
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1, 0.5f, 4.0f).size(), 4); // -1 is 1 meter away
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1, 0.0f, 4.0f).size(), 5); // rangeMin<=0 ignored
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1, 0.0f, 2.5f).size(), 4); // rangeMin<=0 ignored
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1, 2.5f, 0.5f).size(), 0);
|
||||
}
|
||||
|
||||
TEST(Util3dFiltering, commonFilteringVoxel) {
|
||||
cv::Mat data = cv::Mat::zeros(1, 5, CV_32FC3);
|
||||
data.at<cv::Vec3f>(0, 0)[0] = 0.1;
|
||||
data.at<cv::Vec3f>(0, 1)[0] = 0.11;
|
||||
data.at<cv::Vec3f>(0, 2)[0] = 0.5;
|
||||
data.at<cv::Vec3f>(0, 3)[0] = 0.51;
|
||||
data.at<cv::Vec3f>(0, 4)[0] = 3;
|
||||
LaserScan scan(data, data.total(), 0, LaserScan::kXYZ);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.001).size(), 5); // voxel very small
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.05).size(), 3);
|
||||
ASSERT_EQ(util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 1).size(), 2);
|
||||
}
|
||||
|
||||
TEST(Util3dFiltering, commonFilteringNormal) {
|
||||
cv::Mat data = cv::Mat::zeros(1, 100, CV_32FC3);
|
||||
// make points all on same plane (0,1,0);
|
||||
for(int i=0; i<10; ++i) {
|
||||
for(int j=0; j<10; ++j) {
|
||||
data.at<cv::Vec3f>(0, i*10+j)[0] = i*0.05; // x
|
||||
data.at<cv::Vec3f>(0, i*10+j)[1] = i<5 ? -1 : 1; // y
|
||||
data.at<cv::Vec3f>(0, i*10+j)[2] = j*0.05; // z
|
||||
}
|
||||
}
|
||||
LaserScan scan(data, data.total(), 0, LaserScan::kXYZ);
|
||||
|
||||
// NormalK
|
||||
LaserScan result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 10);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
int normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), i<50 ? 1.0f : -1.0f, 1e-5); // ny
|
||||
}
|
||||
|
||||
// Radius
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 0, 0.5f);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), i<50 ? 1.0f : -1.0f, 1e-5); // ny
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Util3dFiltering, commonFilteringNormalRGB) {
|
||||
cv::Mat data = cv::Mat::zeros(1, 100, CV_32FC4);
|
||||
// make points all on same plane (0,1,0);
|
||||
for(int i=0; i<10; ++i) {
|
||||
for(int j=0; j<10; ++j) {
|
||||
data.at<cv::Vec4f>(0, i*10+j)[0] = i*0.05; // x
|
||||
data.at<cv::Vec4f>(0, i*10+j)[1] = i<5 ? -1 : 1; // y
|
||||
data.at<cv::Vec4f>(0, i*10+j)[2] = j*0.05; // z
|
||||
data.at<cv::Vec4f>(0, i*10+j)[3] = LaserScan::packRGB(255, 128, 64);
|
||||
}
|
||||
}
|
||||
LaserScan scan(data, data.total(), 0, LaserScan::kXYZRGB);
|
||||
|
||||
// NormalK
|
||||
LaserScan result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 10);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasRGB());
|
||||
int normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), i<50 ? 1.0f : -1.0f, 1e-5); // ny
|
||||
EXPECT_EQ(result.field(i, result.getRGBOffset()), LaserScan::packRGB(255, 128, 64));
|
||||
}
|
||||
|
||||
// Radius
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 0, 0.5f);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasRGB());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), i<50 ? 1.0f : -1.0f, 1e-5); // ny
|
||||
EXPECT_EQ(result.field(i, result.getRGBOffset()), LaserScan::packRGB(255, 128, 64));
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Util3dFiltering, commonFilteringNormalI) {
|
||||
cv::Mat data = cv::Mat::zeros(1, 100, CV_32FC4);
|
||||
// make points all on same plane (0,1,0);
|
||||
for(int i=0; i<10; ++i) {
|
||||
for(int j=0; j<10; ++j) {
|
||||
data.at<cv::Vec4f>(0, i*10+j)[0] = i*0.05; // x
|
||||
data.at<cv::Vec4f>(0, i*10+j)[1] = i<5 ? -1 : 1; // y
|
||||
data.at<cv::Vec4f>(0, i*10+j)[2] = j*0.05; // z
|
||||
data.at<cv::Vec4f>(0, i*10+j)[3] = 420;
|
||||
}
|
||||
}
|
||||
LaserScan scan(data, data.total(), 0, LaserScan::kXYZI);
|
||||
|
||||
// NormalK
|
||||
LaserScan result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 10);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasIntensity());
|
||||
int normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), i<50 ? 1.0f : -1.0f, 1e-5); // ny
|
||||
EXPECT_EQ(result.field(i, result.getIntensityOffset()), 420);
|
||||
}
|
||||
|
||||
// Radius
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 0, 0.5f);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasIntensity());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), i<50 ? 1.0f : -1.0f, 1e-5); // ny
|
||||
EXPECT_EQ(result.field(i, result.getIntensityOffset()), 420);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Util3dFiltering, commonFilteringNormalVoxel) {
|
||||
cv::Mat data = cv::Mat::zeros(1, 100, CV_32FC(6));
|
||||
// make points all on same plane (0,1,0);
|
||||
for(int i=0; i<10; ++i) {
|
||||
for(int j=0; j<10; ++j) {
|
||||
data.at<cv::Vec6f>(0, i*10+j)[0] = i*0.05; // x
|
||||
data.at<cv::Vec6f>(0, i*10+j)[1] = i<5 ? -1 : 1; // y
|
||||
data.at<cv::Vec6f>(0, i*10+j)[2] = j*0.05; // z
|
||||
data.at<cv::Vec6f>(0, i*10+j)[3] = 1; // nx
|
||||
}
|
||||
}
|
||||
LaserScan scan(data, data.total(), 0, LaserScan::kXYZNormal);
|
||||
|
||||
// NormalK
|
||||
LaserScan result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 10);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
int normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset), 1.0f, 1e-5); // nx
|
||||
}
|
||||
|
||||
// Radius
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 0, 0.5f);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset), 1.0f, 1e-5); // nx
|
||||
}
|
||||
|
||||
// combined with voxel filter, the normals should be recomputed
|
||||
// NormalK
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.1f, 5);
|
||||
EXPECT_EQ(result.size(), 30);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
if(result.field(i, 1) < 0) { // y
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), 1, 1e-5); // ny
|
||||
}
|
||||
else {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), -1, 1e-5); // ny
|
||||
}
|
||||
}
|
||||
|
||||
// Radius
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.1f, 0, 0.25f);
|
||||
EXPECT_EQ(result.size(), 30);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
if(result.field(i, 1) < 0) { // y
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), 1, 1e-5); // ny
|
||||
}
|
||||
else {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), -1, 1e-5); // ny
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Util3dFiltering, commonFilteringNormalVoxelRGB) {
|
||||
cv::Mat data = cv::Mat::zeros(1, 100, CV_32FC(7));
|
||||
// make points all on same plane (0,1,0);
|
||||
for(int i=0; i<10; ++i) {
|
||||
for(int j=0; j<10; ++j) {
|
||||
float * ptr = data.ptr<float>(0, i*10+j);
|
||||
ptr[0] = i*0.05; // x
|
||||
ptr[1] = i<5 ? -1 : 1; // y
|
||||
ptr[2] = j*0.05; // z
|
||||
ptr[3] = LaserScan::packRGB(255,128,64); // rgb
|
||||
ptr[4] = 1; // nx
|
||||
}
|
||||
}
|
||||
LaserScan scan(data, data.total(), 0, LaserScan::kXYZRGBNormal);
|
||||
|
||||
// NormalK
|
||||
LaserScan result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 10);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasRGB());
|
||||
int normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset), 1.0f, 1e-5); // nx
|
||||
EXPECT_EQ(result.field(i, result.getRGBOffset()), LaserScan::packRGB(255,128,64)); //rgb
|
||||
}
|
||||
|
||||
// Radius
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 0, 0.5f);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasRGB());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset), 1.0f, 1e-5); // nx
|
||||
EXPECT_EQ(result.field(i, result.getRGBOffset()), LaserScan::packRGB(255,128,64)); //rgb
|
||||
}
|
||||
|
||||
// combined with voxel filter, the normals should be recomputed
|
||||
// NormalK
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.1f, 5);
|
||||
EXPECT_EQ(result.size(), 30);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasRGB());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
if(result.field(i, 1) < 0) { // y
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), 1, 1e-5); // ny
|
||||
}
|
||||
else {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), -1, 1e-5); // ny
|
||||
}
|
||||
EXPECT_EQ(result.field(i, result.getRGBOffset()), LaserScan::packRGB(255,128,64)); //rgb
|
||||
}
|
||||
|
||||
// Radius
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.1f, 0, 0.25f);
|
||||
EXPECT_EQ(result.size(), 30);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasRGB());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
if(result.field(i, 1) < 0) { // y
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), 1, 1e-5); // ny
|
||||
}
|
||||
else {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), -1, 1e-5); // ny
|
||||
}
|
||||
EXPECT_EQ(result.field(i, result.getRGBOffset()), LaserScan::packRGB(255,128,64)); //rgb
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Util3dFiltering, commonFilteringNormalVoxelI) {
|
||||
cv::Mat data = cv::Mat::zeros(1, 100, CV_32FC(7));
|
||||
// make points all on same plane (0,1,0);
|
||||
for(int i=0; i<10; ++i) {
|
||||
for(int j=0; j<10; ++j) {
|
||||
float * ptr = data.ptr<float>(0, i*10+j);
|
||||
ptr[0] = i*0.05; // x
|
||||
ptr[1] = i<5 ? -1 : 1; // y
|
||||
ptr[2] = j*0.05; // z
|
||||
ptr[3] = 420; // intensity
|
||||
ptr[4] = 1; // nx
|
||||
}
|
||||
}
|
||||
LaserScan scan(data, data.total(), 0, LaserScan::kXYZINormal);
|
||||
|
||||
// NormalK
|
||||
LaserScan result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 10);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasIntensity());
|
||||
int normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset), 1.0f, 1e-5); // nx
|
||||
EXPECT_EQ(result.field(i, result.getIntensityOffset()), 420); //intensity
|
||||
}
|
||||
|
||||
// Radius
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 0, 0.5f);
|
||||
EXPECT_EQ(result.size(), 100);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasIntensity());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
EXPECT_NEAR(result.field(i, normalOffset), 1.0f, 1e-5); // nx
|
||||
EXPECT_EQ(result.field(i, result.getIntensityOffset()), 420); //intensity
|
||||
}
|
||||
|
||||
// combined with voxel filter, the normals should be recomputed
|
||||
// NormalK
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.1f, 5);
|
||||
EXPECT_EQ(result.size(), 30);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasIntensity());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
if(result.field(i, 1) < 0) { // y
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), 1, 1e-5); // ny
|
||||
}
|
||||
else {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), -1, 1e-5); // ny
|
||||
}
|
||||
EXPECT_EQ(result.field(i, result.getIntensityOffset()), 420); //intensity
|
||||
}
|
||||
|
||||
// Radius
|
||||
result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.1f, 0, 0.25f);
|
||||
EXPECT_EQ(result.size(), 30);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
EXPECT_TRUE(result.hasIntensity());
|
||||
normalOffset = result.getNormalsOffset();
|
||||
for(int i=0; i<result.size(); ++i) {
|
||||
if(result.field(i, 1) < 0) { // y
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), 1, 1e-5); // ny
|
||||
}
|
||||
else {
|
||||
EXPECT_NEAR(result.field(i, normalOffset+1), -1, 1e-5); // ny
|
||||
}
|
||||
EXPECT_EQ(result.field(i, result.getIntensityOffset()), 420); //intensity
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Util3dFiltering, commonFilteringGroundNormalsUp) {
|
||||
cv::Mat data = cv::Mat::zeros(1, 6, CV_32FC(6));
|
||||
data.at<cv::Vec6f>(0,0)[5] = 1; // nz
|
||||
|
||||
data.at<cv::Vec6f>(0,1)[5] = -1; // nz
|
||||
|
||||
data.at<cv::Vec6f>(0,2)[2] = 15; // z
|
||||
data.at<cv::Vec6f>(0,2)[5] = 1; // nz
|
||||
|
||||
data.at<cv::Vec6f>(0,3)[2] = 15; // z
|
||||
data.at<cv::Vec6f>(0,3)[5] = -1; // nz
|
||||
|
||||
data.at<cv::Vec6f>(0,4)[0] = 5; // x
|
||||
data.at<cv::Vec6f>(0,4)[2] = 5; // z
|
||||
data.at<cv::Vec6f>(0,4)[3] = -cos(M_PI/4); // nx
|
||||
data.at<cv::Vec6f>(0,4)[5] = sin(M_PI/4); // nz (~0.707)
|
||||
|
||||
data.at<cv::Vec6f>(0,5)[0] = 5; // x
|
||||
data.at<cv::Vec6f>(0,5)[2] = 5; // z
|
||||
data.at<cv::Vec6f>(0,5)[3] = -cos(M_PI/4); // nx
|
||||
data.at<cv::Vec6f>(0,5)[5] = -sin(M_PI/4); // nz (-0.707)
|
||||
LaserScan scan(data, data.total(), 0, LaserScan::kXYZNormal, Transform(0,0,10,0,0,0));
|
||||
|
||||
LaserScan result = util3d::commonFiltering(scan, 1, 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.8f);
|
||||
EXPECT_EQ(result.size(), 6);
|
||||
EXPECT_TRUE(result.hasNormals());
|
||||
int nz = result.getNormalsOffset()+2;
|
||||
EXPECT_EQ(result.field(0, nz), 1.0f);
|
||||
EXPECT_EQ(result.field(1, nz), 1.0f);
|
||||
EXPECT_EQ(result.field(2, nz), -1.0f);
|
||||
EXPECT_EQ(result.field(3, nz), -1.0f);
|
||||
EXPECT_FLOAT_EQ(result.field(4, nz), sin(M_PI/4));
|
||||
EXPECT_FLOAT_EQ(result.field(5, nz), -sin(M_PI/4));
|
||||
}
|
||||
Reference in New Issue
Block a user