double Delaunay::interpolateAttr(const Point2f &pt, int tri_id){ MatrixXd A(3,3); Vector3d yv_mapped(-1,-1,-1); VectorXd coeff; triangle tri=triangulation[tri_id]; //create matrix for (size_t i=0;i<3;++i){ A(i,0)=tri.vtx[i].pt.x; A(i,1)=tri.vtx[i].pt.y; A(i,2)=tri.vtx[i].attr; } //solve for linear least squares fit ColPivHouseholderQR<MatrixXd> qr_decomp(A); auto rank_A=qr_decomp.rank(); MatrixXd B(A.rows(),yv_mapped.cols()+A.cols()); B<<A,yv_mapped; qr_decomp.compute(B); auto rank_B=qr_decomp.rank(); double result; if(rank_A==rank_B && rank_A==A.cols()){ coeff=A.householderQr().solve(yv_mapped); result=(-1.0-coeff[0]*pt.x-coeff[1]*pt.y)/coeff[2]; } else if(A(0,2)==A(1,2) && A(1,2)==A(2,2)) result=A(0,2); else exitwithErrors("Error occured while predicting the disparity!"); return result; }
bool extractFeatures(char *type){ Ptr<FeatureDetector> detector; vector<KeyPoint> keypoints; string ImageID="left",outPath=directory+"keypoints.txt"; bool display=false; bool saveKpts=false; //read common key variables readConfigFile(filename,"ImageID",ImageID); readConfigFile(filename,"DisplayKeypoints",display); readConfigFile(filename,"SaveKeypoints",saveKpts); readConfigFile(filename,"SaveKeypointsPath",outPath); lowerString(ImageID); //set detector switch(type[0]) { case '1': {//Good feature to track cout<<"Feature Extraction:\tGood feature to track..."<<endl<<endl; /*GoodFeaturesToTrackDetector( int maxCorners, double qualityLevel, double minDistance, int blockSize=3,bool useHarrisDetector=false, double k=0.04 );*/ //initialize the parameters int maxCorners=1e8,blockSize=5; double qualityLevel=0.01,minDistance=5,k=0.04; bool useHarrisDEtector=false; //set parameters readConfigFile(filename,"maxCorners",maxCorners); readConfigFile(filename,"blockSize",blockSize); readConfigFile(filename,"qualityLevel",qualityLevel); readConfigFile(filename,"minDistance",minDistance); readConfigFile(filename,"k",k); readConfigFile(filename,"useHarrisDetector",useHarrisDEtector); //initialize feature detector detector=new GoodFeaturesToTrackDetector(maxCorners,qualityLevel,minDistance,blockSize,useHarrisDEtector,k); //detect if(ImageID=="left") detector->detect(img1,keypoints); else if(ImageID=="right") detector->detect(img2,keypoints); else{ detector->detect(img1,keypoints); cerr<<"Unknown option for the image_id, the left image was detected instead!"<<endl; } break; } case '2': {//Fast feature to track cout<<"Feature Extraction:\tFast feature to track..."<<endl<<endl; /* FastFeatureDetector(int threshold=10,bool nonmaxSuppression=true)*/ //initialize the parameters int threshold=1; bool nonmaxSuppression=true; //set parameters readConfigFile(filename,"threshold",threshold); readConfigFile(filename,"nonmaxSuppression",nonmaxSuppression); //initialize feature detector detector=new FastFeatureDetector(threshold, nonmaxSuppression); //detect if(ImageID=="left") detector->detect(img1,keypoints); else if(ImageID=="right") detector->detect(img2,keypoints); else{ detector->detect(img1,keypoints); cerr<<"Unknown option for the image_id, the left image was detected instead!"<<endl; } break; } case '3': {//Grid feature to track cout<<"Feature Extraction:\tGrid feature to track..."<<endl<<endl; /*DenseFeatureDetector(float initFeatureScale=1.f,int featureScaleLevels=1,float featureScaleMul=0.1f, int initXyStep=6,int initImgBound=0,bool varyXyStepWithScale=true,bool varyImgBoundWithScale=false)*/ float initFeatureScale=1.f,featureScaleMul=0.1f; int featureScaleLevels=1,initXyStep=6,initImgBound=0; bool varyXyStepWithScale=true,varyImgBoundWithScale=false; //set parameters readConfigFile(filename,"initFeatureScale",initFeatureScale); readConfigFile(filename,"featureScaleMul",featureScaleMul); readConfigFile(filename,"featureScaleLevels",featureScaleLevels); readConfigFile(filename,"initXyStep",initXyStep); readConfigFile(filename,"initImgBound",initImgBound); readConfigFile(filename,"varyXyStepWithScale",varyXyStepWithScale); readConfigFile(filename,"varyImgBoundWithScale",varyImgBoundWithScale); //initialize feature detector detector=new DenseFeatureDetector(initFeatureScale,featureScaleLevels,featureScaleMul, initXyStep,initImgBound,varyXyStepWithScale,varyImgBoundWithScale); //detect if(ImageID=="left") detector->detect(img1,keypoints); else if(ImageID=="right") detector->detect(img2,keypoints); else{ detector->detect(img1,keypoints); cerr<<"Unknown option for the image_id, the left image was detected instead!"<<endl; } break; } default: exitwithErrors("unknown type for feature extraction!"); } if(display) if(ImageID=="right") showKeypoints(img2,keypoints,imagescale); else showKeypoints(img1,keypoints,imagescale); if(saveKpts){ printKeypoints(outPath,keypoints); } }