From a8a4524ddb023daa0ba8fef46f944f976d06c447 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 2 Sep 2014 18:03:51 +0200 Subject: [PATCH] UNFINISHED: get rid of pointers --- .../surface_matching/doc/surface_matching.rst | 4 +- .../include/opencv2/surface_matching/icp.hpp | 5 +- .../opencv2/surface_matching/pose_3d.hpp | 19 +++-- .../opencv2/surface_matching/ppf_match_3d.hpp | 6 +- .../samples/ppf_load_match.cpp | 8 +-- modules/surface_matching/src/icp.cpp | 39 +++++------ modules/surface_matching/src/pose_3d.cpp | 8 +-- modules/surface_matching/src/ppf_match_3d.cpp | 70 +++++++------------ modules/surface_matching/src/precomp.hpp | 3 +- 9 files changed, 73 insertions(+), 89 deletions(-) diff --git a/modules/surface_matching/doc/surface_matching.rst b/modules/surface_matching/doc/surface_matching.rst index ece516805..e026e0d7c 100644 --- a/modules/surface_matching/doc/surface_matching.rst +++ b/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 results; detector.match(pcTest, results, 1.0/10.0, 0.05); cout << "Poses: " << endl; // print the poses for (size_t i=0; iprintPose(); } diff --git a/modules/surface_matching/include/opencv2/surface_matching/icp.hpp b/modules/surface_matching/include/opencv2/surface_matching/icp.hpp index 253e986ed..134de42f2 100644 --- a/modules/surface_matching/include/opencv2/surface_matching/icp.hpp +++ b/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& poses); + int registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector& poses); private: float m_tolerance; diff --git a/modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp b/modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp index 0510a43ac..8c7fa76b9 100644 --- a/modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp +++ b/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 #include @@ -48,6 +49,13 @@ namespace cv { namespace ppf_match_3d { + +class Pose3D; +typedef Ptr Pose3DPtr; + +class PoseCluster3D; +typedef Ptr 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 poseList; int numVotes; int id; }; + } // namespace ppf_match_3d } // namespace cv diff --git a/modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp b/modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp index 2eaaafcba..31c65fce7 100644 --- a/modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp +++ b/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 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& results, const double relativeSceneSampleStep=1.0/5.0, const double relativeSceneDistance=0.03); + void match(const Mat& scene, std::vector &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& finalPoses); + void clusterPoses(std::vector poseList, int numPoses, std::vector &finalPoses); bool trained; }; diff --git a/modules/surface_matching/samples/ppf_load_match.cpp b/modules/surface_matching/samples/ppf_load_match.cpp index ada87085b..303e8080b 100644 --- a/modules/surface_matching/samples/ppf_load_match.cpp +++ b/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 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::const_iterator first = results.begin(); - vector::const_iterator last = results.begin() + N; - vector resultsSub(first, last); + vector 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; iprintPose(); if (i==0) diff --git a/modules/surface_matching/src/icp.cpp b/modules/surface_matching/src/icp.cpp index 62f98ffd8..8614ed521 100644 --- a/modules/surface_matching/src/icp.cpp +++ b/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 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& poses) +int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector& poses) { for (size_t i=0; iresidual, poseICP); poses[i]->appendPose(poseICP); } - return 0; } diff --git a/modules/surface_matching/src/pose_3d.cpp b/modules/surface_matching/src/pose_3d.cpp index 48a8dfd58..808cb9033 100644 --- a/modules/surface_matching/src/pose_3d.cpp +++ b/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 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; ireadPose(f); } diff --git a/modules/surface_matching/src/ppf_match_3d.cpp b/modules/surface_matching/src/ppf_match_3d.cpp index c8847b529..7c468481f 100644 --- a/modules/surface_matching/src/ppf_match_3d.cpp +++ b/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 (phirotation_threshold && dNorm < this->position_threshold); } -int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector& finalPoses) +void PPF3DDetector::clusterPoses(std::vector poseList, int numPoses, std::vector &finalPoses) { - std::vector poseClusters; - poseClusters.clear(); + std::vector 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; iposeList[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 curPoses = curCluster->poseList; + PoseCluster3DPtr curCluster = poseClusters[i]; + std::vector curPoses = curCluster->poseList; int curSize = (int)curPoses.size(); int numTotalVotes = 0; @@ -420,8 +419,6 @@ int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vectornumVotes=curCluster->numVotes; finalPoses[i]=curPoses[0]->clone(); - - delete poseClusters[i]; } } else @@ -435,9 +432,9 @@ int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector curPoses = curCluster->poseList; - int curSize = (int)curPoses.size(); + PoseCluster3DPtr curCluster = poseClusters[i]; + std::vector curPoses = curCluster->poseList; + const int curSize = (int)curPoses.size(); for (int j=0; jnumVotes=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& results, const double relativeSceneSampleStep, const double relativeSceneDistance) +void PPF3DDetector::match(const Mat& pc, std::vector& results, const double relativeSceneSampleStep, const double relativeSceneDistance) { if (!trained) { @@ -491,7 +483,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector& 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 poseList; int sceneSamplingStep = scene_sample_step; // compute bbox @@ -511,7 +503,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector& 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& 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& results, const do int numPosesAdded = sampled.rows/sceneSamplingStep; clusterPoses(poseList, numPosesAdded, results); - - // free up the used space - sampled.release(); - - for (int i=0; i #include +#include #if defined (_OPENMP) #include #endif -#include // WTF bananas +#include // flann dependency, needed in precomp now #include "opencv2/flann.hpp" #include "c_utils.hpp"