Completed very basic self-test of single and double-pass calculations

src/mean_and_covariance.hpp

@@ -15,7 +15,7 @@ namespace experimental
  *    | 3 4 5 |
  *    | 6 7 8 |
  *
- * Then:
+ * Where:
  *
  *    0 = mean(X*X) - sum(X) * sum(X)
  *    1 = mean(X*Y) - sum(X) * sum(Y)
@@ -77,9 +77,9 @@ computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
 }
 
 
+
 /* This is based on the double-pass calculation suggested here:
  *    https://github.com/PointCloudLibrary/pcl/pull/1407
- *
  */
 template <typename PointT, typename Scalar> inline unsigned int
 computeMeanAndCovarianceMatrixDoublePass (const pcl::PointCloud<PointT> &cloud,
@@ -135,11 +135,6 @@ computeMeanAndCovarianceMatrixDoublePass (const pcl::PointCloud<PointT> &cloud,
   return (static_cast<unsigned int> (point_count));
 }
 
-int add_two_numbers(int a, int b)
-{
-  return (a+b);
-}
-
 
 } // namespace experimental
 

test/test_mean_and_covariance.cpp

@@ -1,14 +1,56 @@
 #include <gtest/gtest.h>
 #include <pcl/point_types.h>              // PointXYZ, PointXYZRGB etc
 #include <pcl/point_cloud.h>
-#include <mean_and_covariance.hpp>
+#include <mean_and_covariance.hpp>        // Includes all the functions we want to test
 
-TEST(TestCaseName, TestCase)
+using namespace pcl;
+using namespace pcl::experimental;
+
+
+TEST(MeanAndCovariance, SelfTest)
 {
-  int a = 1;
-  int b = 2;
-  int c = pcl::experimental::add_two_numbers(a,b);
-  EXPECT_EQ(c, 3);
+  // Create a super-basic cloud
+  PointCloud<PointXYZ> c;
+  PointXYZ p1 (1.0f, 1.0f, 0.0f), p2 (1.0f, -1.0f, 0.0f),
+           p3 (-1.0f, -1.0f, 0.0f), p4 (-1.0f, 1.0f, 0.0f);
+  c.push_back(p1); c.push_back(p2); c.push_back(p3); c.push_back(p4);
+
+  // Create the rest of our input parameters and our expected results
+  const std::vector<int> indices = {0,1,2,3};
+  Eigen::Vector4f centroid_sp, centroid_dp, centroid_expected;
+  Eigen::Matrix3f covariance_matrix_sp, covariance_matrix_dp,
+                  covariance_matrix_expected;
+  centroid_expected = {0,0,0,1};
+  covariance_matrix_expected << 1.0, 0.0, 0.0,
+                                0.0, 1.0, 0.0,
+                                0.0, 0.0, 0.0;
+
+  // Input to our covariance matrix functions
+  computeMeanAndCovarianceMatrix(c, indices, covariance_matrix_sp, centroid_sp);
+  computeMeanAndCovarianceMatrixDoublePass(c, indices, covariance_matrix_dp,
+                                           centroid_dp);
+
+  // Check calculations vs expected values
+  EXPECT_TRUE(centroid_sp.isApprox(centroid_expected));
+  EXPECT_TRUE(centroid_dp.isApprox(centroid_expected));
+  EXPECT_TRUE(covariance_matrix_sp.isApprox(covariance_matrix_expected));
+  EXPECT_TRUE(covariance_matrix_dp.isApprox(covariance_matrix_expected));
+
+
+//  // In general, three options for comparing calculated with expected values
+
+//  // Method 1 (explicit)
+//  EXPECT_EQ(centroid_sp[0], 0.0); EXPECT_EQ(centroid_sp[1], 0.0);
+//  EXPECT_EQ(centroid_sp[2], 0.0); EXPECT_EQ(centroid_sp[3], 1.0);
+
+//  // Method 2 (iterate)
+//  for (int i = 0; i < 4; i++)
+//  {
+//    EXPECT_EQ(centroid_expected[i], centroid_sp[i]);
+//  }
+
+//  // Method 3 (Eigen approx)
+//  EXPECT_TRUE(centroid_sp.isApprox(centroid_expected));
 }