int getMin(int *A, int len){ if(A==NULL) return 0; int maxIdx = getMaxIdx(A, len); int minIdx = (maxIdx + 1)%len; return A[minIdx]; }
void PointCloudProcessing::register2ScenesRansac( const PointCloudC::Ptr sceneRef, const PointCloudC::Ptr sceneNew, const PointCloudC::Ptr corrRef, const PointCloudC::Ptr corrNew, const float inlrThd, const int sampleNb, const float inlrRate, const int maxIter, Eigen::Matrix4f &transMat, PointCloudC::Ptr cloudOut) { int corrSize = corrRef->points.size(); std::cout<<"corrSize: "<<corrSize<<"corrSize: "<<corrNew->points.size()<<std::endl; std::cout<<"inlrThd: "<<sampleNb*inlrRate<<std::endl; std::cout<<"maxIter: "<<maxIter<<std::endl; std::vector<int> inlrNbAll; std::vector<Eigen::Matrix4f> transMatAll; std::vector<std::vector<int> > inlrIdxAll; int iter = 0; std::time_t timeRnd; std::time(&timeRnd); while (iter<maxIter) { // std::cout<<"iter = "<<iter<<"\n\n"; int inlrNb = 0; // Random sampling std::set<int> sampleIdx; sampleIdx.clear(); randIdx(corrSize, sampleNb, timeRnd, sampleIdx); // 1. Transform point cloud and compute distance; std::vector<float> transDist; std::vector<int> inlrIdx; getTransform3dGeometric(corrRef, corrNew, sampleIdx, inlrThd,\ transMat, transDist, inlrIdx, inlrNb); // std::cout<<"inlrIdx size"<<inlrIdx.size()<<"\n"; // std::cout<<"get transform iter done...\n"; inlrNbAll.push_back(inlrNb); transMatAll.push_back(transMat); inlrIdxAll.push_back(inlrIdx); ++iter; // std::cout<<"inlrNb: "<<inlrNb<<"\t"; } // 3. choose the coef with the most inliers /// Ransac refinement int bestSmp = 0; getMaxIdx(inlrNbAll, bestSmp); // std::cout<<"best sample idx: "<<bestSmp<<"\n"; std::cout<<"best sample inliers number: "<<inlrNbAll.at(bestSmp)<<"\n"; std::cout<<"before ransac refinement: "<< transMatAll.at(bestSmp)<<"\n"; std::vector<int> bestInlrIdx = inlrIdxAll.at(bestSmp); if(bestInlrIdx.size()>0) { PointCloudC::Ptr smpRef(new PointCloudC), smpNew(new PointCloudC); for(int i=0; i<bestInlrIdx.size(); i++) { PointC pointRef = corrRef->points.at(bestInlrIdx.at(i)); smpRef->push_back(pointRef); PointC pointNew = corrNew->points.at(bestInlrIdx.at(i)); smpNew->push_back(pointNew); } getRTGeometricLinearSystem(smpRef, smpNew, transMat); std::cout<<"after ransac refinement: "<< transMat<<"\n"; } // transMat = transMatAll.at(bestSmp); // std::cout<<"transMat: "<<transMat<<std::endl; // Transform point cloud PointCloudC::Ptr sceneNewT(new PointCloudC); pcl::transformPointCloud(*sceneNew, *sceneNewT, transMat); // pcl::transformPointCloud(*cloudOut, *cloudOut, transMat); // pcl::copyPointCloud(*sceneRef,*cloudOut); for(int i=0; i<sceneRef->points.size();i++) { cloudOut->points.push_back(sceneRef->points.at(i)); } for(int i=0; i<sceneNewT->points.size();i++) { cloudOut->points.push_back(sceneNewT->points.at(i)); } std::cout<<"registered scene size: "<<cloudOut->points.size()<<std::endl; }
////////////////////////////////////////////////////////////// /// 3D registration from point correspondences with Ransac ////////////////////////////////////////////////////////////// void PointCloudProcessing::register2ScenesMEstimator( const PointCloudC::Ptr sceneRef, const PointCloudC::Ptr sceneNew, const PointCloudC::Ptr corrRef, const PointCloudC::Ptr corrNew, const float inlrThd, const float smpRate, const float inlrRate, const int maxIter, Eigen::Matrix4f &transMat, PointCloudC::Ptr cloudOut) { int corrSize = corrRef->points.size(); // std::cout<<"corrSize: "<<corrSize<<std::endl; int sampleNb = smpRate*corrSize; // std::cout<<"sampleNb: "<<sampleNb<<std::endl; // std::cout<<"inlrThd: "<<sampleNb*inlrRate<<std::endl; // std::cout<<"maxIter: "<<maxIter<<std::endl; std::vector<int> inlrNbAll; std::vector<Eigen::Matrix4f> transMatAll; int iter = 0; std::time_t timeRnd; std::time(&timeRnd); while (iter<maxIter) { // std::cout<<"iter = "<<iter<<"\n\n"; int inlrNb = 0; // Random sampling std::set<int> sampleIdx; sampleIdx.clear(); // std::cout<<"sampleIdx size: "<<sampleIdx.size()<<std::endl; randIdx(corrSize, sampleNb, timeRnd, sampleIdx); // 1. Transform point cloud and compute distance; std::vector<float> transDist; std::vector<int> inlrIdx; getTransformation3d(corrRef, corrNew, sampleIdx, inlrThd,\ transMat, transDist, inlrIdx, inlrNb); // std::cout<<"inlrIdx size"<<inlrIdx.size()<<"\n"; // std::cout<<"get transform iter done...\n"; // 2. count the inlrs, if more than thInlr if(inlrNb < sampleNb*inlrRate) { ++iter; continue; } // std::cout<<"inlrIdx size: "<<inlrIdx.size()<<"\n"; getTransformation3d(corrRef, corrNew, inlrIdx, \ transMat); inlrNbAll.push_back(inlrNb); transMatAll.push_back(transMat); ++iter; // std::cout<<"inlrNb: "<<inlrNb<<"\t"; } // 3. choose the coef with the most inliers int bestSmp = 0; getMaxIdx(inlrNbAll, bestSmp); // std::cout<<"best sample idx: "<<bestSmp<<"\n"; transMat = transMatAll.at(bestSmp); // std::cout<<"transMat: "<<transMat<<std::endl; // Transform point cloud pcl::transformPointCloud(*sceneNew, *sceneNew, transMat); pcl::copyPointCloud(*sceneRef,*cloudOut); for(int i=0; i<sceneNew->points.size();i++) { cloudOut->points.push_back(sceneNew->points.at(i)); } }