static void randomFat(const vector<cv::Mat> &input, cv::Mat &fat)
{
cv::Mat_<int> randIdx(1, input.size());
randomIndex(randIdx);
for (size_t i=0; i<input.size(); i++)
{
int idx = randIdx(i);
fat.push_back(input[idx].reshape(1,1));
}
}
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));
}
}
