Fix Cropbox with indices. Add new assertions for unit tests. (#3306)

Fix Cropbox with indices. Add new assertions for unit tests.
PointCloudLibrary · Sep 10, 2019 · 745789c · 745789c
2 parents 000a762 + 720566e
commit 745789c
Show file tree

Hide file tree

Showing 4 changed files with 432 additions and 12 deletions.
diff --git a/common/include/pcl/pcl_tests.h b/common/include/pcl/pcl_tests.h
@@ -219,6 +219,52 @@ namespace pcl
         return ::testing::AssertionSuccess ();
       }
 
+
+        template<typename V>
+        ::testing::AssertionResult VectorContainsAll(const char*  expr1, const char* expr2, const std::vector<V>& elements, const std::vector<V>& v) {
+            for(const V& item : elements) {
+                if(std::find(v.cbegin(), v.cend(), item)==v.cend()) {
+
+                    std::ostringstream vec_rep;
+                    std::copy(v.cbegin(), v.cend()-1, std::ostream_iterator<V>(vec_rep, ", "));
+                    vec_rep<<v.back();
+
+                    std::ostringstream elements_rep;
+                    std::copy(elements.cbegin(), elements.cend()-1, std::ostream_iterator<V>(elements_rep, ", "));
+                    elements_rep << elements.back();
+
+                    return ::testing::AssertionFailure ()
+                            << "Actual       : " << expr2 << std::endl
+                            << "contains     : " << vec_rep.str() << std::endl
+                            << "Target set   : " << expr1 << std::endl
+                            << "contains     : " << elements_rep.str() << std::endl;
+                }
+            }
+            return ::testing::AssertionSuccess ();
+        }
+
+        template<typename V>
+        ::testing::AssertionResult VectorDoesNotContain(const char*  expr1, const char*  expr2, const std::vector<V>& elements, const std::vector<V>& v) {
+            for(const V& item : elements) {
+                if(std::find(v.cbegin(), v.cend(), item)!=v.cend()) {
+                    std::ostringstream vec_rep;
+                    std::copy(v.cbegin(), v.cend()-1, std::ostream_iterator<V>(vec_rep, ", "));
+                    vec_rep<<v.back();
+
+                    std::ostringstream elements_rep;
+                    std::copy(elements.cbegin(), elements.cend()-1, std::ostream_iterator<V>(elements_rep, ", "));
+                    elements_rep << elements.back();
+
+                    return ::testing::AssertionFailure ()
+                            << "Actual       : " << expr2 << std::endl
+                            << "contains     : " << vec_rep.str() << std::endl
+                            << "Forbidden set: " << expr1 << std::endl
+                            << "contains     : " << elements_rep.str() << std::endl;
+                }
+            }
+            return ::testing::AssertionSuccess ();
+        }
+
     }
 
   }
@@ -312,3 +358,35 @@ namespace pcl
 #define EXPECT_METADATA_EQ(expected, actual)             \
   EXPECT_PRED_FORMAT2(::pcl::test::internal::MetaDataEQ, \
                       expected, actual)
+
+
+/// Expect that the vector contains all elements
+/// from the expected vector.
+#define EXPECT_VECTOR_CONTAINS_ALL(expected, actual)            \
+    EXPECT_PRED_FORMAT2(::pcl::test::internal::VectorContainsAll, \
+                        expected, actual)
+
+
+/// Expect that the vector does not contain any element
+/// from the expected vector.
+#define EXPECT_VECTOR_DOES_NOT_CONTAIN(expected, actual)           \
+    EXPECT_PRED_FORMAT2(::pcl::test::internal::VectorDoesNotContain, \
+                        expected, actual)
+
+
+
+/// Assert that the vector contains all elements
+/// from the expected vector.
+#define ASSERT_VECTOR_CONTAINS_ALL(expected, actual)            \
+    ASSERT_PRED_FORMAT2(::pcl::test::internal::VectorContainsAll, \
+                        expected, actual)
+
+
+/// Assert that the vector does not contain any element
+/// from the expected vector.
+#define ASSERT_VECTOR_DOES_NOT_CONTAIN(expected, actual)           \
+    ASSERT_PRED_FORMAT2(::pcl::test::internal::VectorDoesNotContain, \
+                        expected, actual)
+
+
+
diff --git a/filters/include/pcl/filters/impl/crop_box.hpp b/filters/include/pcl/filters/impl/crop_box.hpp
@@ -98,7 +98,7 @@ pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
   {
     if (!input_->is_dense)
       // Check if the point is invalid
-      if (!isFinite (input_->points[index]))
+      if (!isFinite (input_->points[(*indices_)[index]]))
         continue;
 
     // Get local point
@@ -126,13 +126,13 @@ pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
       if (negative_)
         indices[indices_count++] = (*indices_)[index];
       else if (extract_removed_indices_)
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
+        (*removed_indices_)[removed_indices_count++] = static_cast<int> ((*indices_)[index]);
     }
     // If inside the cropbox
     else
     {
       if (negative_ && extract_removed_indices_)
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
+        (*removed_indices_)[removed_indices_count++] = static_cast<int> ((*indices_)[index]);
       else if (!negative_) 
         indices[indices_count++] = (*indices_)[index];
     }

diff --git a/filters/src/crop_box.cpp b/filters/src/crop_box.cpp
@@ -114,15 +114,15 @@ pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
       }
       else if (extract_removed_indices_)
       {
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
+        (*removed_indices_)[removed_indices_count++] = static_cast<int> ((*indices_)[index]);
       }
     }
     // If inside the cropbox
     else
     {
       if (negative_ && extract_removed_indices_)
       {
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
+        (*removed_indices_)[removed_indices_count++] = static_cast<int> ((*indices_)[index]);
       }
       else if (!negative_) {
         memcpy (&output.data[indices_count++ * output.point_step],
@@ -197,15 +197,15 @@ pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
       }
       else if (extract_removed_indices_)
       {
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
+        (*removed_indices_)[removed_indices_count++] = static_cast<int> ((*indices_)[index]);
       }
     }
     // If inside the cropbox
     else
     {
       if (negative_ && extract_removed_indices_)
       {
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
+        (*removed_indices_)[removed_indices_count++] = static_cast<int> ((*indices_)[index]);
       }
       else if (!negative_) {
         indices[indices_count++] = (*indices_)[index];