We used a Logitech Pro 9000 type USB camera connected to the RoboRio. Wrote custom C++ code to track the tower.
A short video of our driver station in autonomous is on youtube.
https://youtu.be/PRhgljJ9zus
The yellow box is our region of interest. The light blue highlights show detection of bright vertical lines and yellow highlights show detection of bright horizontal lines. The black circle is our guess at the center-bottom of the tower window.
But alas, we only got it working at the last couple of matches. A lot of fun, but did not help us get to St Louis.
Our tracking code follows:
Code:
void Robot::trackTower(){
// Tower Tracking.
// copy the camera image (frame) into a 2D XY array. The array is
// 3D with the 3rd dimension being the 3 color 8 bit characters.
// Restrict search to the upper center of image.
// In both the horizontal X and vertical Y directions, find occurences
// of bright pixels with dark pixels on both sides. Tally every
// occurence in an X and Y histogram.
// this all assumes a 320x240 image and a 4 characters for red/green/blue/extra.
char *arrayP; // point to 2D array
arrayP = (char*)imaqImageToArray(frame,IMAQ_NO_RECT,NULL,NULL);
// not certain how to access array,so copy into local array.
memcpy (array,arrayP, sizeof(array));
memset (histoX,0,sizeof(histoX)); // histograms for dark-bright-dark occurances in X
memset (histoY,0,sizeof(histoY)); // histograms for dark-bright-dark occurances in Y
const int left = 50; // upper center search window
const int right = 210;
const int top = 0;
const int bottom = 60;
const int spread=8; // dark-bright-dark must occur in 6 pixels
const int threshold = 25; // bright must be 30 bigger than dark
// look for the bottom horizontal gaffer tape.
// only look at green color character [1].
// mark each pixel meeting the dark-bright-dark criteria blue
// tally each occurance in X histgram.
for (short col = left; col <= right; col++) {
for (short row = top+spread; row < bottom; row++) {
int center = array[row - spread / 2][col][1];
if (((center - array[row - spread][col][1]) > threshold) &&
((center - array[row][col][1]) > threshold)) {
array[row - spread / 2][col][0] = 0; // blue
// array[row - spread / 2][col][1] = 0;
array[row - spread / 2][col][2] = 255; // red
array[row - spread / 2][col][3] = 0; // flag
histoY[row - spread / 2]++;
}
}
}
// now find horizontal line by finding most occurances.
int max = 0;
int maxY =0; // row number of bottom tape
for (short row = top+1; row < bottom-1; row++) {
// use 3 histogram slots
int sumH = histoY[row-1] + histoY[row] + histoY[row+1];
if (sumH > max){
max = sumH; // found new peak
maxY = row;
}
}
// now look for vertical tapes. Only search down to bottom tape maxY
for (short row = top; row <= maxY; row++) {
for (short col = left+spread; col < right; col++) {
int center = array[row][col - spread / 2][1];
if (((center - array[row][col - spread][1]) > threshold) &&
((center - array[row][col][1]) > threshold)){
array[row][col - spread / 2][0] = 255; // blue
// array[row][col - spread / 2][1] = 255; // green
array[row][col - spread / 2][2] = 0;
array[row][col - spread / 2][3] = 0; // flag
histoX[col - spread / 2]++;
}
}
}
// look for the left and right vertical tapes
int max1 = 0; // first peak
int max2 = 0; // second peak
int maxX1 = 0;
int maxX2 = 0;
for (int col=left+1; col<=right-1; col++) {
// find the biggest peak, use 3 slots
int sumH = histoX[col-1] + histoX[col] + histoX[col+1];
if (sumH > max1){
max1 = sumH;
maxX1 = col;
}
}
for (int col=left+1; col<=right-1; col++) {
// find the 2nd peak
if (abs(maxX1 - col) < spread)
continue; // do not look if close to other peak
int sumH = histoX[col-1] + histoX[col] + histoX[col+1];
if (sumH > max2){
max2 = sumH;
maxX2 = col;
}
}
int maxX = (maxX1 + maxX2) / 2; // center or 2 peaks
if (max2 < 5) // did not find a good second peak
maxX = 0; // put it in middle
int startIndex = 0;
int maxLength = 0;
int maxStart = 0;
int endIndex = 0;
for (int col=left; col<=right; col++) {
int count = 0;
if (array[maxY][col][3] == 0){
count++;
}
if (array[maxY-1][col][3] == 0){
count++;
}
if (array[maxY+1][col][3] == 0){
count++;
}
if (startIndex > 0){
if (count < 1) {
endIndex = col;
if (maxLength < (endIndex - startIndex)){
maxLength = (endIndex - startIndex);
maxStart = startIndex;
}
startIndex = 0;
}
}else{
if(count > 1) {
startIndex = col;
}
}
}
//SmartDashboard::PutNumber("maxLength", maxLength);
maxX = maxStart + (maxLength /2);
// mark region of interest in yellow
for (short row = top; row <= bottom; row++) {
array[row][left][0] = 0; // blue
array[row][left][1] = 255; // green R+G = yellow
array[row][left][2] = 255; // red R+G = yellow
array[row][right][0] = 0; // blue
array[row][right][1] = 255; // green
array[row][right][2] = 255; // red
}
for (short col = left; col < right; col++) {
array[top][col][0] = 0; // blue
array[top][col][1] = 255; // green
array[top][col][2] = 255; // red R+G = yellow
array[bottom][col][0] = 0; // blue
array[bottom][col][1] = 255; // green
array[bottom][col][2] = 255; // red
}
/* look at one color
for (short col = left; col <= right; col++) {
for (short row = top; row < bottom; row++) {
array[row][col][0] = 0; // blue
array[row][col][1] = 0; // green
// array[row][col][2] = 0; // red
}
}
*/
// copy 2D array back into image
memcpy(arrayP, array, sizeof(array));
imaqArrayToImage(frame, array, 320, 240);
//SmartDashboard::PutNumber("a0",array[20][20][0]); // blue
//SmartDashboard::PutNumber("a1",array[20][20][1]); // green
//SmartDashboard::PutNumber("a2",array[20][20][2]); // red
//SmartDashboard::PutNumber("a3",array[20][20][3]); // not used
imaqDispose(arrayP);
// imaqDrawTextOnImage(frame,frame, {10,10},"hi there",NULL,NULL);
imaqDrawShapeOnImage(frame, frame, { maxY-5, maxX-5, 10, 10 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0);
Robot::chassis->trackingX = maxX; // let the world know
Robot::chassis->trackingY = maxY; // let the world know
}