Merge pull request #1022 from Sahloul:features/python_wrapper/surface_matching_step

modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp

@@ -148,7 +148,7 @@ protected:
   double angle_step, angle_step_radians, distance_step;
   double sampling_step_relative, angle_step_relative, distance_step_relative;
   Mat sampled_pc, ppf;
-  int num_ref_points, ppf_step;
+  int num_ref_points;
   hashtable_int* hash_table;
   THash* hash_nodes;
 

modules/surface_matching/src/icp.cpp

@@ -50,7 +50,7 @@ static void subtractColumns(Mat srcPC, double mean[3])
 
   for (int i=0; i<height; i++)
   {
-    float *row = (float*)(&srcPC.data[i*srcPC.step]);
+    float *row = srcPC.ptr<float>(i);
     {
       row[0]-=(float)mean[0];
       row[1]-=(float)mean[1];
@@ -68,7 +68,7 @@ static void computeMeanCols(Mat srcPC, double mean[3])
 
   for (int i=0; i<height; i++)
   {
-    const float *row = (float*)(&srcPC.data[i*srcPC.step]);
+    const float *row = srcPC.ptr<float>(i);
     {
       mean1 += (double)row[0];
       mean2 += (double)row[1];
@@ -100,7 +100,7 @@ static double computeDistToOrigin(Mat srcPC)
 
   for (int i=0; i<height; i++)
   {
-    const float *row = (float*)(&srcPC.data[i*srcPC.step]);
+    const float *row = srcPC.ptr<float>(i);
     dist += sqrt(row[0]*row[0]+row[1]*row[1]+row[2]*row[2]);
   }
 
@@ -203,11 +203,11 @@ static void minimizePointToPlaneMetric(Mat Src, Mat Dst, Mat& X)
 #endif
   for (int i=0; i<Src.rows; i++)
   {
-    const double *srcPt = (double*)&Src.data[i*Src.step];
+    const double *srcPt = Src.ptr<double>(i);
-    const double *dstPt = (double*)&Dst.data[i*Dst.step];
+    const double *dstPt = Dst.ptr<double>(i);
     const double *normals = &dstPt[3];
-    double *bVal = (double*)&b.data[i*b.step];
+    double *bVal = b.ptr<double>(i);
-    double *aRow = (double*)&A.data[i*A.step];
+    double *aRow = A.ptr<double>(i);
 
     const double sub[3]={dstPt[0]-srcPt[0], dstPt[1]-srcPt[1], dstPt[2]-srcPt[2]};
 
@@ -382,13 +382,13 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
 
     while ( (!(fval_perc<(1+TolP) && fval_perc>(1-TolP))) && i<MaxIterationsPyr)
     {
-      size_t di=0, selInd = 0;
+      uint di=0, selInd = 0;
 
       queryPCFlann(flann, Src_Moved, Indices, Distances);
 
       for (di=0; di<numElSrc; di++)
       {
-        newI[di] = (int)di;
+        newI[di] = di;
         newJ[di] = indices[di];
       }
 
@@ -455,17 +455,17 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
       if (selInd)
       {
 
-        Mat Src_Match = Mat((int)selInd, srcPCT.cols, CV_64F);
+        Mat Src_Match = Mat(selInd, srcPCT.cols, CV_64F);
-        Mat Dst_Match = Mat((int)selInd, srcPCT.cols, CV_64F);
+        Mat Dst_Match = Mat(selInd, srcPCT.cols, CV_64F);
 
         for (di=0; di<selInd; di++)
         {
           const int indModel = indicesModel[di];
           const int indScene = indicesScene[di];
-          const float *srcPt = (float*)&srcPCT.data[indModel*srcPCT.step];
+          const float *srcPt = srcPCT.ptr<float>(indModel);
-          const float *dstPt = (float*)&dstPC0.data[indScene*dstPC0.step];
+          const float *dstPt = dstPC0.ptr<float>(indScene);
-          double *srcMatchPt = (double*)&Src_Match.data[di*Src_Match.step];
+          double *srcMatchPt = Src_Match.ptr<double>(di);
-          double *dstMatchPt = (double*)&Dst_Match.data[di*Dst_Match.step];
+          double *dstMatchPt = Dst_Match.ptr<double>(di);
           int ci=0;
 
           for (ci=0; ci<srcPCT.cols; ci++)

modules/surface_matching/src/ppf_helpers.cpp

@@ -86,7 +86,7 @@ Mat loadPLYSimple(const char* fileName, int withNormals)
 
   for (int i = 0; i < numVertices; i++)
   {
-    float* data = (float*)(&cloud.data[i*cloud.step[0]]);
+    float* data = cloud.ptr<float>(i);
     if (withNormals)
     {
       ifs >> data[0] >> data[1] >> data[2] >> data[3] >> data[4] >> data[5];
@@ -148,7 +148,7 @@ void writePLY(Mat PC, const char* FileName)
 
   for ( int pi = 0; pi < pointNum; ++pi )
   {
-    const float* point = (float*)(&PC.data[ pi*PC.step ]);
+    const float* point = PC.ptr<float>(pi);
 
     outFile << point[0] << " "<<point[1]<<" "<<point[2];
 
@@ -202,7 +202,7 @@ void writePLYVisibleNormals(Mat PC, const char* FileName)
 
   for (int pi = 0; pi < pointNum; ++pi)
   {
-    const float* point = (float*)(&PC.data[pi*PC.step]);
+    const float* point = PC.ptr<float>(pi);
 
     outFile << point[0] << " " << point[1] << " " << point[2];
 
@@ -295,7 +295,7 @@ Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrang
   //#pragma omp parallel for
   for (int i=0; i<pc.rows; i++)
   {
-    const float* point = (float*)(&pc.data[i * pc.step]);
+    const float* point = pc.ptr<float>(i);
 
     // quantize a point
     const int xCell =(int) ((float)numSamplesDim*(point[0]-xrange[0])/xr);
@@ -342,7 +342,7 @@ Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrang
         for (int j=0; j<cn; j++)
         {
           const int ptInd = curCell[j];
-          float* point = (float*)(&pc.data[ptInd * pc.step]);
+          float* point = pc.ptr<float>(ptInd);
           const double dx = point[0]-xc;
           const double dy = point[1]-yc;
           const double dz = point[2]-zc;
@@ -380,7 +380,7 @@ Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrang
         for (int j=0; j<cn; j++)
         {
           const int ptInd = curCell[j];
-          float* point = (float*)(&pc.data[ptInd * pc.step]);
+          float* point = pc.ptr<float>(ptInd);
 
           px += (double)point[0];
           py += (double)point[1];
@@ -399,7 +399,7 @@ Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrang
 
       }
 
-      float *pcData = (float*)(&pcSampled.data[c*pcSampled.step[0]]);
+      float *pcData = pcSampled.ptr<float>(c);
       pcData[0]=(float)px;
       pcData[1]=(float)py;
       pcData[2]=(float)pz;
@@ -542,8 +542,8 @@ Mat transformPCPose(Mat pc, double Pose[16])
 #endif
   for (int i=0; i<pc.rows; i++)
   {
-    const float *pcData = (float*)(&pc.data[i*pc.step]);
+    const float *pcData = pc.ptr<float>(i);
-    float *pcDataT = (float*)(&pct.data[i*pct.step]);
+    float *pcDataT = pct.ptr<float>(i);
     const float *n1 = &pcData[3];
     float *nT = &pcDataT[3];
 
@@ -738,7 +738,7 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe
 
   for (i=0; i<PC.rows; i++)
   {
-    const float* src = (float*)(&PC.data[i*PC.step]);
+    const float* src = PC.ptr<float>(i);
     float* dst = (float*)(&dataset[i*3]);
 
     dst[0] = src[0];
@@ -763,7 +763,7 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe
   {
     double C[3][3], mu[4];
     const float* pci = &dataset[i*3];
-    float* pcr = (float*)(&PCNormals.data[i*PCNormals.step]);
+    float* pcr = PCNormals.ptr<float>(i);
     double nr[3];
 
     int* indLocal = &indices[i*NumNeighbors];

modules/surface_matching/src/ppf_match_3d.cpp

@@ -41,7 +41,7 @@
 #include "precomp.hpp"
 #include "hash_murmur.hpp"
 
-namespace cv 
+namespace cv
 {
 namespace ppf_match_3d
 {
@@ -252,8 +252,6 @@ void PPF3DDetector::trainModel(const Mat &PC)
 
   int numPPF = sampled.rows*sampled.rows;
   ppf = Mat(numPPF, PPF_LENGTH, CV_32FC1);
-  int ppfStep = (int)ppf.step;
-  int sampledStep = (int)sampled.step;
 
   // TODO: Maybe I could sample 1/5th of them here. Check the performance later.
   int numRefPoints = sampled.rows;
@@ -268,7 +266,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
   // since this is just a training part.
   for (int i=0; i<numRefPoints; i++)
   {
-    float* f1 = (float*)(&sampled.data[i * sampledStep]);
+    float* f1 = sampled.ptr<float>(i);
     const double p1[4] = {f1[0], f1[1], f1[2], 0};
     const double n1[4] = {f1[3], f1[4], f1[5], 0};
 
@@ -278,7 +276,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
       // cannnot compute the ppf with myself
       if (i!=j)
       {
-        float* f2 = (float*)(&sampled.data[j * sampledStep]);
+        float* f2 = sampled.ptr<float>(j);
         const double p2[4] = {f2[0], f2[1], f2[2], 0};
         const double n2[4] = {f2[3], f2[4], f2[5], 0};
 
@@ -286,8 +284,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
         computePPFFeatures(p1, n1, p2, n2, f);
         KeyType hashValue = hashPPF(f, angle_step_radians, distanceStep);
         double alpha = computeAlpha(p1, n1, p2);
-        unsigned int corrInd = i*numRefPoints+j;
+        unsigned int ppfInd = i*numRefPoints+j;
-        unsigned int ppfInd = corrInd*ppfStep;
 
         THash* hashNode = &hash_nodes[i*numRefPoints+j];
         hashNode->id = hashValue;
@@ -296,7 +293,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
 
         hashtableInsertHashed(hashTable, hashValue, (void*)hashNode);
 
-        float* ppfRow = (float*)(&(ppf.data[ ppfInd ]));
+        float* ppfRow = ppf.ptr<float>(ppfInd);
         ppfRow[0] = (float)f[0];
         ppfRow[1] = (float)f[1];
         ppfRow[2] = (float)f[2];
@@ -309,7 +306,6 @@ void PPF3DDetector::trainModel(const Mat &PC)
   angle_step = angle_step_radians;
   distance_step = distanceStep;
   hash_table = hashTable;
-  ppf_step = ppfStep;
   num_ref_points = numRefPoints;
   sampled_pc = sampled;
   trained = true;
@@ -513,7 +509,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
     unsigned int refIndMax = 0, alphaIndMax = 0;
     unsigned int maxVotes = 0;
 
-    float* f1 = (float*)(&sampled.data[i * sampled.step]);
+    float* f1 = sampled.ptr<float>(i);
     const double p1[4] = {f1[0], f1[1], f1[2], 0};
     const double n1[4] = {f1[3], f1[4], f1[5], 0};
     double *row2, *row3, tsg[3]={0}, Rsg[9]={0}, RInv[9]={0};
@@ -532,7 +528,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
     {
       if (i!=j)
       {
-        float* f2 = (float*)(&sampled.data[j * sampled.step]);
+        float* f2 = sampled.ptr<float>(j);
         const double p2[4] = {f2[0], f2[1], f2[2], 0};
         const double n2[4] = {f2[3], f2[4], f2[5], 0};
         double p2t[4], alpha_scene;
@@ -565,7 +561,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
           THash* tData = (THash*) node->data;
           int corrI = (int)tData->i;
           int ppfInd = (int)tData->ppfInd;
-          float* ppfCorrScene = (float*)(&ppf.data[ppfInd]);
+          float* ppfCorrScene = ppf.ptr<float>(ppfInd);
           double alpha_model = (double)ppfCorrScene[PPF_LENGTH-1];
           double alpha = alpha_model - alpha_scene;
 
@@ -620,7 +616,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
                         };
 
     // TODO : Compute pose
-    const float* fMax = (float*)(&sampled_pc.data[refIndMax * sampled_pc.step]);
+    const float* fMax = sampled_pc.ptr<float>(refIndMax);
     const double pMax[4] = {fMax[0], fMax[1], fMax[2], 1};
     const double nMax[4] = {fMax[3], fMax[4], fMax[5], 1};