Refactored API classes, removed unused params

modules/surface_matching/include/opencv2/surface_matching/icp.hpp

@@ -89,9 +89,9 @@ public:
 
   ICP()
   {
-    m_tolerence = 0.005f;
+    m_tolerance = 0.005f;
     m_rejectionScale = 2.5f;
-    m_maxItereations = 250;
+    m_maxIterations = 250;
     m_numLevels = 6;
     m_sampleType = ICP_SAMPLING_TYPE_UNIFORM;
     m_numNeighborsCorr = 1;
@@ -112,10 +112,10 @@ public:
      */
   ICP(const int iterations, const float tolerence=0.05, const float rejectionScale=2.5, const int numLevels=6, const ICP_SAMPLING_TYPE sampleType = ICP_SAMPLING_TYPE_UNIFORM, const int numMaxCorr=1)
   {
-    m_tolerence = tolerence;
+    m_tolerance = tolerence;
     m_numNeighborsCorr = numMaxCorr;
     m_rejectionScale = rejectionScale;
-    m_maxItereations = iterations;
+    m_maxIterations = iterations;
     m_numLevels = numLevels;
     m_sampleType = sampleType;
   }
@@ -147,8 +147,8 @@ public:
   int registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3D*>& poses);
 
 private:
-  float m_tolerence;
-  int m_maxItereations;
+  float m_tolerance;
+  int m_maxIterations;
   float m_rejectionScale;
   int m_numNeighborsCorr;
   int m_numLevels;

modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp

@@ -66,7 +66,7 @@ public:
     residual = 0;
 
     for (int i=0; i<16; i++)
-      Pose[i]=0;
+      pose[i]=0;
   }
 
   Pose3D(double Alpha, unsigned int ModelIndex=0, unsigned int NumVotes=0)
@@ -77,7 +77,7 @@ public:
     residual=0;
 
     for (int i=0; i<16; i++)
-      Pose[i]=0;
+      pose[i]=0;
   }
 
   /**
@@ -117,7 +117,7 @@ public:
   double alpha, residual;
   unsigned int modelIndex;
   unsigned int numVotes;
-  double Pose[16], angle, t[3], q[4];
+  double pose[16], angle, t[3], q[4];
 };
 
 /**

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

@@ -67,16 +67,14 @@ namespace cv
 namespace ppf_match_3d
 {
 
-#define T_PPF_LENGTH 5
-
 /**
   * @struct THash
   * @brief Struct, holding a node in the hashtable
   */
 typedef struct THash
 {
-    int id;
-    int i, ppfInd;
+  int id;
+  int i, ppfInd;
 } THash;
 
 /**
@@ -113,13 +111,11 @@ public:
 
   /**
     *  Set the parameters for the search
-    *  @param [in] numPoses The maximum number of poses to return
     *  @param [in] positionThreshold Position threshold controlling the similarity of translations. Depends on the units of calibration/model.
     *  @param [in] rotationThreshold Position threshold controlling the similarity of rotations. This parameter can be perceived as a threshold over the difference of angles
-    *  @param [in] minMatchScore Not used at the moment
     *  @param [in] useWeightedClustering The algorithm by default clusters the poses without weighting. A non-zero value would indicate that the pose clustering should take into account the number of votes as the weights and perform a weighted averaging instead of a simple one.
     */
-  void setSearchParams(const int numPoses=5, const double positionThreshold=-1, const double rotationThreshold=-1, const double minMatchScore=0.5, const bool useWeightedClustering=false);
+  void setSearchParams(const double positionThreshold=-1, const double rotationThreshold=-1, const bool useWeightedClustering=false);
 
   /**
     *  \brief Trains a new model.
@@ -145,19 +141,17 @@ public:
 
 protected:
 
-  double maxDist, angle_step, angleStepRadians, distance_step;
-  double samplingStepRelative, angleStepRelative, distanceStepRelative;
-  Mat inputPC, sampledPC, PPF;
-  int num_ref_points, sampled_step, ppf_step;
+  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;
   hashtable_int* hash_table;
   THash* hash_nodes;
 
-  int NumPoses;
-  double PositionThreshold, RotationThreshold, MinMatchScore;
-  bool UseWeightedAvg;
+  double position_threshold, rotation_threshold;
+  bool use_weighted_avg;
 
-  float sampleStepSearch;
-  int SceneSampleStep;
+  int scene_sample_step;
 
   void clearTrainingModels();
 

modules/surface_matching/samples/ppf_load_match.cpp

@@ -133,12 +133,12 @@ int main(int argc, char** argv)
     // debug first five poses
     for (size_t i=0; i<resultsSub.size(); i++)
     {
-        Pose3D* pose = resultsSub[i];
+        Pose3D* result = resultsSub[i];
         cout << "Pose Result " << i << endl;
-        pose->printPose();
+        result->printPose();
         if (i==0)
         {
-            Mat pct = transformPCPose(pc, pose->Pose);
+            Mat pct = transformPCPose(pc, result->pose);
             writePLY(pct, "para6700PCTrans.ply");
         }
     }

modules/surface_matching/src/icp.cpp

@@ -338,8 +338,8 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& Residu
     double div = pow((double)impact, (double)level);
     //double div2 = div*div;
     const int numSamples = cvRound((double)(n/(div)));
-    const double TolP = m_tolerence*(double)(level+1)*(level+1);
-    const int MaxIterationsPyr = cvRound((double)m_maxItereations/(level+1));
+    const double TolP = m_tolerance*(double)(level+1)*(level+1);
+    const int MaxIterationsPyr = cvRound((double)m_maxIterations/(level+1));
 
     // Obtain the sampled point clouds for this level: Also rotates the normals
     Mat srcPCT = transformPCPose(srcPC0, Pose);
@@ -546,10 +546,10 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Po
 {
   for (size_t i=0; i<poses.size(); i++)
   {
-    double PoseICP[16]={0};
-    Mat srcTemp = transformPCPose(srcPC, poses[i]->Pose);
-    registerModelToScene(srcTemp, dstPC, poses[i]->residual, PoseICP);
-    poses[i]->appendPose(PoseICP);
+    double poseICP[16]={0};
+    Mat srcTemp = transformPCPose(srcPC, poses[i]->pose);
+    registerModelToScene(srcTemp, dstPC, poses[i]->residual, poseICP);
+    poses[i]->appendPose(poseICP);
   }
 
   return 0;

modules/surface_matching/src/pose_3d.cpp

@@ -50,21 +50,21 @@ void Pose3D::updatePose(double NewPose[16])
   double R[9];
 
   for (int i=0; i<16; i++)
-    Pose[i]=NewPose[i];
-
-  R[0] = Pose[0];
-  R[1] = Pose[1];
-  R[2] = Pose[2];
-  R[3] = Pose[4];
-  R[4] = Pose[5];
-  R[5] = Pose[6];
-  R[6] = Pose[8];
-  R[7] = Pose[9];
-  R[8] = Pose[10];
-
-  t[0]=Pose[3];
-  t[1]=Pose[7];
-  t[2]=Pose[11];
+    pose[i]=NewPose[i];
+
+  R[0] = pose[0];
+  R[1] = pose[1];
+  R[2] = pose[2];
+  R[3] = pose[4];
+  R[4] = pose[5];
+  R[5] = pose[6];
+  R[6] = pose[8];
+  R[7] = pose[9];
+  R[8] = pose[10];
+
+  t[0]=pose[3];
+  t[1]=pose[7];
+  t[2]=pose[11];
 
   // compute the angle
   const double trace = R[0] + R[4] + R[8];
@@ -89,22 +89,22 @@ void Pose3D::updatePose(double NewPose[16])
 
 void Pose3D::updatePose(double NewR[9], double NewT[3])
 {
-  Pose[0]=NewR[0];
-  Pose[1]=NewR[1];
-  Pose[2]=NewR[2];
-  Pose[3]=NewT[0];
-  Pose[4]=NewR[3];
-  Pose[5]=NewR[4];
-  Pose[6]=NewR[5];
-  Pose[7]=NewT[1];
-  Pose[8]=NewR[6];
-  Pose[9]=NewR[7];
-  Pose[10]=NewR[8];
-  Pose[11]=NewT[2];
-  Pose[12]=0;
-  Pose[13]=0;
-  Pose[14]=0;
-  Pose[15]=1;
+  pose[0]=NewR[0];
+  pose[1]=NewR[1];
+  pose[2]=NewR[2];
+  pose[3]=NewT[0];
+  pose[4]=NewR[3];
+  pose[5]=NewR[4];
+  pose[6]=NewR[5];
+  pose[7]=NewT[1];
+  pose[8]=NewR[6];
+  pose[9]=NewR[7];
+  pose[10]=NewR[8];
+  pose[11]=NewT[2];
+  pose[12]=0;
+  pose[13]=0;
+  pose[14]=0;
+  pose[15]=1;
 
   // compute the angle
   const double trace = NewR[0] + NewR[4] + NewR[8];
@@ -137,22 +137,22 @@ void Pose3D::updatePoseQuat(double Q[4], double NewT[3])
   q[2]=Q[2];
   q[3]=Q[3];
 
-  Pose[0]=NewR[0];
-  Pose[1]=NewR[1];
-  Pose[2]=NewR[2];
-  Pose[3]=NewT[0];
-  Pose[4]=NewR[3];
-  Pose[5]=NewR[4];
-  Pose[6]=NewR[5];
-  Pose[7]=NewT[1];
-  Pose[8]=NewR[6];
-  Pose[9]=NewR[7];
-  Pose[10]=NewR[8];
-  Pose[11]=NewT[2];
-  Pose[12]=0;
-  Pose[13]=0;
-  Pose[14]=0;
-  Pose[15]=1;
+  pose[0]=NewR[0];
+  pose[1]=NewR[1];
+  pose[2]=NewR[2];
+  pose[3]=NewT[0];
+  pose[4]=NewR[3];
+  pose[5]=NewR[4];
+  pose[6]=NewR[5];
+  pose[7]=NewT[1];
+  pose[8]=NewR[6];
+  pose[9]=NewR[7];
+  pose[10]=NewR[8];
+  pose[11]=NewT[2];
+  pose[12]=0;
+  pose[13]=0;
+  pose[14]=0;
+  pose[15]=1;
 
   // compute the angle
   const double trace = NewR[0] + NewR[4] + NewR[8];
@@ -179,7 +179,7 @@ void Pose3D::appendPose(double IncrementalPose[16])
 {
   double R[9], PoseFull[16]={0};
 
-  matrixProduct44(IncrementalPose, this->Pose, PoseFull);
+  matrixProduct44(IncrementalPose, this->pose, PoseFull);
 
   R[0] = PoseFull[0];
   R[1] = PoseFull[1];
@@ -216,27 +216,27 @@ void Pose3D::appendPose(double IncrementalPose[16])
   dcmToQuat(R, q);
 
   for (int i=0; i<16; i++)
-    Pose[i]=PoseFull[i];
+    pose[i]=PoseFull[i];
 }
 
 Pose3D* Pose3D::clone()
 {
-  Pose3D* pose = new Pose3D(alpha, modelIndex, numVotes);
+  Pose3D* new_pose = new Pose3D(alpha, modelIndex, numVotes);
   for (int i=0; i<16; i++)
-    pose->Pose[i]= Pose[i];
+    new_pose->pose[i]= this->pose[i];
 
-  pose->q[0]=q[0];
-  pose->q[1]=q[1];
-  pose->q[2]=q[2];
-  pose->q[3]=q[3];
+  new_pose->q[0]=q[0];
+  new_pose->q[1]=q[1];
+  new_pose->q[2]=q[2];
+  new_pose->q[3]=q[3];
 
-  pose->t[0]=t[0];
-  pose->t[1]=t[1];
-  pose->t[2]=t[2];
+  new_pose->t[0]=t[0];
+  new_pose->t[1]=t[1];
+  new_pose->t[2]=t[2];
 
-  pose->angle=angle;
+  new_pose->angle=angle;
 
-  return pose;
+  return new_pose;
 }
 
 void Pose3D::printPose()
@@ -246,7 +246,7 @@ void Pose3D::printPose()
   {
     for (int k=0; k<4; k++)
     {
-      printf("%f ", this->Pose[j*4+k]);
+      printf("%f ", this->pose[j*4+k]);
     }
     printf("\n");
   }
@@ -260,7 +260,7 @@ int Pose3D::writePose(FILE* f)
   fwrite(&angle, sizeof(double), 1, f);
   fwrite(&numVotes, sizeof(int), 1, f);
   fwrite(&modelIndex, sizeof(int), 1, f);
-  fwrite(Pose, sizeof(double)*16, 1, f);
+  fwrite(pose, sizeof(double)*16, 1, f);
   fwrite(t, sizeof(double)*3, 1, f);
   fwrite(q, sizeof(double)*4, 1, f);
   fwrite(&residual, sizeof(double), 1, f);
@@ -277,7 +277,7 @@ int Pose3D::readPose(FILE* f)
     status = fread(&angle, sizeof(double), 1, f);
     status = fread(&numVotes, sizeof(int), 1, f);
     status = fread(&modelIndex, sizeof(int), 1, f);
-    status = fread(Pose, sizeof(double)*16, 1, f);
+    status = fread(pose, sizeof(double)*16, 1, f);
     status = fread(t, sizeof(double)*3, 1, f);
     status = fread(q, sizeof(double)*4, 1, f);
     status = fread(&residual, sizeof(double), 1, f);

modules/surface_matching/src/ppf_match_3d.cpp

@@ -45,6 +45,9 @@ namespace cv
 {
 namespace ppf_match_3d
 {
+
+static const size_t PPF_LENGTH = 5;
+
 // routines for assisting sort
 static int qsortPoseCmp(const void * a, const void * b)
 {
@@ -118,12 +121,12 @@ static double computeAlpha(const double p1[4], const double n1[4], const double
 
 PPF3DDetector::PPF3DDetector()
 {
-  samplingStepRelative = 0.05;
-  distanceStepRelative = 0.05;
-  SceneSampleStep = (int)(1/0.04);
-  angleStepRelative = 30;
-  angleStepRadians = (360.0/angleStepRelative)*M_PI/180.0;
-  angle_step = angleStepRadians;
+  sampling_step_relative = 0.05;
+  distance_step_relative = 0.05;
+  scene_sample_step = (int)(1/0.04);
+  angle_step_relative = 30;
+  angle_step_radians = (360.0/angle_step_relative)*M_PI/180.0;
+  angle_step = angle_step_radians;
   trained = false;
 
   setSearchParams();
@@ -131,33 +134,30 @@ PPF3DDetector::PPF3DDetector()
 
 PPF3DDetector::PPF3DDetector(const double RelativeSamplingStep, const double RelativeDistanceStep, const double NumAngles)
 {
-  samplingStepRelative = RelativeSamplingStep;
-  distanceStepRelative = RelativeDistanceStep;
-  angleStepRelative = NumAngles;
-  angleStepRadians = (360.0/angleStepRelative)*M_PI/180.0;
+  sampling_step_relative = RelativeSamplingStep;
+  distance_step_relative = RelativeDistanceStep;
+  angle_step_relative = NumAngles;
+  angle_step_radians = (360.0/angle_step_relative)*M_PI/180.0;
   //SceneSampleStep = 1.0/RelativeSceneSampleStep;
-  angle_step = angleStepRadians;
+  angle_step = angle_step_radians;
   trained = false;
 
   setSearchParams();
 }
 
-void PPF3DDetector::setSearchParams(const int numPoses, const double positionThreshold, const double rotationThreshold, const double minMatchScore, const bool useWeightedClustering)
+void PPF3DDetector::setSearchParams(const double positionThreshold, const double rotationThreshold, const bool useWeightedClustering)
 {
-  NumPoses=numPoses;
-
   if (positionThreshold<0)
-    PositionThreshold = samplingStepRelative;
+    position_threshold = sampling_step_relative;
   else
-    PositionThreshold = positionThreshold;
+    position_threshold = positionThreshold;
 
   if (rotationThreshold<0)
-    RotationThreshold = ((360/angle_step) / 180.0 * M_PI);
+    rotation_threshold = ((360/angle_step) / 180.0 * M_PI);
   else
-    RotationThreshold = rotationThreshold;
+    rotation_threshold = rotationThreshold;
 
-  UseWeightedAvg = useWeightedClustering;
-  MinMatchScore = minMatchScore;
+  use_weighted_avg = useWeightedClustering;
 }
 
 // compute per point PPF as in paper
@@ -242,17 +242,17 @@ void PPF3DDetector::trainModel(const Mat &PC)
   float dz = zRange[1] - zRange[0];
   float diameter = sqrt ( dx * dx + dy * dy + dz * dz );
 
-  float distanceStep = (float)(diameter * samplingStepRelative);
+  float distanceStep = (float)(diameter * sampling_step_relative);
 
-  Mat sampled = samplePCByQuantization(PC, xRange, yRange, zRange, (float)samplingStepRelative,0);
+  Mat sampled = samplePCByQuantization(PC, xRange, yRange, zRange, (float)sampling_step_relative,0);
 
   int size = sampled.rows*sampled.rows;
 
   hashtable_int* hashTable = hashtableCreate(size, NULL);
 
   int numPPF = sampled.rows*sampled.rows;
-  PPF = Mat(numPPF, T_PPF_LENGTH, CV_32FC1);
-  int ppfStep = (int)PPF.step;
+  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.
@@ -284,7 +284,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
 
         double f[4]={0};
         computePPFFeatures(p1, n1, p2, n2, f);
-        KeyType hashValue = hashPPF(f, angleStepRadians, distanceStep);
+        KeyType hashValue = hashPPF(f, angle_step_radians, distanceStep);
         double alpha = computeAlpha(p1, n1, p2);
         unsigned int corrInd = i*numRefPoints+j;
         unsigned int ppfInd = corrInd*ppfStep;
@@ -296,7 +296,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
 
         hashtableInsertHashed(hashTable, hashValue, (void*)hashNode);
 
-        float* ppfRow = (float*)(&(PPF.data[ ppfInd ]));
+        float* ppfRow = (float*)(&(ppf.data[ ppfInd ]));
         ppfRow[0] = (float)f[0];
         ppfRow[1] = (float)f[1];
         ppfRow[2] = (float)f[2];
@@ -306,13 +306,12 @@ void PPF3DDetector::trainModel(const Mat &PC)
     }
   }
 
-  angle_step = angleStepRadians;
+  angle_step = angle_step_radians;
   distance_step = distanceStep;
   hash_table = hashTable;
-  sampled_step = sampledStep;
   ppf_step = ppfStep;
   num_ref_points = numRefPoints;
-  sampledPC = sampled;
+  sampled_pc = sampled;
   trained = true;
 }
 
@@ -329,7 +328,7 @@ bool PPF3DDetector::matchPose(const Pose3D& sourcePose, const Pose3D& targetPose
 
   const double phi = fabs ( sourcePose.angle - targetPose.angle );
 
-  return (phi<this->RotationThreshold && dNorm < this->PositionThreshold);
+  return (phi<this->rotation_threshold && dNorm < this->position_threshold);
 }
 
 int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pose3D*>& finalPoses)
@@ -371,7 +370,7 @@ int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pos
 
   // TODO: Use MinMatchScore
 
-  if (UseWeightedAvg)
+  if (use_weighted_avg)
   {
 #if defined _OPENMP
 #pragma omp parallel for
@@ -486,14 +485,14 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
   CV_Assert(pc.type() == CV_32F || pc.type() == CV_32FC1);
   CV_Assert(relativeSceneSampleStep<=1 && relativeSceneSampleStep>0);
 
-  SceneSampleStep = (int)(1.0/relativeSceneSampleStep);
+  scene_sample_step = (int)(1.0/relativeSceneSampleStep);
 
   //int numNeighbors = 10;
   int numAngles = (int) (floor (2 * M_PI / angle_step));
   float distanceStep = (float)distance_step;
   unsigned int n = num_ref_points;
   Pose3D** poseList;
-  int sceneSamplingStep = SceneSampleStep;
+  int sceneSamplingStep = scene_sample_step;
 
   // compute bbox
   float xRange[2], yRange[2], zRange[2];
@@ -574,8 +573,8 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
           THash* tData = (THash*) node->data;
           int corrI = (int)tData->i;
           int ppfInd = (int)tData->ppfInd;
-          float* ppfCorrScene = (float*)(&PPF.data[ppfInd]);
-          double alpha_model = (double)ppfCorrScene[T_PPF_LENGTH-1];
+          float* ppfCorrScene = (float*)(&ppf.data[ppfInd]);
+          double alpha_model = (double)ppfCorrScene[PPF_LENGTH-1];
           double alpha = alpha_model - alpha_scene;
 
           /*  Tolga Birdal's note: Map alpha to the indices:
@@ -629,7 +628,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
                         };
 
     // TODO : Compute pose
-    const float* fMax = (float*)(&sampledPC.data[refIndMax * sampledPC.step]);
+    const float* fMax = (float*)(&sampled_pc.data[refIndMax * sampled_pc.step]);
     const double pMax[4] = {fMax[0], fMax[1], fMax[2], 1};
     const double nMax[4] = {fMax[3], fMax[4], fMax[5], 1};
 
@@ -656,11 +655,9 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
     matrixProduct44(Talpha, Tmg, Temp);
     matrixProduct44(TsgInv, Temp, Pose);
 
-    Pose3D *ppf = new Pose3D(alpha, refIndMax, maxVotes);
-
-    ppf->updatePose(Pose);
-
-    poseList[i/sceneSamplingStep] = ppf;
+    Pose3D *pose = new Pose3D(alpha, refIndMax, maxVotes);
+    pose->updatePose(Pose);
+    poseList[i/sceneSamplingStep] = pose;
 
 #if defined (_OPENMP)
     free(accumulator);