Add option to save subsampled clouds used for registration

nmellado · Feb 21, 2017 · ed916df · ed916df
1 parent 5faec54
commit ed916df
Show file tree

Hide file tree

Showing 3 changed files with 67 additions and 5 deletions.
diff --git a/src/super4pcs/shared4pcs.h b/src/super4pcs/shared4pcs.h
@@ -200,6 +200,9 @@ class MatchSuper4PCS {
   float ComputeTransformation(const std::vector<Point3D>& P,
                               std::vector<Point3D>* Q, cv::Mat* transformation);
 
+  const std::vector<Point3D>& getFirstSampled() const;
+  const std::vector<Point3D>& getSecondSampled() const;
+
  private:
   std::unique_ptr<MatchSuper4PCSImpl> pimpl_;
 };

diff --git a/src/super4pcs/super4pcs.cc b/src/super4pcs/super4pcs.cc
@@ -283,6 +283,16 @@ class MatchSuper4PCSImpl {
   float ComputeTransformation(const std::vector<Point3D>& P,
                               std::vector<Point3D>* Q, cv::Mat* transformation);
 
+  // Read access to the sampled clouds used for the registration
+  inline const std::vector<Point3D>& getFirstSampled() const {
+      return sampled_P_3D_;
+  }
+
+  // Read access to the sampled clouds used for the registration
+  inline const std::vector<Point3D>& getSecondSampled() const {
+      return sampled_Q_3D_;
+  }
+
  private:
   // Private data contains parameters and internal variables that are computed
   // and change during the match computation. All parameters have default
@@ -446,6 +456,7 @@ class MatchSuper4PCSImpl {
   // terminated (the target LCP was obtained or the maximum number of trials has
   // been reached), false otherwise.
   bool Perform_N_steps(int n, cv::Mat* transformation, std::vector<Point3D>* Q);
+
 };
 
 // Finds congruent candidates in the set Q, given the invariants and threshold
@@ -1471,5 +1482,16 @@ MatchSuper4PCS::ComputeTransformation(const std::vector<Point3D>& P,
                                        cv::Mat* transformation) {
   return pimpl_->ComputeTransformation(P, Q, transformation);
 }
+
+const std::vector<Point3D>&
+MatchSuper4PCS::getFirstSampled() const{
+  return pimpl_->getFirstSampled();
+}
+
+const std::vector<Point3D>&
+MatchSuper4PCS::getSecondSampled() const{
+  return pimpl_->getSecondSampled();
+}
+
 }
 
diff --git a/src/super4pcs/super4pcs_test.cc b/src/super4pcs/super4pcs_test.cc
@@ -28,6 +28,12 @@ std::string defaultOutput = "output.obj";
 // Transformation matrice.
 std::string outputMat = "";
 
+// Sampled cloud 1
+std::string outputSampled1 = "";
+
+// Sampled cloud 2
+std::string outputSampled2 = "";
+
 // Delta (see the paper).
 double delta = 5.0;
 
@@ -77,6 +83,10 @@ void getArgs(int argc, char **argv) {
       outputMat = argv[++i];
     } else if (!strcmp(argv[i], "-x")) {
       use_super4pcs = false;
+    } else if (!strcmp(argv[i], "--sampled1")) {
+      outputSampled1 = argv[++i];
+    } else if (!strcmp(argv[i], "--sampled2")) {
+      outputSampled2 = argv[++i];
     } else if (!strcmp(argv[i], "-h")) {
       fprintf(stderr, "\nUsage: %s -i input1 input2\n\n", argv[0]);
       fprintf(stderr, "\t[ -o overlap (%2.2f) ]\n", overlap);
@@ -88,6 +98,8 @@ void getArgs(int argc, char **argv) {
       fprintf(stderr, "\t[ -r result_file_name (%s) ]\n", output.c_str());
       fprintf(stderr, "\t[ -m output matrix file (%s) ]\n", outputMat.c_str());
       fprintf(stderr, "\t[ -x (use 4pcs: false by default) ]\n");
+      fprintf(stderr, "\t[ --sampled1 (output sampled cloud 1 -- debug+super4pcs only) ]\n");
+      fprintf(stderr, "\t[ --sampled2 (output sampled cloud 2 -- debug+super4pcs only) ]\n");
       exit(0);
     } else if (argv[i][0] == '-') {
       cerr << "Unknown flag\n";
@@ -184,13 +196,40 @@ int main(int argc, char **argv) {
 	  if (use_super4pcs) {
 		  MatchSuper4PCS matcher(options);
 		  cout << "Use Super4PCS" << endl;
-		  score = matcher.ComputeTransformation(set1, &set2, &mat);
+          score = matcher.ComputeTransformation(set1, &set2, &mat);
+
+          if(! outputSampled1.empty() ){
+              std::cout << "Exporting Sampled cloud 1 to "
+                        << outputSampled1.c_str()
+                        << "..." << std::flush;
+              iomananger.WriteObject((char *)outputSampled1.c_str(),
+                                     matcher.getFirstSampled(),
+                                     vector<cv::Point2f>(),
+                                     vector<cv::Point3f>(),
+                                     vector<tripple>(),
+                                     vector<string>());
+              std::cout << "DONE" << std::endl;
+          }
+
+          if(! outputSampled2.empty() ){
+              std::cout << "Exporting Sampled cloud 2 to "
+                        << outputSampled2.c_str()
+                        << "..." << std::flush;
+              iomananger.WriteObject((char *)outputSampled2.c_str(),
+                                     matcher.getSecondSampled(),
+                                     vector<cv::Point2f>(),
+                                     vector<cv::Point3f>(),
+                                     vector<tripple>(),
+                                     vector<string>());
+              std::cout << "DONE" << std::endl;
+          }
 	  }
 	  else {
 		  Match4PCS matcher(options);
 		  cout << "Use old 4PCS" << endl;
-		  score = matcher.ComputeTransformation(set1, &set2, &mat);
-	  }
+          score = matcher.ComputeTransformation(set1, &set2, &mat);
+      }
+
   }
   catch (const std::exception& e) {
 	  std::cout << "[Error]: " << e.what() << '\n';
@@ -224,8 +263,6 @@ int main(int argc, char **argv) {
       iomananger.WriteMatrix(outputMat, mat, IOManager::POLYWORKS);
       std::cout << "DONE" << std::endl;
   }
-
-
 
   if (! output.empty() ){
       std::cout << "Exporting Registered geometry to "