unsigned char * recogBMP(char* filename)
{
int i = 0;
int j = 0;
int k = 0;
int top_height,bot_height;
/*read image head information*/
FILE *f = fopen(filename,"rb");
unsigned char info[54];
fread(info, sizeof(unsigned char), 54, f); // read the 54-byte header
int width = *(int*)&info[18];
int height = *(int*)&info[22];
/*read image data*/
int l_width = ((width*3 + 3)>>2)<<2; /*alignment*/
int size =l_width*height;
unsigned char* data = new unsigned char[size];
fread(data,sizeof(unsigned char),size,f);
fclose(f);
/*gray picture*/
for (i=0; i<height; i++) {
for (j=0; j<width; j++) {
k = i*l_width+ j*3;
unsigned char tmp = data[k];
data[k] = data[k+2];
data[k+2] = tmp;
int gray = (int)(((int)data[k])*30 + ((int)data[k+1])*59 + ((int)data[k+2])*11 + 50)/100;
if (gray >= 230) {/*white color*/
data[k] = 255;
data[k+1] = 255;
data[k+2] = 255;
} else { /*black color*/
data[k] = 0;
data[k+1] = 0;
data[k+2] = 0;
}
}
}
// ImageRotation(data,width,height);
featureExtract(data,width,height);
return 0;
}
void YawAngleEstimator::train()
{
printf("YawAngleEstimator:train\n");
vector<vector<KeyPoint>> kp(AngleNum,vector<KeyPoint>());
vector<Mat> descriptors(AngleNum,Mat());
featureExtract(YawTemplate, kp, descriptors, Feature);
if (useIndex)
{
//build Index with Lsh and Hamming distance
for (int i = 0; i < AngleNum; i++)
{
flann::Index tempIndex;
if (Feature == USE_SIFT)
{
tempIndex.build(descriptors[i], flann::KDTreeIndexParams(4), cvflann::FLANN_DIST_L2);
}
else
{
tempIndex.build(descriptors[i], flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);
}
YawIndex.push_back(tempIndex);
}
}
else
{
//build BFMathers
for (int i = 0; i < AngleNum; i++)
{
//record
fss<<"\nKeypoints number of template "<< i <<" is "<< descriptors[i].rows << endl;
BFMatcher tempMatcher;
vector<Mat> train_des(1, descriptors[i]);
tempMatcher.add(train_des);
tempMatcher.train();
matchers.push_back(tempMatcher);
}
}
}
//TODO:FlannIndexEstimate Debug
bool YawAngleEstimator::Estimate(Mat& CurrentFrame,float& CurrentAngle)
{
printf("YawAngleEstimator:estimate\n");
Mat TempFrame = CurrentFrame.clone();
if (TempFrame.channels() == 3)
cvtColor(TempFrame, TempFrame, CV_BGR2GRAY);
vector<Mat> MatchingImg(1, TempFrame);
vector<vector<KeyPoint>> CurrentKp(1, vector<KeyPoint>());
vector<Mat> CurrentDescriptors(1,Mat());
float* CurrentVote = new float[AngleNum];
float maxVote=0;
//Extract current feature
featureExtract(MatchingImg, CurrentKp, CurrentDescriptors, Feature);
//draw Keypoints on frame and record
drawKeypoints(CurrentFrame, CurrentKp[0], CurrentFrame);
fss <<"\nKeypoints number of CurrentFrame is "<<CurrentDescriptors[0].rows<<endl;
printf("Estimate::The Number of keypoints is:%d\n", CurrentDescriptors[0].rows);
if (useIndex)
Indexmatch(CurrentDescriptors[0], CurrentVote);
else
BFmatch(CurrentDescriptors[0], CurrentVote);
if (FramesVote.size() < FrameNum)
FramesVote.push_back(CurrentVote);
else//caculate FinalVote for each angle from last frame's vote
{
float* ptrTemp = FramesVote.front();
FramesVote.pop_front();
FramesVote.push_back(CurrentVote);
delete ptrTemp;
for (int i = 0; i < FrameNum; i++)
{
for (int j = 0; j < AngleNum; j++)
{
FinalVote[j] = FramesVote[i][j] * VoteWeight[i];
if (FinalVote[j] > maxVote)
{
maxVote = FinalVote[j];
AngleIndex = j;
}
}
}
for (int i = 0; i < AngleNum; i++)
{
cout << "Final Vote for template " << i << " is " << FinalVote[i] << endl;
//record
fss << "Final Vote for template " << i << " is " << FinalVote[i] << endl;
}
}
if (AngleIndex == -1)
{
printf("Cannot estimate right now!\n");
return false;
}
else
{
CurrentAngle = Angle[AngleIndex];
printf("The max vote of angle is: %f\n", maxVote);
return true;
}
}