// TODO: add more scalarization options here: CIELABs, XYZ, PCA ...
static float *scalar(float *x, int w, int h, int pd, char *level_type)
{
if (pd == 1) return x;
float *y = xmalloc(w * h * pd * sizeof*y);
if (false) { ;
} else if (pd == 3 && 0 == strcmp(level_type, "h")) {
FORI(w*h) get_hsv(y+i, NULL, NULL, x + i*pd);
} else if (pd == 3 && 0 == strcmp(level_type, "s")) {
FORI(w*h) get_hsv(NULL, y+i, NULL, x + i*pd);
} else if (pd == 3 && 0 == strcmp(level_type, "v")) {
FORI(w*h) get_hsv(NULL, NULL, y+i, x + i*pd);
} else if (pd == 3 && 0 == strcmp(level_type, "i")) {
FORI(w*h) y[i] = get_rgb_intensity(x + i*pd);
} else if (pd == 3 && 0 == strcmp(level_type, "r")) {
FORI(w*h) y[i] = x[i*pd];
} else if (pd == 3 && 0 == strcmp(level_type, "g")) {
FORI(w*h) y[i] = x[i*pd + 1];
} else if (pd == 3 && 0 == strcmp(level_type, "b")) {
FORI(w*h) y[i] = x[i*pd + 2];
} else if (isdigit(level_type[0])) {
int c = bound(0, atoi(level_type), pd-1);
FORI(w*h) y[i] = x[i*pd + c];
} else {
FORI(w*h) {
float m = 0;
FORL(pd) m += x[i*pd + l];
y[i] = m/pd;
}
}
void render(PyObject * obj)
{
uint8_t * pixels;
int x, y;
float h, s, v;
PyArrayObject* arr = (PyArrayObject*)obj;
assert(PyArray_ISCARRAY(arr));
assert(PyArray_NDIM(arr) == 3);
assert(PyArray_DIM(arr, 0) == ccw_size);
assert(PyArray_DIM(arr, 1) == ccw_size);
assert(PyArray_DIM(arr, 2) == 4);
pixels = (uint8_t*)(PyArray_DATA(arr));
precalcDataIndex++;
precalcDataIndex %= 4;
PrecalcData * pre = precalcData[precalcDataIndex];
if (!pre) {
pre = precalcData[precalcDataIndex] = precalc_data(2*M_PI*(precalcDataIndex/4.0));
}
for (y=0; y<ccw_size; y++) {
for (x=0; x<ccw_size; x++) {
get_hsv(h, s, v, pre);
pre++;
hsv_to_rgb_range_one (&h, &s, &v);
uint8_t * p = pixels + 4*(y*ccw_size + x);
p[0] = h; p[1] = s; p[2] = v; p[3] = 255;
}
}
}
PyObject* pick_color_at(float x_, float y_)
{
float h,s,v;
PrecalcData * pre = precalcData[precalcDataIndex];
assert(precalcDataIndex >= 0);
assert(pre != NULL);
int x = CLAMP(x_, 0, ccw_size);
int y = CLAMP(y_, 0, ccw_size);
pre += y*ccw_size + x;
get_hsv(h, s, v, pre);
return Py_BuildValue("fff",h,s,v);
}
bool Robot::adjustWorldCoordinate(IplImage* image, double coordAdjustRate)
{
IplImage *img;
IplImage* src1=cvCreateImage(cvGetSize(image),IPL_DEPTH_8U,1);
if(image->nChannels==3)
{
IplImage *hsv_img = get_hsv(image);
img=worldMap.getField(hsv_img);
cvReleaseImage(&hsv_img);
src1=img;
}
else
{
img=image;
src1=img;
//cvCvtColor(img, src1, CV_BGR2GRAY);
}
if( img != 0 )
{
IplImage* dst = cvCreateImage( cvGetSize(img), 8, 1 );
IplImage* color_dst = cvCreateImage( cvGetSize(img), 8, 3 );
CvMemStorage* storage = cvCreateMemStorage(0);
CvSeq* ls = 0;
int i;
cvCanny( src1, dst, 50, 200, 3 );
cvCvtColor( dst, color_dst, CV_GRAY2BGR );
ls = cvHoughLines2( dst, storage, CV_HOUGH_PROBABILISTIC, 2, CV_PI/90, 20, 5, 30 );
//ls = cvHoughLines2( dst, storage, CV_HOUGH_PROBABILISTIC, 5, CV_PI/30, 10, 20, 5 );
vector<myLine> tmplines;
for( i = 0; i < ls->total; i++ )
{
CvPoint* tmpl = (CvPoint*)cvGetSeqElem(ls,i);
cvLine( color_dst, tmpl[0], tmpl[1], CV_RGB(255,0,0), 1, 8 );
cv::Point2f tmpp[2];
cv::Point2f scrPos(tmpl[0].x,tmpl[0].y);
cv::Point2f roboPos=worldMap.coord_screen2robot(scrPos,true);
cv::Point2f worldPos=worldMap.coord_robot2world(roboPos);
tmpp[0]=worldPos;
scrPos=cv::Point2f(tmpl[1].x,tmpl[1].y);
roboPos=worldMap.coord_screen2robot(scrPos,true);
worldPos=worldMap.coord_robot2world(roboPos);
tmpp[1]=worldPos;
myLine templ(tmpp[0],tmpp[1]);
if(templ.l>LINE_LENGTH_LBOUND)
tmplines.push_back(templ);
// //printf("length=%f angle=%f\n",sqrt(float((tmpl[1].y-tmpl[0].y)*(tmpl[1].y-tmpl[0].y));
// +float((tmpl[1].x-tmpl[0].x)*(tmpl[1].x-tmpl[0].x)))
// ,atan2(float(tmpl[1].y-tmpl[0].y),float(tmpl[1].x-tmpl[0].x)));
}
//printf("\n");
cvNamedWindow( "Source", 1 );
cvShowImage( "Source", img );
cvNamedWindow( "Hough", 1 );
cvShowImage( "Hough", color_dst );
cvWaitKey(10);
cvReleaseImage(&dst);
cvReleaseImage(&src1);
cvReleaseImage(&color_dst);
cvReleaseMemStorage(&storage);
if(coordAdjustRate==0)
{
for(i=0;i<tmplines.size();++i)
{
lines.push_back(tmplines[i]);
}
}
else if(coordAdjustRate==2)
{
for(i=0;i<tmplines.size();++i)
{
lines.push_back(tmplines[i]);
}
//vector<double> oris;
vector<int> lineNums;
vector<double> lineValues;
int groupId=0;
for(i=0;i<lines.size();++i)
{
bool classified=false;
int j;
for(j=0;j<i;++j)
{
double angle=lines[i].theta-lines[j].theta+CV_PI/4.0; //to make the process simple, add 45 degree
//to turn the cared angles to the middle of a phase
if(angle<0)
angle+=CV_PI*2.0;
int phase=(int)(angle/(CV_PI/2.0));
double angle90=angle-CV_PI/2.0*(double)phase;
phase%=2;
if(abs(angle90-CV_PI/4.0)<CV_PI/60.0)//subtract the added 45 degree
{
lines[i].clsId=lines[j].clsId/2*2+phase;
++lineNums[lines[i].clsId];
lineValues[lines[i].clsId]+=lines[i].l;
classified=true;
break;
}
}
if(classified==false)
{
lines[i].clsId=groupId;
lineNums.push_back(1);
lineNums.push_back(0);
lineValues.push_back(lines[i].l);
lineValues.push_back(0);
groupId+=2;
}
}
int maxValueGroup=0;
double maxValue=0;
for(i=0;i<lineNums.size();i+=2)
{
if(lineValues[i]+lineValues[i+1]>maxValue)
{
maxValue=lineValues[i]+lineValues[i+1];
maxValueGroup=i;
}
}
maxValueGroup/=2;
double sumAngle=0;
double sumL=0;
for(i=0;i<lines.size();++i)
{
if(lines[i].clsId/2==maxValueGroup)
{
double angle=lines[i].theta+CV_PI/4.0;//similar strategy, add 45 degree
if(angle<0)
angle+=CV_PI*2.0;
double angle90=angle-CV_PI/2.0*(double)((int)(angle/(CV_PI/2.0)));
sumAngle+=(angle90-CV_PI/4.0)*lines[i].l;//subtract 45 degree
sumL+=lines[i].l;
}
}
if(sumL==0)
{
//printf("false 2 sumL=0\n");
return false;
}
mainAngle=sumAngle/sumL;
mainGroupId=maxValueGroup;
//printf("mainAngle=%f mainGroupId=%d\n",mainAngle,mainGroupId);
}
else if(coordAdjustRate==1)
{
CvRect bBox=worldMap.getMap_bbox();
//printf("in func param=1\n");
//printf("tmplines.size=%d\n",tmplines.size());
for(i=0;i<tmplines.size();++i)
{
cv::Point2f imgPos=world2image(tmplines[i].p[0]);
if(!(imgPos.x>bBox.x-BBOX_DELTA && imgPos.x<bBox.x+bBox.width+BBOX_DELTA && imgPos.y>bBox.y-BBOX_DELTA && imgPos.y<bBox.y+bBox.height+BBOX_DELTA))
continue;
bool classified=false;
double minAngle=CV_PI;
int minAnglePhase=0;
int bestJ=-1;
int j;
for(j=0;j<lines.size();++j)
{
if(lines[j].clsId/2!=mainGroupId)
continue;
double angle=tmplines[i].theta-lines[j].theta+CV_PI/4.0; //to make the process simple, add 45 degree
//to turn the cared angles to the middle of a phase
if(angle<0)
angle+=CV_PI*2.0;
int phase=(int)(angle/(CV_PI/2.0));
double angle90=angle-CV_PI/2.0*(double)phase;
phase%=2;
if(abs(angle90-CV_PI/4.0)<minAngle)//subtract the added 45 degree
{
minAngle=abs(angle90-CV_PI/4.0);
bestJ=j;
minAnglePhase=phase;
}
}
if(bestJ>-1)
{
//if(minAngle<CV_PI/6.0)
tmplines[i].clsId=mainGroupId*2+minAnglePhase;
classified=true;
//printf("nearest main ori found. angle diff=%f\n",minAngle);
}
}
double sumAngle=0;
double sumL=0;
for(i=0;i<tmplines.size();++i)
{
if(tmplines[i].clsId/2==mainGroupId)
{
//printf("comparing with a main line..i=%d\n",i);
double angle=tmplines[i].theta+CV_PI/4.0;//similar strategy, add 45 degree
if(angle<0)
angle+=CV_PI*2.0;
double angle90=angle-CV_PI/2.0*double((int)(angle/(CV_PI/2.0)));
sumAngle+=angle90*tmplines[i].l;//use the 45 degree to balance the unwanted lines
sumL+=tmplines[i].l;
}
}
if(sumL<LINE_LENGTH_SUM_LBOUND)
{
//printf("false sumL=%f<%d\n",sumL,LINE_LENGTH_SUM_LBOUND);
return false;
}
double curAngle=sumAngle/sumL-CV_PI/4.0;//subtract 45 degree
ori+=curAngle-mainAngle;
//printf("true oriChange=%f\n",curAngle-mainAngle);
}
}
return true;
}
