Added normal computation sample and ply writing with visible normals for debugging

modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp

@@ -70,6 +70,14 @@ CV_EXPORTS Mat loadPLYSimple(const char* fileName, int withNormals);
 */
 CV_EXPORTS void writePLY(Mat PC, const char* fileName);
 
+/**
+*  @brief Used for debbuging pruposes, writes a point cloud to a PLY file with the tip
+*  of the normal vectors as visible red points
+*  @param [in] PC Input point cloud
+*  @param [in] fileName The PLY model file to write
+*/
+CV_EXPORTS void writePLYVisibleNormals(Mat PC, const char* fileName);
+
 Mat samplePCUniform(Mat PC, int sampleStep);
 Mat samplePCUniformInd(Mat PC, int sampleStep, std::vector<int>& indices);
 

modules/surface_matching/samples/ppf_normal_computation.cpp

+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+
+#include <iostream>
+#include "opencv2/surface_matching.hpp"
+#include "opencv2/surface_matching/ppf_helpers.hpp"
+
+using namespace std;
+
+static void help(const string& errorMessage)
+{
+  cout << "Program init error : " << errorMessage << endl;
+  cout << "\nUsage : ppf_normal_computation [input model file] [output model file]" << endl;
+  cout << "\nPlease start again with new parameters" << endl;
+}
+
+int main(int argc, char** argv)
+{
+  if (argc < 3)
+  {
+    help("Not enough input arguments");
+    exit(1);
+  }
+
+  string modelFileName = (string)argv[1];
+  string outputFileName = (string)argv[2];
+  cv::Mat points, pointsAndNormals;
+
+  cout << "Loading points\n";
+  cv::ppf_match_3d::loadPLYSimple(modelFileName.c_str(), 1).copyTo(points);
+
+  cout << "Computing normals\n";
+  double viewpoint[3] = { 0.0, 0.0, 0.0 };
+  cv::ppf_match_3d::computeNormalsPC3d(points, pointsAndNormals, 6, false, viewpoint);
+
+  std::cout << "Writing points\n";
+  cv::ppf_match_3d::writePLY(pointsAndNormals, outputFileName.c_str());
+  //the following function can also be used for debugging purposes
+  //cv::ppf_match_3d::writePLYVisibleNormals(pointsAndNormals, outputFileName.c_str());
+
+  std::cout << "Done\n";
+  return 0;
+}

modules/surface_matching/src/ppf_helpers.cpp

@@ -163,6 +163,62 @@ void writePLY(Mat PC, const char* FileName)
   return;
 }
 
+void writePLYVisibleNormals(Mat PC, const char* FileName)
+{
+  std::ofstream outFile(FileName);
+
+  if (!outFile)
+  {
+    //cerr << "Error opening output file: " << FileName << "!" << endl;
+    printf("Error opening output file: %s!\n", FileName);
+    exit(1);
+  }
+
+  ////
+  // Header
+  ////
+
+  const int pointNum = (int)PC.rows;
+  const int vertNum = (int)PC.cols;
+  const bool hasNormals = vertNum == 6;
+
+  outFile << "ply" << std::endl;
+  outFile << "format ascii 1.0" << std::endl;
+  outFile << "element vertex " << (hasNormals? 2*pointNum:pointNum) << std::endl;
+  outFile << "property float x" << std::endl;
+  outFile << "property float y" << std::endl;
+  outFile << "property float z" << std::endl;
+  if (hasNormals)
+  {
+    outFile << "property uchar red" << std::endl;
+    outFile << "property uchar green" << std::endl;
+    outFile << "property uchar blue" << std::endl;
+  }
+  outFile << "end_header" << std::endl;
+
+  ////
+  // Points
+  ////
+
+  for (int pi = 0; pi < pointNum; ++pi)
+  {
+    const float* point = (float*)(&PC.data[pi*PC.step]);
+
+    outFile << point[0] << " " << point[1] << " " << point[2];
+
+    if (hasNormals)
+    {
+      outFile << " 127 127 127" << std::endl;
+      outFile << point[0]+point[3] << " " << point[1]+point[4] << " " << point[2]+point[5];
+      outFile << " 255 0 0";
+    }
+
+    outFile << std::endl;
+  }
+
+  return;
+}
+
 Mat samplePCUniform(Mat PC, int sampleStep)
 {
   int numRows = PC.rows/sampleStep;