implement registration

try running simple wsl-command fails
add Ply-File export
change build-target to x64 (for wsl)

PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc

@@ -1,7 +1,9 @@
 // An example showing TEASER++ registration with the Stanford bunny model
 #include <chrono>
 #include <iostream>
+#include <fstream>
 #include <random>
+#include <filesystem>
 
 #include <Eigen/Core>
 
@@ -10,70 +12,50 @@
 
 // Macro constants for generating noise and outliers
 #define NOISE_BOUND 0.05
-#define N_OUTLIERS 1700
-#define OUTLIER_TRANSLATION_LB 5
-#define OUTLIER_TRANSLATION_UB 10
+// #define N_OUTLIERS 1700
+// #define OUTLIER_TRANSLATION_LB 5
+// #define OUTLIER_TRANSLATION_UB 10
 
-inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est) {
+inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est)
+{
   return std::abs(std::acos(fmin(fmax(((R_exp.transpose() * R_est).trace() - 1) / 2, -1.0), 1.0)));
 }
 
-void addNoiseAndOutliers(Eigen::Matrix<double, 3, Eigen::Dynamic>& tgt) {
-  // Add uniform noise
-  Eigen::Matrix<double, 3, Eigen::Dynamic> noise =
-      Eigen::Matrix<double, 3, Eigen::Dynamic>::Random(3, tgt.cols()) * NOISE_BOUND;
-  NOISE_BOUND / 2;
-  tgt = tgt + noise;
+int main(int argc, char **argv)
+{
+  std::cout << "You have entered " << argc
+            << " arguments:"
+            << "\n";
 
-  // Add outliers
-  std::random_device rd;
-  std::mt19937 gen(rd());
-  std::uniform_int_distribution<> dis2(0, tgt.cols() - 1); // pos of outliers
-  std::uniform_int_distribution<> dis3(OUTLIER_TRANSLATION_LB,
-                                       OUTLIER_TRANSLATION_UB); // random translation
-  std::vector<bool> expected_outlier_mask(tgt.cols(), false);
-  for (int i = 0; i < N_OUTLIERS; ++i) {
-    int c_outlier_idx = dis2(gen);
-    assert(c_outlier_idx < expected_outlier_mask.size());
-    expected_outlier_mask[c_outlier_idx] = true;
-    tgt.col(c_outlier_idx).array() += dis3(gen); // random translation
-  }
-}
+  for (int i = 0; i < argc; ++i)
+    std::cout << argv[i] << "\n";
+
+  auto src_fileName = argv[1];
+  auto tgt_fileName = argv[2];
 
-int main() {
   // Load the .ply file
   teaser::PLYReader reader;
   teaser::PointCloud src_cloud;
-  auto status = reader.read("./example_data/bun_zipper_res3.ply", src_cloud);
-  int N = src_cloud.size();
+  auto status = reader.read(src_fileName, src_cloud);
+  int src_cloud_size = src_cloud.size();
 
   // Convert the point cloud to Eigen
-  Eigen::Matrix<double, 3, Eigen::Dynamic> src(3, N);
-  for (size_t i = 0; i < N; ++i) {
+  Eigen::Matrix<double, 3, Eigen::Dynamic> src(3, src_cloud_size);
+  for (size_t i = 0; i < src_cloud_size; ++i)
+  {
     src.col(i) << src_cloud[i].x, src_cloud[i].y, src_cloud[i].z;
   }
 
-  // Homogeneous coordinates
-  Eigen::Matrix<double, 4, Eigen::Dynamic> src_h;
-  src_h.resize(4, src.cols());
-  src_h.topRows(3) = src;
-  src_h.bottomRows(1) = Eigen::Matrix<double, 1, Eigen::Dynamic>::Ones(N);
+  teaser::PointCloud tgt_cloud;
+  status = reader.read(tgt_fileName, tgt_cloud);
+  int tgt_cloud_size = tgt_cloud.size();
 
-  // Apply an arbitrary SE(3) transformation
-  Eigen::Matrix4d T;
-  // clang-format off
-  T << 9.96926560e-01,  6.68735757e-02, -4.06664421e-02, -1.15576939e-01,
-      -6.61289946e-02, 9.97617877e-01,  1.94008687e-02, -3.87705398e-02,
-      4.18675510e-02, -1.66517807e-02,  9.98977765e-01, 1.14874890e-01,
-      0,              0,                0,              1;
-  // clang-format on
-
-  // Apply transformation
-  Eigen::Matrix<double, 4, Eigen::Dynamic> tgt_h = T * src_h;
-  Eigen::Matrix<double, 3, Eigen::Dynamic> tgt = tgt_h.topRows(3);
-
-  // Add some noise & outliers
-  addNoiseAndOutliers(tgt);
+  // Convert the point cloud to Eigen
+  Eigen::Matrix<double, 3, Eigen::Dynamic> tgt(3, tgt_cloud_size);
+  for (size_t i = 0; i < tgt_cloud_size; ++i)
+  {
+    tgt.col(i) << tgt_cloud[i].x, tgt_cloud[i].y, tgt_cloud[i].z;
+  }
 
   // Run TEASER++ registration
   // Prepare solver parameters
@@ -99,21 +81,12 @@ int main() {
   std::cout << "=====================================" << std::endl;
   std::cout << "          TEASER++ Results           " << std::endl;
   std::cout << "=====================================" << std::endl;
-  std::cout << "Expected rotation: " << std::endl;
-  std::cout << T.topLeftCorner(3, 3) << std::endl;
   std::cout << "Estimated rotation: " << std::endl;
   std::cout << solution.rotation << std::endl;
-  std::cout << "Error (deg): " << getAngularError(T.topLeftCorner(3, 3), solution.rotation)
-            << std::endl;
   std::cout << std::endl;
-  std::cout << "Expected translation: " << std::endl;
-  std::cout << T.topRightCorner(3, 1) << std::endl;
   std::cout << "Estimated translation: " << std::endl;
   std::cout << solution.translation << std::endl;
-  std::cout << "Error (m): " << (T.topRightCorner(3, 1) - solution.translation).norm() << std::endl;
   std::cout << std::endl;
-  std::cout << "Number of correspondences: " << N << std::endl;
-  std::cout << "Number of outliers: " << N_OUTLIERS << std::endl;
   std::cout << "Time taken (s): "
             << std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count() /
                    1000000.0