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);
}
}
