PCL：实现 shot估计（附完整源码）
发布时间
阅读量：
阅读量
// [T]est: Estimation #include <Eigen/estimation.h> #include <pcl/test/geometry.h> #include <pcl/points.h> #include <pcl/search.h> using namespace Eigen; using namespace pcl; void testShotIndicesAndSearchSurface( const PointCloud<PointXYZ>::Ptr & points, const PointCloud<Normal>::Ptr & normals, const pcl::Indices::Ptr & indices, PointCloud<SHOT352>::Ptr shots ) { pcl::IndicesPtr test_indices(new pcl::Indices(0)); for (std::size_t i = 0; i < points->size(); i += 3) { testindices->pushback(static_cast<int>(i)); } testSHOTIndicesAndSearchSurface<SHOTEstimation<PointXYZ, Normal, SHOT352>>( points.makeShared(), normals, test_indices ); } void testSHOTLocalReferenceFrame( const PointCloud<PointXYZ>::Ptr & points, const PointCloud<Normal>::Ptr & normals, const pcl::Indices::Ptr & indices, PointCloud<SHOT>::Ptr shots ) { std::vector<size_t> descriptors; std::vector<int> rinds(9); for (std::size_t i = 0; i < (indices->size() / 2); ++i) { std::size_t j = *indices->descriptor(i).descriptor(rinds[0]).descriptor(rinds[1]); for (std::sizet k = 0; k < staticcast<int>(j); ++k) { std::string desc(j, 'x'); std::string f(j, 'f'); std::string d(j, 'd'); d[rinds[2]] = "L"; descriptors.push_back(std::string(j, 'x') + d + f); std::string fStr(j, 'f'); fStr[rinds[4]] = "L"; descriptors.push_back(std::string(j, 'x') + d + fStr); } std::vector<stdpoint cloudProps>[j]; point cloud cloud(j); cloud.setPoints(*indices->subset(j)); cloud.setNormals(*normals); cloud.setLocalRadiusSearch(2.4); cloud.setSearchMethod(tree); cloud.setRadiusSearch(4.8); shotCompute<OutputT>(cloud.makeShared(), *indices[j].descriptor(), outputPoints[j]); checkDesc<OutputT> (outputPoints[j], descriptors[j], "Reconstructed results"); } } void testUSCEstimation( const PointCloud<PointXYZ>::Ptr & points, const PointCloud<Normal>::Ptr & normals, const pcl::Indices::Ptr & indices, PointCloud<USC>::Ptr uscds ) { } void main(int argc, char argv) { } </think>plaintext
// [T]
PCL：实现 shot估计
#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h> // for NormalEstimation
#include <pcl/io/pcd_io.h>
#include <pcl/features/shot.h>
#include <pcl/features/shot_omp.h>
#include "pcl/features/shot_lrf.h"
#include <pcl/features/3dsc.h>
#include <pcl/features/usc.h>
using namespace pcl;
using namespace pcl::io;
using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
PointCloud<PointXYZ> cloud;
pcl::Indices indices;
KdTreePtr tree;
///
template<typename PointT> void
shotCopyPointCloud (const PointCloud<PointT> &cloud_in, const pcl::Indices &indices,
PointCloud<PointT> &cloud_out)
{
pcl::copyPointCloud<PointT>(cloud_in, indices, cloud_out);
}
///
template <typename PointT> void
checkDesc(const pcl::PointCloud<PointT>& d0, const pcl::PointCloud<PointT>& d1)
{
ASSERT_EQ (d0.size (), d1.size ());
for (std::size_t i = 0; i < d1.size (); ++i)
for (std::size_t j = 0; j < d0[i].descriptor.size(); ++j)
ASSERT_EQ (d0[i].descriptor[j], d1[i].descriptor[j]);
}
///
template <> void
checkDesc<SHOT352>(const pcl::PointCloud<SHOT352>& d0, const pcl::PointCloud<SHOT352>& d1)
{
ASSERT_EQ (d0.size (), d1.size ());
for (std::size_t i = 0; i < d1.size (); ++i)
for (std::size_t j = 0; j < 352; ++j)
ASSERT_EQ (d0[i].descriptor[j], d1[i].descriptor[j]);
}
///
template <> void
checkDesc<SHOT1344>(const pcl::PointCloud<SHOT1344>& d0, const pcl::PointCloud<SHOT1344>& d1)
{
ASSERT_EQ (d0.size (), d1.size ());
for (std::size_t i = 0; i < d1.size (); ++i)
for (std::size_t j = 0; j < 1344; ++j)
ASSERT_EQ (d0[i].descriptor[j], d1[i].descriptor[j]);
}
///
template <> void
checkDesc<ShapeContext1980>(const pcl::PointCloud<ShapeContext1980>& d0, const pcl::PointCloud<ShapeContext1980>& d1)
{
ASSERT_EQ (d0.size (), d1.size ());
for (std::size_t i = 0; i < d1.size (); ++i)
for (std::size_t j = 0; j < 1980; ++j)
ASSERT_EQ (d0[i].descriptor[j], d1[i].descriptor[j]);
}
///
template <> void
checkDesc<UniqueShapeContext1960>(const pcl::PointCloud<UniqueShapeContext1960>& d0, const pcl::PointCloud<UniqueShapeContext1960>& d1)
{
ASSERT_EQ (d0.size (), d1.size ());
for (std::size_t i = 0; i < d1.size (); ++i)
for (std::size_t j = 0; j < 1960; ++j)
ASSERT_EQ (d0[i].descriptor[j], d1[i].descriptor[j]);
}
///
template <typename FeatureEstimation, typename PointT, typename NormalT, typename OutputT>
struct createSHOTDesc
{
FeatureEstimation operator ()(const typename PointCloud<NormalT>::Ptr & normals,
const int nr_shape_bins_ = 10,
const int = 30,
const bool = true,
const bool = false) const
{
FeatureEstimation f(nr_shape_bins_);
f.setInputNormals (normals);
return (f);
}
};
///
template <typename FeatureEstimation, typename PointT, typename NormalT>
struct createSHOTDesc<FeatureEstimation, PointT, NormalT, SHOT352>
{
FeatureEstimation operator ()(const typename PointCloud<NormalT>::Ptr & normals,
const int = 10,
const int = 30,
const bool = true,
const bool = false) const
{
FeatureEstimation f;
f.setInputNormals (normals);
return (f);
}
};
///
template <typename FeatureEstimation, typename PointT, typename NormalT>
struct createSHOTDesc<FeatureEstimation, PointT, NormalT, SHOT1344>
{
FeatureEstimation operator ()(const typename PointCloud<NormalT>::Ptr & normals,
const int = 10,
const int = 30,
const bool describe_shape = true,
const bool describe_color = false) const
{
FeatureEstimation f (describe_shape, describe_color);
f.setInputNormals (normals);
return (f);
}
};
///
template <typename PointT, typename NormalT, typename OutputT>
struct createSHOTDesc<ShapeContext3DEstimation<PointT, NormalT, OutputT>, PointT, NormalT, OutputT>
{
ShapeContext3DEstimation<PointT, NormalT, OutputT>
operator ()(const typename PointCloud<NormalT>::Ptr & normals,
const int,
const int,
const bool,
const bool) const
{
ShapeContext3DEstimation<PointT, NormalT, OutputT> sc3d;
//sc3d.setAzimuthBins (4);
//sc3d.setElevationBins (4);
//sc3d.setRadiusBins (4);
sc3d.setMinimalRadius (0.004);
sc3d.setPointDensityRadius (0.008);
sc3d.setInputNormals (normals);
return (sc3d);
}
};
///
template <typename PointT, typename NormalT, typename OutputT>
struct createSHOTDesc<UniqueShapeContext<PointT, OutputT>, PointT, NormalT, OutputT>
{
UniqueShapeContext<PointT, OutputT>
operator ()(const typename PointCloud<NormalT>::Ptr &,
const int,
const int,
const bool,
const bool) const
{
UniqueShapeContext<PointT, OutputT> usc;
usc.setMinimalRadius (0.004);
usc.setPointDensityRadius (0.008);
usc.setLocalRadius (0.04);
return (usc);
}
};
///
template <typename FeatureEstimation, typename PointT, typename NormalT, typename OutputT> void
testSHOTIndicesAndSearchSurface (const typename PointCloud<PointT>::Ptr & points,
const typename PointCloud<NormalT>::Ptr & normals,
const pcl::IndicesPtr & indices,
const int nr_shape_bins = 10,
const int nr_color_bins = 30,
const bool describe_shape = true,
const bool describe_color = false)
{
double radius = 0.04;
//
// Test setIndices and setSearchSurface
//
PointCloud<OutputT> full_output, output0, output1, output2;
// Compute for all points and then subsample the results
FeatureEstimation est0 = createSHOTDesc<FeatureEstimation, PointT, NormalT, OutputT>()(normals, nr_shape_bins,nr_color_bins,describe_shape,describe_color);
est0.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
est0.setRadiusSearch (radius);
est0.setInputCloud (points);
est0.compute (full_output);
shotCopyPointCloud<OutputT> (full_output, *indices, output0);
// Compute with all points as "search surface" and the specified sub-cloud as "input"
typename PointCloud<PointT>::Ptr subpoints (new PointCloud<PointT>);
copyPointCloud (*points, *indices, *subpoints);
FeatureEstimation est1 = createSHOTDesc<FeatureEstimation, PointT, NormalT, OutputT>()(normals, nr_shape_bins,nr_color_bins,describe_shape,describe_color);
est1.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
est1.setRadiusSearch (radius);
est1.setInputCloud (subpoints);
est1.setSearchSurface (points);
est1.compute (output1);
Compute with all points as "input" and the specified indices
FeatureEstimation est2 = createSHOTDesc<FeatureEstimation, PointT, NormalT, OutputT>()(normals, nr_shape_bins,nr_color_bins,describe_shape,describe_color);
est2.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
est2.setRadiusSearch (radius);
est2.setInputCloud (points);
est2.setIndices (indices);
est2.compute (output2);
// All three of the above cases should produce equivalent results
checkDesc<OutputT> (output0, output1);
checkDesc<OutputT> (output1, output2);
//
// Test the combination of setIndices and setSearchSurface
//
PointCloud<OutputT> output3, output4;
pcl::IndicesPtr indices2 (new pcl::Indices (0));
for (std::size_t i = 0; i < (indices->size ()/2); ++i)
indices2->push_back (static_cast<int> (i));
// Compute with all points as search surface + the specified sub-cloud as "input" but for only a subset of indices
FeatureEstimation est3 = createSHOTDesc<FeatureEstimation, PointT, NormalT, OutputT>()(normals, nr_shape_bins,nr_color_bins,describe_shape,describe_color);
est3.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
est3.setRadiusSearch (radius);
est3.setSearchSurface (points);
est3.setInputCloud (subpoints);
est3.setIndices (indices2);
est3.compute (output3);
// Start with features for each point in "subpoints" and then subsample the results
shotCopyPointCloud<OutputT> (output0, *indices2, output4); // (Re-using "output0" from above)
// The two cases above should produce equivalent results
checkDesc<OutputT> (output3, output4);
}
///
template <typename FeatureEstimation, typename PointT, typename NormalT, typename OutputT> void
testSHOTLocalReferenceFrame (const typename PointCloud<PointT>::Ptr & points,
const typename PointCloud<NormalT>::Ptr & normals,
const pcl::IndicesPtr & indices,
const int nr_shape_bins = 10,
const int nr_color_bins = 30,
const bool describe_shape = true,
const bool describe_color = false)
{
double radius = 0.04;
typename PointCloud<PointT>::Ptr subpoints (new PointCloud<PointT> ());
copyPointCloud (*points, *indices, *subpoints);
pcl::IndicesPtr indices2 (new pcl::Indices (0));
for (std::size_t i = 0; i < (indices->size ()/2); ++i)
indices2->push_back (static_cast<int> (i));
//
// Test an external computation for the local reference frames
//
PointCloud<ReferenceFrame>::Ptr frames (new PointCloud<ReferenceFrame> ());
SHOTLocalReferenceFrameEstimation<PointT, pcl::ReferenceFrame> lrf_estimator;
lrf_estimator.setRadiusSearch (radius);
lrf_estimator.setInputCloud (subpoints);
lrf_estimator.setIndices (indices2);
lrf_estimator.setSearchSurface(points);
lrf_estimator.compute (*frames);
PointCloud<OutputT> output, output2;
FeatureEstimation est = createSHOTDesc<FeatureEstimation, PointT, NormalT, OutputT>()(normals, nr_shape_bins,nr_color_bins,describe_shape,describe_color);
est.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
est.setRadiusSearch (radius);
est.setSearchSurface (points);
est.setInputCloud (subpoints);
est.setIndices (indices2);
est.compute (output);
FeatureEstimation est2 = createSHOTDesc<FeatureEstimation, PointT, NormalT, OutputT>()(normals, nr_shape_bins,nr_color_bins,describe_shape,describe_color);
est2.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
est2.setRadiusSearch (radius);
est2.setSearchSurface (points);
est2.setInputCloud (subpoints);
est2.setIndices (indices2);
est2.setInputReferenceFrames (frames);
est2.compute (output2);
// Check frames
pcl::PointCloud<pcl::ReferenceFrame>::ConstPtr f = est.getInputReferenceFrames ();
pcl::PointCloud<pcl::ReferenceFrame>::ConstPtr f2 = est2.getInputReferenceFrames ();
ASSERT_EQ (frames->size (), f->size ());
ASSERT_EQ (f2->size (), f->size ());
for (int i = 0; i < static_cast<int> (frames->size ()); ++i)
{
for (unsigned j = 0; j < 9; ++j)
ASSERT_EQ ((*frames)[i].rf[j], (*f)[i].rf[j]);
for (unsigned j = 0; j < 9; ++j)
ASSERT_EQ ((*frames)[i].rf[j], (*f2)[i].rf[j]);
}
// The two cases above should produce equivalent results
checkDesc<OutputT> (output, output2);
}
// "Placeholder" for the type specialized test fixture
template<typename T>
struct SHOTShapeTest;
// Template specialization test for SHOTEstimation
template<>
struct SHOTShapeTest<SHOTEstimation<PointXYZ, Normal, SHOT352> >
: public ::testing::Test
{
SHOTEstimation<PointXYZ, Normal, SHOT352> shot;
};
// Template specialization test for SHOTEstimationOMP
template<>
struct SHOTShapeTest<SHOTEstimationOMP<PointXYZ, Normal, SHOT352> >
: public ::testing::Test
{
SHOTEstimationOMP<PointXYZ, Normal, SHOT352> shot{4}; // 4 threads
};
// Types which will be instantiated
using SHOTEstimatorTypes = ::testing::Types
<SHOTEstimation<PointXYZ, Normal, SHOT352>,
SHOTEstimationOMP<PointXYZ, Normal, SHOT352> >;
TYPED_TEST_SUITE (SHOTShapeTest, SHOTEstimatorTypes);
// This is a copy of the old SHOTShapeEstimation test which will now
// be applied to both SHOTEstimation and SHOTEstimationOMP
TYPED_TEST (SHOTShapeTest, Estimation)
{
// Estimate normals first
double mr = 0.002;
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
n.setInputCloud (cloud.makeShared ());
pcl::IndicesPtr indicesptr (new pcl::Indices (indices));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setRadiusSearch (20 * mr);
n.compute (*normals);
EXPECT_NEAR ((*normals)[103].normal_x, 0.36683175, 1e-4);
EXPECT_NEAR ((*normals)[103].normal_y, -0.44696972, 1e-4);
EXPECT_NEAR ((*normals)[103].normal_z, -0.81587529, 1e-4);
EXPECT_NEAR ((*normals)[200].normal_x, -0.71414840, 1e-4);
EXPECT_NEAR ((*normals)[200].normal_y, -0.06002361, 1e-4);
EXPECT_NEAR ((*normals)[200].normal_z, -0.69741613, 1e-4);
EXPECT_NEAR ((*normals)[140].normal_x, -0.45109111, 1e-4);
EXPECT_NEAR ((*normals)[140].normal_y, -0.19499126, 1e-4);
EXPECT_NEAR ((*normals)[140].normal_z, -0.87091631, 1e-4);
/*
SHOTEstimation<PointXYZ, Normal, SHOT> shot;
shot.setInputNormals (normals);
EXPECT_EQ (shot.getInputNormals (), normals);
shot.setRadiusSearch (20 * mr);
// Object
PointCloud<SHOT>::Ptr shots (new PointCloud<SHOT> ());
// set parameters
shot.setInputCloud (cloud.makeShared ());
shot.setIndices (indicesptr);
shot.setSearchMethod (tree);
// estimate
shot.compute (*shots);
EXPECT_EQ (shots->size (), indices.size ());
EXPECT_NEAR ((*shots)[103].descriptor[9 ], 0.0072018504, 1e-4);
EXPECT_NEAR ((*shots)[103].descriptor[10], 0.0023103887, 1e-4);
EXPECT_NEAR ((*shots)[103].descriptor[11], 0.0024724449, 1e-4);
EXPECT_NEAR ((*shots)[103].descriptor[19], 0.0031367359, 1e-4);
EXPECT_NEAR ((*shots)[103].descriptor[20], 0.17439659, 1e-4);
EXPECT_NEAR ((*shots)[103].descriptor[21], 0.070665278, 1e-4);
EXPECT_NEAR ((*shots)[103].descriptor[42], 0.013304681, 1e-4);
EXPECT_NEAR ((*shots)[103].descriptor[53], 0.0073520984, 1e-4);
EXPECT_NEAR ((*shots)[103].descriptor[54], 0.013584172, 1e-4);
EXPECT_NEAR ((*shots)[103].descriptor[55], 0.0050609680, 1e-4);
*/
// SHOT352
TypeParam& shot352 = this->shot;
shot352.setInputNormals (normals);
EXPECT_EQ (shot352.getInputNormals (), normals);
shot352.setRadiusSearch (20 * mr);
// Object
PointCloud<SHOT352>::Ptr shots352 (new PointCloud<SHOT352> ());
// set parameters
shot352.setInputCloud (cloud.makeShared ());
shot352.setIndices (indicesptr);
shot352.setSearchMethod (tree);
// estimate
shot352.compute (*shots352);
EXPECT_EQ (shots352->size (), indices.size ());
EXPECT_NEAR ((*shots352)[103].descriptor[9 ], 0.0072018504, 1e-4);
EXPECT_NEAR ((*shots352)[103].descriptor[10], 0.0023103887, 1e-4);
EXPECT_NEAR ((*shots352)[103].descriptor[11], 0.0024724449, 1e-4);
EXPECT_NEAR ((*shots352)[103].descriptor[19], 0.0031367359, 1e-4);
EXPECT_NEAR ((*shots352)[103].descriptor[20], 0.17439659, 1e-4);
EXPECT_NEAR ((*shots352)[103].descriptor[21], 0.06542316, 1e-4);
EXPECT_NEAR ((*shots352)[103].descriptor[42], 0.013304681, 1e-4);
EXPECT_NEAR ((*shots352)[103].descriptor[53], 0.0073520984, 1e-4);
EXPECT_NEAR ((*shots352)[103].descriptor[54], 0.013584172, 1e-4);
EXPECT_NEAR ((*shots352)[103].descriptor[55], 0.0050609680, 1e-4);
// Test results when setIndices and/or setSearchSurface are used
pcl::IndicesPtr test_indices (new pcl::Indices (0));
for (std::size_t i = 0; i < cloud.size (); i+=3)
test_indices->push_back (static_cast<int> (i));
//testSHOTIndicesAndSearchSurface<SHOTEstimation<PointXYZ, Normal, SHOT>, PointXYZ, Normal, SHOT> (cloud.makeShared (), normals, test_indices);
//testSHOTLocalReferenceFrame<SHOTEstimation<PointXYZ, Normal, SHOT>, PointXYZ, Normal, SHOT> (cloud.makeShared (), normals, test_indices);
testSHOTIndicesAndSearchSurface<TypeParam, PointXYZ, Normal, SHOT352> (cloud.makeShared (), normals, test_indices);
testSHOTLocalReferenceFrame<TypeParam, PointXYZ, Normal, SHOT352> (cloud.makeShared (), normals, test_indices);
}
//
/*
TEST (PCL, GenericSHOTShapeEstimation)
{
// SHOT length
const int shapeStep_ = 20;
//const int dim = 32*(shapeStep_+1);
// Estimate normals first
double mr = 0.002;
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
n.setInputCloud (cloud.makeShared ());
pcl::IndicesPtr indicesptr (new pcl::Indices (indices));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setRadiusSearch (20 * mr);
n.compute (*normals);
SHOTEstimation<PointXYZ, Normal, SHOT> shot (shapeStep_);
shot.setInputNormals (normals);
EXPECT_EQ (shot.getInputNormals (), normals);
shot.setRadiusSearch (20 * mr);
PointCloud< SHOT >::Ptr shots (new PointCloud< SHOT > ());
// set parameters
shot.setInputCloud (cloud.makeShared ());
shot.setIndices (indicesptr);
shot.setSearchMethod (tree);
// estimate
shot.compute (*shots);
EXPECT_EQ (shots->size (), indices.size ());
EXPECT_NEAR ((*shots)[103].descriptor[18], 0.0077019366, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[19], 0.0024708188, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[21], 0.0079652183, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[38], 0.0067090928, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[39], 0.17498907, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[40], 0.078413926, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[81], 0.014228539, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[103], 0.022390056, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[105], 0.0058866320, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[123], 0.019105887, 1e-5);
// Test results when setIndices and/or setSearchSurface are used
pcl::IndicesPtr test_indices (new pcl::Indices (0));
for (std::size_t i = 0; i < cloud.size (); i+=3)
test_indices->push_back (static_cast<int> (i));
testSHOTIndicesAndSearchSurface<SHOTEstimation<PointXYZ, Normal, SHOT>, PointXYZ, Normal, SHOT> (cloud.makeShared (), normals, test_indices, shapeStep_);
testSHOTLocalReferenceFrame<SHOTEstimation<PointXYZ, Normal, SHOT>, PointXYZ, Normal, SHOT> (cloud.makeShared (), normals, test_indices, shapeStep_);
}
*/
// "Placeholder" for the type specialized test fixture
template<typename T>
struct SHOTShapeAndColorTest;
// Template specialization test for SHOTColorEstimation
template<>
struct SHOTShapeAndColorTest<SHOTColorEstimation<PointXYZRGBA, Normal, SHOT1344> >
: public ::testing::Test
{
SHOTColorEstimation<PointXYZRGBA, Normal, SHOT1344> shot;
};
// Template specialization test for SHOTColorEstimationOMP
template<>
struct SHOTShapeAndColorTest<SHOTColorEstimationOMP<PointXYZRGBA, Normal, SHOT1344> >
: public ::testing::Test
{
SHOTColorEstimationOMP<PointXYZRGBA, Normal, SHOT1344> shot{true, true, 4}; // 4 threads
};
// Types which will be instantiated
using SHOTColorEstimatorTypes= ::testing::Types
<SHOTColorEstimation<PointXYZRGBA, Normal, SHOT1344>,
SHOTColorEstimationOMP<PointXYZRGBA, Normal, SHOT1344> >;
TYPED_TEST_SUITE (SHOTShapeAndColorTest, SHOTColorEstimatorTypes);
// This is a copy of the old SHOTShapeAndColorEstimation test which will now
// be applied to both SHOTColorEstimation and SHOTColorEstimationOMP
TYPED_TEST (SHOTShapeAndColorTest, Estimation)
{
double mr = 0.002;
// Estimate normals first
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
n.setInputCloud (cloud.makeShared ());
pcl::IndicesPtr indicesptr (new pcl::Indices (indices));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setRadiusSearch (20 * mr);
n.compute (*normals);
search::KdTree<PointXYZRGBA>::Ptr rgbaTree;
rgbaTree.reset (new search::KdTree<PointXYZRGBA> (false));
// Create fake point cloud with colors
PointCloud<PointXYZRGBA> cloudWithColors;
for (int i = 0; i < static_cast<int> (cloud.size ()); ++i)
{
PointXYZRGBA p;
p.x = cloud[i].x;
p.y = cloud[i].y;
p.z = cloud[i].z;
p.rgba = ( (i%255) << 16 ) + ( ( (255 - i ) %255) << 8) + ( ( i*37 ) %255);
cloudWithColors.push_back(p);
}
/*
// Object
SHOTEstimation<PointXYZRGBA, Normal, SHOT> shot (true, true);
shot.setInputNormals (normals);
EXPECT_EQ (shot.getInputNormals (), normals);
shot.setRadiusSearch ( 20 * mr);
rgbaTree->setInputCloud (cloudWithColors.makeShared ());
PointCloud<SHOT>::Ptr shots (new PointCloud<SHOT>);
shot.setInputCloud (cloudWithColors.makeShared ());
shot.setIndices (indicesptr);
shot.setSearchMethod (rgbaTree);
// estimate
shot.compute (*shots);
EXPECT_EQ (shots->size (), indices.size ());
EXPECT_NEAR ((*shots)[103].descriptor[10], 0.0020453099, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[11], 0.0021887729, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[21], 0.062557608, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[42], 0.011778189, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[53], 0.0065085669, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[54], 0.012025614, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[55], 0.0044803056, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[64], 0.064429596, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[65], 0.046486385, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[86], 0.011518310, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[357], 0.0020453099, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[360], 0.0027993850, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[386], 0.045115642, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[387], 0.059068538, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[389], 0.0047547864, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[453], 0.0051176427, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[481], 0.0053625242, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[482], 0.012025614, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[511], 0.0057367259, 1e-5);
EXPECT_NEAR ((*shots)[103].descriptor[512], 0.048357654, 1e-5);
*/
// SHOT1344
TypeParam& shot1344 = this->shot;
shot1344.setInputNormals (normals);
EXPECT_EQ (shot1344.getInputNormals (), normals);
shot1344.setRadiusSearch ( 20 * mr);
PointCloud<SHOT1344>::Ptr shots1344 (new PointCloud<SHOT1344>);
shot1344.setInputCloud (cloudWithColors.makeShared ());
shot1344.setIndices (indicesptr);
shot1344.setSearchMethod (rgbaTree);
// estimate
shot1344.compute (*shots1344);
EXPECT_EQ (shots1344->size (), indices.size ());
EXPECT_NEAR ((*shots1344)[103].descriptor[10], 0.0020453099, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[11], 0.0021887729, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[21], 0.0579300672, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[42], 0.011778189, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[53], 0.0065085669, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[54], 0.012025614, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[55], 0.0044803056, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[64], 0.064453065, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[65], 0.046504568, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[86], 0.011518310, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[357], 0.0020453099, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[360], 0.0027993850, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[386], 0.0451327376, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[387], 0.0544394031, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[389], 0.0047547864, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[453], 0.0051176427, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[481], 0.0053625242, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[482], 0.012025614, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[511], 0.0057367259, 1e-5);
EXPECT_NEAR ((*shots1344)[103].descriptor[512], 0.048375979, 1e-5);
// Test results when setIndices and/or setSearchSurface are used
pcl::IndicesPtr test_indices (new pcl::Indices (0));
for (std::size_t i = 0; i < cloud.size (); i+=3)
test_indices->push_back (static_cast<int> (i));
//testSHOTIndicesAndSearchSurface<SHOTEstimation<PointXYZRGBA, Normal, SHOT>, PointXYZRGBA, Normal, SHOT> (cloudWithColors.makeShared (), normals, test_indices);
//testSHOTLocalReferenceFrame<SHOTEstimation<PointXYZRGBA, Normal, SHOT>, PointXYZRGBA, Normal, SHOT> (cloudWithColors.makeShared (), normals, test_indices);
testSHOTIndicesAndSearchSurface<TypeParam, PointXYZRGBA, Normal, SHOT1344> (cloudWithColors.makeShared (), normals, test_indices);
testSHOTLocalReferenceFrame<TypeParam, PointXYZRGBA, Normal, SHOT1344> (cloudWithColors.makeShared (), normals, test_indices);
}
//
TEST (PCL,3DSCEstimation)
{
float meshRes = 0.002f;
//size_t nBinsL = 4;
//size_t nBinsK = 4;
//size_t nBinsJ = 4;
float radius = 20.0f * meshRes;
float rmin = radius / 10.0f;
float ptDensityRad = radius / 5.0f;
PointCloud<PointXYZ>::Ptr cloudptr = cloud.makeShared ();
// Estimate normals first
NormalEstimation<PointXYZ, Normal> ne;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
ne.setInputCloud (cloudptr);
ne.setSearchMethod (tree);
ne.setRadiusSearch (radius);
// estimate
ne.compute (*normals);
ShapeContext3DEstimation<PointXYZ, Normal, ShapeContext1980> sc3d;
sc3d.setInputCloud (cloudptr);
sc3d.setInputNormals (normals);
sc3d.setSearchMethod (tree);
sc3d.setRadiusSearch (radius);
//sc3d.setAzimuthBins (nBinsL);
//sc3d.setElevationBins (nBinsK);
//sc3d.setRadiusBins (nBinsJ);
sc3d.setMinimalRadius (rmin);
sc3d.setPointDensityRadius (ptDensityRad);
// Compute the features
PointCloud<ShapeContext1980>::Ptr sc3ds (new PointCloud<ShapeContext1980> ());
sc3d.compute (*sc3ds);
EXPECT_EQ (sc3ds->size (), cloud.size ());
// 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user
//EXPECT_NEAR ((*sc3ds)[0].rf[0], 0.2902f, 1e-4f);
//EXPECT_NEAR ((*sc3ds)[0].rf[1], 0.7334f, 1e-4f);
//EXPECT_NEAR ((*sc3ds)[0].rf[2], -0.6146f, 1e-4f);
//EXPECT_NEAR ((*sc3ds)[0].rf[3], 0.9486f, 1e-4f);
//EXPECT_NEAR ((*sc3ds)[0].rf[4], -0.3051f, 1e-4f);
//EXPECT_NEAR ((*sc3ds)[0].rf[5], 0.0838f, 1e-4f);
//EXPECT_NEAR ((*sc3ds)[0].rf[6], -0.1261f, 1e-4f);
//EXPECT_NEAR ((*sc3ds)[0].rf[7], -0.6074f, 1e-4f);
//EXPECT_NEAR ((*sc3ds)[0].rf[8], -0.7843f, 1e-4f);
EXPECT_FLOAT_EQ ((*sc3ds)[0].rf[0], 0.0f);
EXPECT_FLOAT_EQ ((*sc3ds)[0].rf[1], 0.0f);
EXPECT_FLOAT_EQ ((*sc3ds)[0].rf[2], 0.0f);
EXPECT_FLOAT_EQ ((*sc3ds)[0].rf[3], 0.0f);
EXPECT_FLOAT_EQ ((*sc3ds)[0].rf[4], 0.0f);
EXPECT_FLOAT_EQ ((*sc3ds)[0].rf[5], 0.0f);
EXPECT_FLOAT_EQ ((*sc3ds)[0].rf[6], 0.0f);
EXPECT_FLOAT_EQ ((*sc3ds)[0].rf[7], 0.0f);
EXPECT_FLOAT_EQ ((*sc3ds)[0].rf[8], 0.0f);
//EXPECT_EQ ((*sc3ds)[0].descriptor.size (), 64);
EXPECT_FLOAT_EQ ((*sc3ds)[94].descriptor[88], 55.271168f);
EXPECT_FLOAT_EQ ((*sc3ds)[94].descriptor[584], 71.108765f);
EXPECT_FLOAT_EQ ((*sc3ds)[94].descriptor[1106], 79.5896f);
EXPECT_FLOAT_EQ ((*sc3ds)[94].descriptor[1560], 0.f);
EXPECT_FLOAT_EQ ((*sc3ds)[94].descriptor[1929], 36.063553f);
EXPECT_FLOAT_EQ ((*sc3ds)[108].descriptor[67], 0.f);
EXPECT_FLOAT_EQ ((*sc3ds)[108].descriptor[548], 126.14106f);
EXPECT_FLOAT_EQ ((*sc3ds)[108].descriptor[1091], 30.470392f);
EXPECT_FLOAT_EQ ((*sc3ds)[108].descriptor[1421], 38.08799f);
EXPECT_FLOAT_EQ ((*sc3ds)[108].descriptor[1900], 43.799442f);
// Test results when setIndices and/or setSearchSurface are used
pcl::IndicesPtr test_indices (new pcl::Indices (0));
for (std::size_t i = 0; i < cloud.size (); i++)
test_indices->push_back (static_cast<int> (i));
testSHOTIndicesAndSearchSurface<ShapeContext3DEstimation<PointXYZ, Normal, ShapeContext1980>, PointXYZ, Normal, ShapeContext1980> (cloudptr, normals, test_indices);
}
//
TEST (PCL, USCEstimation)
{
float meshRes = 0.002f;
float radius = 20.0f * meshRes;
float rmin = radius / 10.0f;
float ptDensityRad = radius / 5.0f;
// estimate
UniqueShapeContext<PointXYZ, UniqueShapeContext1960> uscd;
uscd.setInputCloud (cloud.makeShared ());
uscd.setSearchMethod (tree);
uscd.setRadiusSearch (radius);
uscd.setMinimalRadius (rmin);
uscd.setPointDensityRadius (ptDensityRad);
uscd.setLocalRadius (radius);
// Compute the features
PointCloud<UniqueShapeContext1960>::Ptr uscds (new PointCloud<UniqueShapeContext1960>);
uscd.compute (*uscds);
EXPECT_EQ (uscds->size (), cloud.size ());
EXPECT_NEAR ((*uscds)[160].rf[0], -0.97767f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].rf[1], 0.0353674f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].rf[2], -0.20715f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].rf[3], 0.0125394f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].rf[4], 0.993798f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].rf[5], 0.110493f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].rf[6], 0.209773f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].rf[7], 0.105428f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].rf[8], -0.972049f, 1e-4f);
//EXPECT_EQ ((*uscds)[0].descriptor.size (), 64);
EXPECT_NEAR ((*uscds)[160].descriptor[355], 123.0733f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].descriptor[494], 154.9401f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].descriptor[897], 0.f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].descriptor[1178], 62.7496f, 1e-4f);
EXPECT_NEAR ((*uscds)[160].descriptor[1878], 31.3748f, 1e-4f);
EXPECT_NEAR ((*uscds)[168].descriptor[57], 39.4986f, 1e-4f);
EXPECT_NEAR ((*uscds)[168].descriptor[704], 0.f, 1e-4f);
EXPECT_NEAR ((*uscds)[168].descriptor[906], 48.8803f, 1e-4f);
EXPECT_NEAR ((*uscds)[168].descriptor[1175], 83.4680f, 1e-4f);
EXPECT_NEAR ((*uscds)[168].descriptor[1756], 65.1737f, 1e-4f);
// Test results when setIndices and/or setSearchSurface are used
pcl::IndicesPtr test_indices (new pcl::Indices (0));
for (std::size_t i = 0; i < cloud.size (); i+=3)
test_indices->push_back (static_cast<int> (i));
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
testSHOTIndicesAndSearchSurface<UniqueShapeContext<PointXYZ, UniqueShapeContext1960>, PointXYZ, Normal, UniqueShapeContext1960> (cloud.makeShared (), normals, test_indices);
testSHOTLocalReferenceFrame<UniqueShapeContext<PointXYZ, UniqueShapeContext1960>, PointXYZ, Normal, UniqueShapeContext1960> (cloud.makeShared (), normals, test_indices);
}
/* ---[ */
int
main (int argc, char** argv)
{
if (argc < 2)
{
std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
return (-1);
}
if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0)
{
std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
return (-1);
}
indices.resize (cloud.size ());
for (std::size_t i = 0; i < indices.size (); ++i)
indices[i] = static_cast<int> (i);
tree.reset (new search::KdTree<PointXYZ> (false));
tree->setInputCloud (cloud.makeShared ());
testing::InitGoogleTest (&argc, argv);
return (RUN_ALL_TESTS ());
}
/* ]--- */
AI助手本文为作者独立创作的文章，请勿未经许可进行转载。完整文章链接：
全部评论 (0)
还没有任何评论哟~