void TFT_ILI9163C::drawPixel(int16_t x, int16_t y, uint16_t color) {
if (boundaryCheck(x,y)) return;
if ((x < 0) || (y < 0)) return;
setAddr(x,y,x+1,y+1);
#if defined(__MK20DX128__) || defined(__MK20DX256__)
writedata16_last(color);
endProc();
#else
writedata16(color);
#endif
}
void TFT_ILI9163C::drawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t color) {
// Rudimentary clipping
if (boundaryCheck(x,y)) return;
if (((x+w) - 1) >= _width) w = _width-x;
setAddr(x,y,(x+w)-1,y);
while (w-- > 1) {
#if defined(__MK20DX128__) || defined(__MK20DX256__)
if (w == 0){
writedata16_last(color);
} else {
writedata16_cont(color);
}
#else
writedata16(color);
#endif
}
endProc();
}
void TFT_ILI9163C::drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color) {
// Rudimentary clipping
if (boundaryCheck(x,y)) return;
if (((y + h) - 1) >= _height) h = _height-y;
setAddr(x,y,x,(y+h)-1);
while (h-- > 1) {
#if defined(__MK20DX128__) || defined(__MK20DX256__)
if (h == 0){
writedata16_last(color);
} else {
writedata16_cont(color);
}
#else
writedata16(color);
#endif
}
endProc();
}
// fill a rectangle
void TFT_ILI9163C::fillRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color) {
if (boundaryCheck(x,y)) return;
if (((x + w) - 1) >= _width) w = _width - x;
if (((y + h) - 1) >= _height) h = _height - y;
setAddr(x,y,(x+w)-1,(y+h)-1);
for (y = h;y > 0;y--) {
for (x = w;x > 1;x--) {
#if defined(__MK20DX128__) || defined(__MK20DX256__)
writedata16_cont(color);
#else
writedata16(color);
#endif
}
#if defined(__MK20DX128__) || defined(__MK20DX256__)
writedata16_last(color);
#endif
}
endProc();
}
//=========================================
CvRect camKalTrack(IplImage* frame, camshift_kalman_tracker& camKalTrk) {
//=========================================
if (!frame)
printf("Input frame empty!\n");
cvCopy(frame, camKalTrk.image, 0);
cvCvtColor(camKalTrk.image, camKalTrk.hsv, CV_BGR2HSV); // BGR to HSV
if (camKalTrk.trackObject) {
int _vmin = vmin, _vmax = vmax;
cvInRangeS(camKalTrk.hsv, cvScalar(0, smin, MIN(_vmin,_vmax), 0), cvScalar(180, 256, MAX(_vmin,_vmax), 0), camKalTrk.mask); // MASK
cvSplit(camKalTrk.hsv, camKalTrk.hue, 0, 0, 0); // HUE
if (camKalTrk.trackObject < 0) {
float max_val = 0.f;
boundaryCheck(camKalTrk.originBox, frame->width, frame->height);
cvSetImageROI(camKalTrk.hue, camKalTrk.originBox); // for ROI
cvSetImageROI(camKalTrk.mask, camKalTrk.originBox); // for camKalTrk.mask
cvCalcHist(&camKalTrk.hue, camKalTrk.hist, 0, camKalTrk.mask); //
cvGetMinMaxHistValue(camKalTrk.hist, 0, &max_val, 0, 0);
cvConvertScale(camKalTrk.hist->bins, camKalTrk.hist->bins, max_val ? 255. / max_val : 0., 0); // bin [0,255]
cvResetImageROI(camKalTrk.hue); // remove ROI
cvResetImageROI(camKalTrk.mask);
camKalTrk.trackWindow = camKalTrk.originBox;
camKalTrk.trackObject = 1;
camKalTrk.lastpoint = camKalTrk.predictpoint = cvPoint(camKalTrk.trackWindow.x + camKalTrk.trackWindow.width / 2,
camKalTrk.trackWindow.y + camKalTrk.trackWindow.height / 2);
getCurrState(camKalTrk.kalman, camKalTrk.lastpoint, camKalTrk.predictpoint);//input curent state
}
//(x,y,vx,vy),
camKalTrk.prediction = cvKalmanPredict(camKalTrk.kalman, 0);//predicton=kalman->state_post
camKalTrk.predictpoint = cvPoint(cvRound(camKalTrk.prediction->data.fl[0]), cvRound(camKalTrk.prediction->data.fl[1]));
camKalTrk.trackWindow = cvRect(camKalTrk.predictpoint.x - camKalTrk.trackWindow.width / 2, camKalTrk.predictpoint.y
- camKalTrk.trackWindow.height / 2, camKalTrk.trackWindow.width, camKalTrk.trackWindow.height);
camKalTrk.trackWindow = checkRectBoundary(cvRect(0, 0, frame->width, frame->height), camKalTrk.trackWindow);
camKalTrk.searchWindow = cvRect(camKalTrk.trackWindow.x - region, camKalTrk.trackWindow.y - region, camKalTrk.trackWindow.width + 2
* region, camKalTrk.trackWindow.height + 2 * region);
camKalTrk.searchWindow = checkRectBoundary(cvRect(0, 0, frame->width, frame->height), camKalTrk.searchWindow);
cvSetImageROI(camKalTrk.hue, camKalTrk.searchWindow);
cvSetImageROI(camKalTrk.mask, camKalTrk.searchWindow);
cvSetImageROI(camKalTrk.backproject, camKalTrk.searchWindow);
cvCalcBackProject( &camKalTrk.hue, camKalTrk.backproject, camKalTrk.hist ); // back project
cvAnd(camKalTrk.backproject, camKalTrk.mask, camKalTrk.backproject, 0);
camKalTrk.trackWindow = cvRect(region, region, camKalTrk.trackWindow.width, camKalTrk.trackWindow.height);
if (camKalTrk.trackWindow.height > 5 && camKalTrk.trackWindow.width > 5) {
// calling CAMSHIFT
cvCamShift(camKalTrk.backproject, camKalTrk.trackWindow, cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1),
&camKalTrk.trackComp, &camKalTrk.trackBox);
/*cvMeanShift( camKalTrk.backproject, camKalTrk.trackWindow,
cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
&camKalTrk.trackComp);*/
}
else {
camKalTrk.trackComp.rect.x = 0;
camKalTrk.trackComp.rect.y = 0;
camKalTrk.trackComp.rect.width = 0;
camKalTrk.trackComp.rect.height = 0;
}
cvResetImageROI(camKalTrk.hue);
cvResetImageROI(camKalTrk.mask);
cvResetImageROI(camKalTrk.backproject);
camKalTrk.trackWindow = camKalTrk.trackComp.rect;
camKalTrk.trackWindow = cvRect(camKalTrk.trackWindow.x + camKalTrk.searchWindow.x, camKalTrk.trackWindow.y
+ camKalTrk.searchWindow.y, camKalTrk.trackWindow.width, camKalTrk.trackWindow.height);
camKalTrk.measurepoint = cvPoint(camKalTrk.trackWindow.x + camKalTrk.trackWindow.width / 2, camKalTrk.trackWindow.y
+ camKalTrk.trackWindow.height / 2);
camKalTrk.realposition->data.fl[0] = camKalTrk.measurepoint.x;
camKalTrk.realposition->data.fl[1] = camKalTrk.measurepoint.y;
camKalTrk.realposition->data.fl[2] = camKalTrk.measurepoint.x - camKalTrk.lastpoint.x;
camKalTrk.realposition->data.fl[3] = camKalTrk.measurepoint.y - camKalTrk.lastpoint.y;
camKalTrk.lastpoint = camKalTrk.measurepoint;//keep the current real position
//measurement x,y
cvMatMulAdd( camKalTrk.kalman->measurement_matrix/*2x4*/, camKalTrk.realposition/*4x1*/,/*measurementstate*/0, camKalTrk.measurement );
cvKalmanCorrect(camKalTrk.kalman, camKalTrk.measurement);
cvRectangle(frame, cvPoint(camKalTrk.trackWindow.x, camKalTrk.trackWindow.y), cvPoint(camKalTrk.trackWindow.x
+ camKalTrk.trackWindow.width, camKalTrk.trackWindow.y + camKalTrk.trackWindow.height), CV_RGB(255,128,0), 4, 8, 0);
}
// set new selection if it exists
if (camKalTrk.selectObject && camKalTrk.selection.width > 0 && camKalTrk.selection.height > 0) {
cvSetImageROI(camKalTrk.image, camKalTrk.selection);
cvXorS(camKalTrk.image, cvScalarAll(255), camKalTrk.image, 0);
cvResetImageROI(camKalTrk.image);
}
return camKalTrk.trackWindow;
}
void TFT_ILI9163C::setCursor(int16_t x, int16_t y) {
if (boundaryCheck(x,y)) return;
setAddrWindow(0x00,0x00,x,y);
cursor_x = x;
cursor_y = y;
}
