UNFINISHED: get rid of pointers

modules/surface_matching/doc/surface_matching.rst

@@ -138,13 +138,13 @@ Source Code for PPF Matching
 	// the scene (Mx6)
 	ppf_match_3d::PPF3DDetector detector(0.03, 0.05);
 	detector.trainModel(pc);
-	vector < Pose3D* > results;
+	vector<Pose3DPtr> results;
 	detector.match(pcTest, results, 1.0/10.0, 0.05);
 	cout << "Poses: " << endl;
 	// print the poses
 	for (size_t i=0; i<results.size(); i++)
 	{
-	    Pose3D* pose = results[i];
+	    Pose3DPtr pose = results[i];
 	    cout << "Pose Result " << i << endl;
 	    pose->printPose();
 	}

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

@@ -127,6 +127,7 @@ public:
      *  CV_32F is the only supported data type.
      *  @param [in] dstPC The input point cloud for the scene. It is assumed that the model is registered on the scene. Scene remains static. Expected to have the normals (Nx6). Currently, CV_32F is the only supported data type.
      *  @param [out] residual The output registration error.
+     *  @param [out] pose Transformation between srcPC and dstPC.
      *  \return On successful termination, the function returns 0.
      *
      *  \details It is assumed that the model is registered on the scene. Scene remains static, while the model transforms. The output poses transform the models onto the scene. Because of the point to plane minimization, the scene is expected to have the normals available. Expected to have the normals (Nx6).
@@ -139,12 +140,12 @@ public:
      *  @param [in] srcPC The input point cloud for the model. Expected to have the normals (Nx6). Currently,
      *  CV_32F is the only supported data type.
      *  @param [in] dstPC The input point cloud for the scene. Currently, CV_32F is the only supported data type.
-     *  @param [out] poses List output of poses. For more detailed information check out Pose3D.
+     *  @param [in,out] poses Input poses to start with but also list output of poses.
      *  \return On successful termination, the function returns 0.
      *
      *  \details It is assumed that the model is registered on the scene. Scene remains static, while the model transforms. The output poses transform the models onto the scene. Because of the point to plane minimization, the scene is expected to have the normals available. Expected to have the normals (Nx6).
      */
-  int registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3D*>& poses);
+  int registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3DPtr>& poses);
 
 private:
   float m_tolerance;

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

@@ -41,6 +41,7 @@
 #ifndef __OPENCV_SURFACE_MATCHING_POSE3D_HPP__
 #define __OPENCV_SURFACE_MATCHING_POSE3D_HPP__
 
+#include "opencv2/core/cvstd.hpp" // cv::Ptr
 #include <vector>
 #include <string>
 
@@ -48,6 +49,13 @@ namespace cv
 {
 namespace ppf_match_3d
 {
+
+class Pose3D;
+typedef Ptr<Pose3D> Pose3DPtr;
+
+class PoseCluster3D;
+typedef Ptr<PoseCluster3D> PoseCluster3DPtr;
+
 /**
 * @class Pose3D
 * @brief Class, allowing the storage of a pose. The data structure stores both
@@ -105,7 +113,7 @@ public:
   void appendPose(double IncrementalPose[16]);
   void printPose();
 
-  Pose3D* clone();
+  Pose3DPtr clone();
 
   int writePose(FILE* f);
   int readPose(FILE* f);
@@ -135,7 +143,7 @@ public:
     id=0;
   }
 
-  PoseCluster3D(Pose3D* newPose)
+  PoseCluster3D(Pose3DPtr newPose)
   {
     poseList.clear();
     poseList.push_back(newPose);
@@ -143,7 +151,7 @@ public:
     id=0;
   }
 
-  PoseCluster3D(Pose3D* newPose, int newId)
+  PoseCluster3D(Pose3DPtr newPose, int newId)
   {
     poseList.push_back(newPose);
     this->numVotes = newPose->numVotes;
@@ -158,18 +166,19 @@ public:
    *  in order to preserve the consistency
    *  \param [in] newPose Pose to add to the cluster
    */
-  void addPose(Pose3D* newPose);
+  void addPose(Pose3DPtr newPose);
 
   int writePoseCluster(FILE* f);
   int readPoseCluster(FILE* f);
   int writePoseCluster(const std::string& FileName);
   int readPoseCluster(const std::string& FileName);
 
-  std::vector < Pose3D* > poseList;
+  std::vector<Pose3DPtr> poseList;
   int numVotes;
   int id;
 };
 
+
 } // namespace ppf_match_3d
 } // namespace cv
 

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

@@ -86,7 +86,7 @@ typedef struct THash
   *   ppf_match_3d::PPF3DDetector detector(0.05, 0.05);
   * detector.trainModel(pc);
   * // Search the model in a given scene
-  *   vector < Pose3D* > results;
+  *   vector<Pose3DPtr> results;
   * detector.match(pcTest, results, 1.0/5.0,0.05);
   *
   */
@@ -134,7 +134,7 @@ public:
     *  @param [in] relativeSceneSampleStep The ratio of scene points to be used for the matching after sampling with relativeSceneDistance. For example, if this value is set to 1.0/5.0, every 5th point from the scene is used for pose estimation. This parameter allows an easy trade-off between speed and accuracy of the matching. Increasing the value leads to less points being used and in turn to a faster but less accurate pose computation. Decreasing the value has the inverse effect.
     *  @param [in] relativeSceneDistance Set the distance threshold relative to the diameter of the model. This parameter is equivalent to relativeSamplingStep in the training stage. This parameter acts like a prior sampling with the relativeSceneSampleStep parameter.
     */
-  void match(const Mat& scene, std::vector<Pose3D*>& results, const double relativeSceneSampleStep=1.0/5.0, const double relativeSceneDistance=0.03);
+  void match(const Mat& scene, std::vector<Pose3DPtr> &results, const double relativeSceneSampleStep=1.0/5.0, const double relativeSceneDistance=0.03);
 
   void read(const FileNode& fn);
   void write(FileStorage& fs) const;
@@ -162,7 +162,7 @@ private:
 
   bool matchPose(const Pose3D& sourcePose, const Pose3D& targetPose);
 
-  int clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pose3D*>& finalPoses);
+  void clusterPoses(std::vector<Pose3DPtr> poseList, int numPoses, std::vector<Pose3DPtr> &finalPoses);
 
   bool trained;
 };

modules/surface_matching/samples/ppf_load_match.cpp

@@ -104,7 +104,7 @@ int main(int argc, char** argv)
     
     // Match the model to the scene and get the pose
     cout << endl << "Starting matching..." << endl;
-    vector < Pose3D* > results;
+    vector<Pose3DPtr> results;
     tick1 = cv::getTickCount();
     detector.match(pcTest, results, 1.0/40.0, 0.05);
     tick2 = cv::getTickCount();
@@ -113,9 +113,7 @@ int main(int argc, char** argv)
          
     // Get only first N results
     int N = 2;
-    vector<Pose3D*>::const_iterator first = results.begin();
-    vector<Pose3D*>::const_iterator last = results.begin() + N;
-    vector<Pose3D*> resultsSub(first, last);
+    vector<Pose3DPtr> resultsSub(results.begin(),results.begin()+N);
     
     // Create an instance of ICP
     ICP icp(100, 0.005f, 2.5f, 8);
@@ -133,7 +131,7 @@ int main(int argc, char** argv)
     // debug first five poses
     for (size_t i=0; i<resultsSub.size(); i++)
     {
-        Pose3D* result = resultsSub[i];
+        Pose3DPtr result = resultsSub[i];
         cout << "Pose Result " << i << endl;
         result->printPose();
         if (i==0)

modules/surface_matching/src/icp.cpp

@@ -293,12 +293,11 @@ static hashtable_int* getHashtable(int* data, size_t length, int numMaxElement)
 }
 
 // source point clouds are assumed to contain their normals
-int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& Residual, double Pose[16])
+int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residual, double pose[16])
 {
   int n = srcPC.rows;
-  //double PoseInit[16];
 
-  bool UseRobustReject = m_rejectionScale>0;
+  const bool useRobustReject = m_rejectionScale>0;
 
   Mat srcTemp = srcPC.clone();
   Mat dstTemp = dstPC.clone();
@@ -314,8 +313,6 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& Residu
 
   double scale = (double)n / ((distSrc + distDst)*0.5);
 
-  //srcTemp(cv::Range(0, srcTemp.rows), cv::Range(0,3)) = (srcTemp(cv::Range(0, srcTemp.rows), cv::Range(0,3)) )  * scale;
-  //dstTemp(cv::Range(0, dstTemp.rows), cv::Range(0,3)) = (dstTemp(cv::Range(0, dstTemp.rows), cv::Range(0,3)) )  * scale;
   srcTemp(cv::Range(0, srcTemp.rows), cv::Range(0,3)) *= scale;
   dstTemp(cv::Range(0, dstTemp.rows), cv::Range(0,3)) *= scale;
 
@@ -323,7 +320,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& Residu
   Mat dstPC0 = dstTemp;
 
   // initialize pose
-  matrixIdentity(4, Pose);
+  matrixIdentity(4, pose);
 
   void* flann = indexPCFlann(dstPC0);
   Mat M = Mat::eye(4,4,CV_64F);
@@ -342,7 +339,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& Residu
     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);
+    Mat srcPCT = transformPCPose(srcPC0, pose);
 
     const int sampleStep = cvRound((double)n/(double)numSamples);
     std::vector<int> srcSampleInd;
@@ -395,7 +392,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& Residu
         newJ[di] = indices[di];
       }
 
-      if (UseRobustReject)
+      if (useRobustReject)
       {
         int numInliers = 0;
         float threshold = getRejectionThreshold(distances, Distances.rows, m_rejectionScale);
@@ -503,13 +500,13 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& Residu
     }
 
     double TempPose[16];
-    matrixProduct44(PoseX, Pose, TempPose);
+    matrixProduct44(PoseX, pose, TempPose);
 
     // no need to copy the last 4 rows
     for (int c=0; c<12; c++)
-      Pose[c] = TempPose[c];
+      pose[c] = TempPose[c];
 
-    Residual = tempResidual;
+    residual = tempResidual;
 
     delete[] newI;
     delete[] newJ;
@@ -522,27 +519,26 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& Residu
   }
 
   // Pose(1:3, 4) = Pose(1:3, 4)./scale;
-  Pose[3] = Pose[3]/scale + meanAvg[0];
-  Pose[7] = Pose[7]/scale + meanAvg[1];
-  Pose[11] = Pose[11]/scale + meanAvg[2];
+  pose[3] = pose[3]/scale + meanAvg[0];
+  pose[7] = pose[7]/scale + meanAvg[1];
+  pose[11] = pose[11]/scale + meanAvg[2];
 
   // In MATLAB this would be : Pose(1:3, 4) = Pose(1:3, 4)./scale + meanAvg' - Pose(1:3, 1:3)*meanAvg';
   double Rpose[9], Cpose[3];
-  poseToR(Pose, Rpose);
+  poseToR(pose, Rpose);
   matrixProduct331(Rpose, meanAvg, Cpose);
-  Pose[3] -= Cpose[0];
-  Pose[7] -= Cpose[1];
-  Pose[11] -= Cpose[2];
+  pose[3] -= Cpose[0];
+  pose[7] -= Cpose[1];
+  pose[11] -= Cpose[2];
 
-  Residual = tempResidual;
+  residual = tempResidual;
 
   destroyFlann(flann);
-  flann = 0;
   return 0;
 }
 
 // source point clouds are assumed to contain their normals
-int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3D*>& poses)
+int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3DPtr>& poses)
 {
   for (size_t i=0; i<poses.size(); i++)
   {
@@ -551,7 +547,6 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Po
     registerModelToScene(srcTemp, dstPC, poses[i]->residual, poseICP);
     poses[i]->appendPose(poseICP);
   }
-
   return 0;
 }
 

modules/surface_matching/src/pose_3d.cpp

@@ -219,9 +219,9 @@ void Pose3D::appendPose(double IncrementalPose[16])
     pose[i]=PoseFull[i];
 }
 
-Pose3D* Pose3D::clone()
+Pose3DPtr Pose3D::clone()
 {
-  Pose3D* new_pose = new Pose3D(alpha, modelIndex, numVotes);
+  Ptr<Pose3D> new_pose(new Pose3D(alpha, modelIndex, numVotes));
   for (int i=0; i<16; i++)
     new_pose->pose[i]= this->pose[i];
 
@@ -314,7 +314,7 @@ int Pose3D::readPose(const std::string& FileName)
 }
 
 
-void PoseCluster3D::addPose(Pose3D* newPose)
+void PoseCluster3D::addPose(Pose3DPtr newPose)
 {
   poseList.push_back(newPose);
   this->numVotes += newPose->numVotes;
@@ -356,7 +356,7 @@ int PoseCluster3D::readPoseCluster(FILE* f)
   poseList.resize(numPoses);
   for (size_t i=0; i<poseList.size(); i++)
   {
-    poseList[i] = new Pose3D();
+    poseList[i] = Pose3DPtr(new Pose3D());
     poseList[i]->readPose(f);
   }
 

modules/surface_matching/src/ppf_match_3d.cpp

@@ -49,15 +49,15 @@ namespace ppf_match_3d
 static const size_t PPF_LENGTH = 5;
 
 // routines for assisting sort
-static int qsortPoseCmp(const void * a, const void * b)
+static bool pose3DPtrCompare(const Pose3DPtr& a, const Pose3DPtr& b)
 {
-  Pose3D* pose1 = *(Pose3D**)a;
-  Pose3D* pose2 = *(Pose3D**)b;
-  return ( pose2->numVotes - pose1->numVotes );
+  CV_Assert(!a.empty() && !b.empty());
+  return ( a->numVotes > b->numVotes );
 }
 
-static int sortPoseClusters(const PoseCluster3D* a, const PoseCluster3D* b)
+static int sortPoseClusters(const PoseCluster3DPtr& a, const PoseCluster3DPtr& b)
 {
+  CV_Assert(!a.empty() && !b.empty());
   return ( a->numVotes > b->numVotes );
 }
 
@@ -331,25 +331,24 @@ bool PPF3DDetector::matchPose(const Pose3D& sourcePose, const Pose3D& targetPose
   return (phi<this->rotation_threshold && dNorm < this->position_threshold);
 }
 
-int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pose3D*>& finalPoses)
+void PPF3DDetector::clusterPoses(std::vector<Pose3DPtr> poseList, int numPoses, std::vector<Pose3DPtr> &finalPoses)
 {
-  std::vector<PoseCluster3D*> poseClusters;
-  poseClusters.clear();
+  std::vector<PoseCluster3DPtr> poseClusters;
 
   finalPoses.clear();
 
   // sort the poses for stability
-  qsort(poseList, numPoses, sizeof(Pose3D*), qsortPoseCmp);
+  std::sort(poseList.begin(), poseList.end(), pose3DPtrCompare);
 
   for (int i=0; i<numPoses; i++)
   {
-    Pose3D* pose = poseList[i];
+    Pose3DPtr pose = poseList[i];
     bool assigned = false;
 
     // search all clusters
     for (size_t j=0; j<poseClusters.size() && !assigned; j++)
     {
-      const Pose3D* poseCenter = poseClusters[j]->poseList[0];
+      const Pose3DPtr poseCenter = poseClusters[j]->poseList[0];
       if (matchPose(*pose, *poseCenter))
       {
         poseClusters[j]->addPose(pose);
@@ -359,12 +358,12 @@ int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pos
 
     if (!assigned)
     {
-      poseClusters.push_back ( new PoseCluster3D(pose));
+      poseClusters.push_back(PoseCluster3DPtr(new PoseCluster3D(pose)));
     }
   }
 
   // sort the clusters so that we could output multiple hypothesis
-  std::sort (poseClusters.begin(), poseClusters.end(), sortPoseClusters);
+  std::sort(poseClusters.begin(), poseClusters.end(), sortPoseClusters);
 
   finalPoses.resize(poseClusters.size());
 
@@ -382,8 +381,8 @@ int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pos
       double qAvg[4]={0}, tAvg[3]={0};
 
       // Perform the final averaging
-      PoseCluster3D* curCluster = poseClusters[i];
-      std::vector<Pose3D*> curPoses = curCluster->poseList;
+      PoseCluster3DPtr curCluster = poseClusters[i];
+      std::vector<Pose3DPtr> curPoses = curCluster->poseList;
       int curSize = (int)curPoses.size();
       int numTotalVotes = 0;
 
@@ -420,8 +419,6 @@ int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pos
       curPoses[0]->numVotes=curCluster->numVotes;
 
       finalPoses[i]=curPoses[0]->clone();
-
-      delete poseClusters[i];
     }
   }
   else
@@ -435,9 +432,9 @@ int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pos
       double qAvg[4]={0}, tAvg[3]={0};
 
       // Perform the final averaging
-      PoseCluster3D* curCluster = poseClusters[i];
-      std::vector<Pose3D*> curPoses = curCluster->poseList;
-      int curSize = (int)curPoses.size();
+      PoseCluster3DPtr curCluster = poseClusters[i];
+      std::vector<Pose3DPtr> curPoses = curCluster->poseList;
+      const int curSize = (int)curPoses.size();
 
       for (int j=0; j<curSize; j++)
       {
@@ -464,18 +461,13 @@ int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pos
       curPoses[0]->numVotes=curCluster->numVotes;
 
       finalPoses[i]=curPoses[0]->clone();
-
-      // we won't need this
-      delete poseClusters[i];
     }
   }
 
   poseClusters.clear();
-
-  return 0;
 }
 
-void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const double relativeSceneSampleStep, const double relativeSceneDistance)
+void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const double relativeSceneSampleStep, const double relativeSceneDistance)
 {
   if (!trained)
   {
@@ -491,7 +483,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
   int numAngles = (int) (floor (2 * M_PI / angle_step));
   float distanceStep = (float)distance_step;
   unsigned int n = num_ref_points;
-  Pose3D** poseList;
+  std::vector<Pose3DPtr> poseList;
   int sceneSamplingStep = scene_sample_step;
 
   // compute bbox
@@ -511,7 +503,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
      unsigned int* accumulator = (unsigned int*)calloc(numAngles*n, sizeof(unsigned int));
   #endif*/
 
-  poseList = (Pose3D**)calloc((sampled.rows/sceneSamplingStep)+4, sizeof(Pose3D*));
+  poseList.reserve((sampled.rows/sceneSamplingStep)+4);
 
 #if defined _OPENMP
 #pragma omp parallel for
@@ -651,13 +643,13 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
     getUnitXRotation_44(alpha, Talpha);
 
     double Temp[16]={0};
-    double Pose[16]={0};
+    double rawPose[16]={0};
     matrixProduct44(Talpha, Tmg, Temp);
-    matrixProduct44(TsgInv, Temp, Pose);
+    matrixProduct44(TsgInv, Temp, rawPose);
 
-    Pose3D *pose = new Pose3D(alpha, refIndMax, maxVotes);
-    pose->updatePose(Pose);
-    poseList[i/sceneSamplingStep] = pose;
+    Pose3DPtr pose(new Pose3D(alpha, refIndMax, maxVotes));
+    pose->updatePose(rawPose);
+    poseList.push_back(pose);
 
 #if defined (_OPENMP)
     free(accumulator);
@@ -670,18 +662,6 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
   int numPosesAdded = sampled.rows/sceneSamplingStep;
 
   clusterPoses(poseList, numPosesAdded, results);
-
-  // free up the used space
-  sampled.release();
-
-  for (int i=0; i<numPosesAdded; i++)
-  {
-    Pose3D* pose = poseList[i];
-    delete pose;
-    poseList[i]=0;
-  }
-
-  free(poseList);
 }
 
 } // namespace ppf_match_3d

modules/surface_matching/src/precomp.hpp

@@ -54,12 +54,13 @@ the use of this software, even if advised of the possibility of such damage.
 
 #include <fstream>
 #include <iostream>
+#include <algorithm>
 
 #if defined (_OPENMP)
 #include<omp.h>
 #endif
 
-#include <sstream>  // WTF bananas
+#include <sstream>  // flann dependency, needed in precomp now
 #include "opencv2/flann.hpp"
 
 #include "c_utils.hpp"