diff --git a/features/include/pcl/features/impl/3dsc.hpp b/features/include/pcl/features/impl/3dsc.hpp index 8ce62ab68e1..fc9d257c5b6 100644 --- a/features/include/pcl/features/impl/3dsc.hpp +++ b/features/include/pcl/features/impl/3dsc.hpp @@ -164,17 +164,20 @@ pcl::ShapeContext3DEstimation::computePoint ( } normal = normals[minIndex].getNormalVector3fMap (); - // Compute and store the RF direction - x_axis[0] = rnd (); - x_axis[1] = rnd (); - x_axis[2] = rnd (); - if (!pcl::utils::equal (normal[2], 0.0f)) - x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2]; - else if (!pcl::utils::equal (normal[1], 0.0f)) - x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1]; - else if (!pcl::utils::equal (normal[0], 0.0f)) - x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0]; - + // Compute a deterministic x_axis by projecting the direction to the most + // distant neighbor onto the tangent plane. This replaces the previous + // random axis selection and makes the descriptor stable across runs. + const auto maxDistIt = std::max_element (nn_dists.begin (), nn_dists.end ()); + const auto maxIndex = nn_indices[std::distance (nn_dists.begin (), maxDistIt)]; + x_axis = (*surface_)[maxIndex].getVector3fMap () - origin; + x_axis -= x_axis.dot (normal) * normal; + if (x_axis.norm () < 1e-6f) + { + // Fallback: use a fixed global axis projected onto the tangent plane + x_axis = Eigen::Vector3f::UnitX () - Eigen::Vector3f::UnitX ().dot (normal) * normal; + if (x_axis.norm () < 1e-6f) + x_axis = Eigen::Vector3f::UnitY () - Eigen::Vector3f::UnitY ().dot (normal) * normal; + } x_axis.normalize (); // Check if the computed x axis is orthogonal to the normal @@ -187,7 +190,7 @@ pcl::ShapeContext3DEstimation::computePoint ( for (std::size_t ne = 0; ne < neighb_cnt; ne++) { if (pcl::utils::equal (nn_dists[ne], 0.0f)) - continue; + continue; // Get neighbours coordinates Eigen::Vector3f neighbour = (*surface_)[nn_indices[ne]].getVector3fMap (); @@ -245,8 +248,9 @@ pcl::ShapeContext3DEstimation::computePoint ( assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0); } // end for each neighbour - // 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user - std::fill_n (rf, 9, 0); + // Note: unlike the original 3DSC, we keep the RF intact here because + // we now compute a deterministic x_axis. The original code zeroed the RF + // to signal non-repeatability, but that also destroyed azimuth binning. return (true); } @@ -258,7 +262,7 @@ pcl::ShapeContext3DEstimation::computeFeature (Poi output.is_dense = true; // Iterate over all points and compute the descriptors - for (std::size_t point_index = 0; point_index < indices_->size (); point_index++) + for (std::size_t point_index = 0; point_index < indices_->size (); point_index++) { //output[point_index].descriptor.resize (descriptor_length_); diff --git a/test/features/test_shot_estimation.cpp b/test/features/test_shot_estimation.cpp index d84b958578e..a6156e42a8b 100644 --- a/test/features/test_shot_estimation.cpp +++ b/test/features/test_shot_estimation.cpp @@ -461,9 +461,6 @@ TYPED_TEST (SHOTShapeTest, Estimation) for (std::size_t i = 0; i < cloud.size (); i+=3) test_indices->push_back (static_cast (i)); - //testSHOTIndicesAndSearchSurface, PointXYZ, Normal, SHOT> (cloud.makeShared (), normals, test_indices); - //testSHOTLocalReferenceFrame, PointXYZ, Normal, SHOT> (cloud.makeShared (), normals, test_indices); - testSHOTIndicesAndSearchSurface (cloud.makeShared (), normals, test_indices); testSHOTLocalReferenceFrame (cloud.makeShared (), normals, test_indices); } @@ -670,9 +667,6 @@ TYPED_TEST (SHOTShapeAndColorTest, Estimation) for (std::size_t i = 0; i < cloud.size (); i+=3) test_indices->push_back (static_cast (i)); - //testSHOTIndicesAndSearchSurface, PointXYZRGBA, Normal, SHOT> (cloudWithColors.makeShared (), normals, test_indices); - //testSHOTLocalReferenceFrame, PointXYZRGBA, Normal, SHOT> (cloudWithColors.makeShared (), normals, test_indices); - testSHOTIndicesAndSearchSurface (cloudWithColors.makeShared (), normals, test_indices); testSHOTLocalReferenceFrame (cloudWithColors.makeShared (), normals, test_indices); } @@ -681,9 +675,6 @@ TYPED_TEST (SHOTShapeAndColorTest, Estimation) 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; @@ -705,9 +696,6 @@ TEST (PCL,3DSCEstimation) 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 @@ -716,39 +704,37 @@ TEST (PCL,3DSCEstimation) 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_NEAR ((*sc3ds)[0].rf[0], -0.932515f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[0].rf[1], 0.342274f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[0].rf[2], -0.115173f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[0].rf[3], 0.33841f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[0].rf[4], 0.716871f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[0].rf[5], -0.60957f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[0].rf[6], -0.126076f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[0].rf[7], -0.607408f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[0].rf[8], -0.784321f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[108].descriptor[54], 54.0953f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[108].descriptor[71], 76.176f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[108].descriptor[87], 64.4415f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[108].descriptor[88], 110.54235f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[108].descriptor[89], 55.3068f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[108].descriptor[218], 126.141f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[108].descriptor[235], 88.8147f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[108].descriptor[237], 43.5572f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[108].descriptor[238], 56.0383f, 1e-4f); + EXPECT_NEAR ((*sc3ds)[108].descriptor[253], 36.8475f, 1e-4f); + + EXPECT_FLOAT_EQ ((*sc3ds)[94].descriptor[88], 27.635588f); + EXPECT_FLOAT_EQ ((*sc3ds)[94].descriptor[584], 47.405849f); + EXPECT_FLOAT_EQ ((*sc3ds)[94].descriptor[1106], 39.794807f); 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); + EXPECT_FLOAT_EQ ((*sc3ds)[108].descriptor[67], 0.f); + EXPECT_FLOAT_EQ ((*sc3ds)[108].descriptor[548], 0.f); + EXPECT_FLOAT_EQ ((*sc3ds)[108].descriptor[1091], 0.f); + EXPECT_FLOAT_EQ ((*sc3ds)[108].descriptor[1421], 0.f); + EXPECT_FLOAT_EQ ((*sc3ds)[108].descriptor[1900], 0.f); // Test results when setIndices and/or setSearchSurface are used pcl::IndicesPtr test_indices (new pcl::Indices (0)); @@ -789,8 +775,6 @@ TEST (PCL, USCEstimation) 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); @@ -839,4 +823,4 @@ main (int argc, char** argv) testing::InitGoogleTest (&argc, argv); return (RUN_ALL_TESTS ()); } -/* ]--- */ +/* ]--- */ \ No newline at end of file