void our_bot::FindCenter(){
vector <int> Area_base;
int Area_frontl=0;
int Area_frontr=0;
vector <RotatedRect> BaseCenter = all_contours( mask[0], Area_base,0 );
Point frontl_center = Point( 0, 0 );
Point frontr_center = Point( 0, 0 );
RotatedRect front_left;
RotatedRect front_right;
back_center = Point( 0, 0 );
// cout<<"BaseCenter size : "<<BaseCenter.size()<<"\n";
for( int i = 0; i < BaseCenter.size(); i++ ){
// cout<<"before contour if else\n";
if(Area_base[i] > BASE_AREA_THRESH) {
front_left = closest_contour( mask[1], BaseCenter[i].center, Area_frontl );
front_right = closest_contour( mask[2], BaseCenter[i].center, Area_frontr );
// cout<<front_left.center.x<<" "<<front_right.center.x<<"\n";
if( front_left.center.x != 0 && front_right.center.x !=0 ){
float angle_check =angl(front_right.center,BaseCenter[i].center,front_left.center);
cout << "angle check " << angle_check << endl;
if( //angle_check > 40 && angle_check < 80 &&
distanc(front_left.center,BaseCenter[i].center) < BOT_LENGTH &&
distanc(front_right.center,BaseCenter[i].center) < BOT_LENGTH )
{
// cout<<"finding centers\n";
back_center = BaseCenter[i].center;
frontl_center = front_left.center;
frontr_center = front_right.center;
break;
}
}
}
}
front_center = Point( ( frontl_center.x + frontr_center.x ) / 2, ( frontl_center.y + frontr_center.y ) / 2 );
}
double our_bot::orientation(){
angle = angl( Point( dst_point.x,dst_point.y ), back_center, front_center );
cout << "angle = " << angle << endl;
// if( angle < 0 )
// angle = -1 * angle;
// else if( angle == 1000) //case where the bot is not found in the bounding box.
// angle = 0;
// else if( angle > 0 )
// angle = 360 - angle;
bot_angle = angle;
// cout<<"Distance: "<<sqrt(pow(back_center.x-dst_point.x,2)+pow(back_center.y-dst_point.y,2))<<"\t";
distance = sqrt(pow(back_center.x-dst_point.x,2)+pow(back_center.y-dst_point.y,2));
// printf("angle%f\t",angle);
}
Feature
HOGFeatureExtractor::operator()(const CByteImage& img_) const
{
/******** BEGIN TODO ********/
// Compute the Histogram of Oriented Gradients feature
// Steps are:
// 1) Compute gradients in x and y directions. We provide the
// derivative kernel proposed in the paper in _kernelDx and
// _kernelDy.
// 2) Compute gradient magnitude and orientation
// 3) Add contribution each pixel to HOG cells whose
// support overlaps with pixel. Each cell has a support of size
// _cellSize and each histogram has _nAngularBins.
// 4) Normalize HOG for each cell. One simple strategy that is
// is also used in the SIFT descriptor is to first threshold
// the bin values so that no bin value is larger than some
// threshold (we leave it up to you do find this value) and
// then re-normalize the histogram so that it has norm 1. A more
// elaborate normalization scheme is proposed in Dalal & Triggs
// paper but we leave that as extra credit.
//
// Useful functions:
// convertRGB2GrayImage, TypeConvert, WarpGlobal, Convolve
int xCells = ceil(1.*img_.Shape().width / _cellSize);
int yCells = ceil(1.*img_.Shape().height / _cellSize);
CFloatImage HOGHist(xCells, yCells, _nAngularBins);
HOGHist.ClearPixels();
CByteImage gray(img_.Shape());
CFloatImage grayF(img_.Shape().width, img_.Shape().height, 1);
convertRGB2GrayImage(img_, gray);
TypeConvert(gray, grayF);
CFloatImage diffX( img_.Shape()), diffY( img_.Shape());
Convolve(grayF, diffX, _kernelDx);
Convolve(grayF, diffY, _kernelDy);
CFloatImage grad(grayF.Shape()), grad2(grayF.Shape());
CFloatImage angl(grayF.Shape()), angl2(grayF.Shape());
for (int y = 0; y <grayF.Shape().height; y++){
for (int x = 0; x<grayF.Shape().width; x++) {
grad2.Pixel(x,y,0) = (diffX.Pixel(x,y,0) * diffX.Pixel(x,y,0) +
diffY.Pixel(x,y,0) * diffY.Pixel(x,y,0));
angl2.Pixel(x,y,0) = atan(diffY.Pixel(x,y,0) / abs(diffY.Pixel(x,y,0)));
}
}
// Bilinear Filter
ConvolveSeparable(grad2, grad, ConvolveKernel_121,ConvolveKernel_121,1);
ConvolveSeparable(angl2, angl, ConvolveKernel_121,ConvolveKernel_121,1);
//WriteFile(diffX, "angle.tga");
//WriteFile(diffY, "angleG.tga");
for (int y = 0; y <grayF.Shape().height; y++){
for (int x = 0; x<grayF.Shape().width; x++) {
// Fit in the bins
int a = angl.Pixel(x,y,0) / 3.14 * (_nAngularBins) + _nAngularBins/2;
// Histogram
HOGHist.Pixel(floor(1.*x / _cellSize),
floor(1.*y / _cellSize),
a) += grad.Pixel(x,y,0);
}
}
// Normalization
float threshold = 0.7;
for (int y = 0; y < yCells; y++){
for (int x = 0; x < xCells; x++){
float total = 0;
for (int a = 0; a < _nAngularBins; a++) {
if (HOGHist.Pixel(x,y,a) > threshold)
HOGHist.Pixel(x,y,a) = threshold;
// Sum for normalization
total += HOGHist.Pixel(x,y,a);
}
for (int a = 0;a< _nAngularBins; a++) {
HOGHist.Pixel(x,y,a) /= total;
}
}
}
return HOGHist;
/******** END TODO ********/
}
