static UINT32 opSETFi(void) // setf imm5,r2
{
UINT32 op1=I5(OP);
UINT32 op2=PSW&0xf;
op1&=0xf;
SETREG(GET2,(op1==op2)?1:0);
return clkIF;
}
static UINT32 opCMPi(void) // cmpi imm5,r2
{
UINT32 op1=I5(OP);
UINT32 op2=GETREG(GET2);
UINT64 res=(UINT64)op2-(UINT64)op1;
CHECK_CY(res);
CHECK_OVSUB(op1,op2,res);
CHECK_ZS(res);
return clkIF;
}
static UINT32 opADDi(void) // add imm5,r2
{
UINT32 op1=I5(OP);
UINT32 op2=GETREG(GET2);
UINT64 res=(UINT64)op2+(UINT64)op1;
CHECK_CY(res);
CHECK_OVADD(op1,op2,res);
CHECK_ZS(res);
SETREG(GET2,res);
return clkIF;
}
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
//bridge that will transform the message (image) from ROS code back to "image" code
sensor_msgs::CvBridge bridge;
fprintf(stderr, "\n callBaack funtion \n");
//publish data (obstacle waypoints) back to the boat
ros::NodeHandle n;
//std_msgs::Float32 xWaypoint_msg; // X coordinate obstacle message
//std_msgs::Float32 yWaypoint_msg; // Y coordinate obstacle message
//publish the waypoint data
//ros::Publisher waypoint_info_pub = n.advertise<std_msgs::Float32>("waypoint_info", 1000);
//ros::Publisher Ywaypoint_info_pub = n.advertise<std_msgs::Float32>("waypoint_info", 1000);
//std::stringstream ss;
/***********************************************************************/
//live image coming streamed straight from the boat's camera
IplImage* boatFront = bridge.imgMsgToCv(msg, "bgr8");
IplImage* backUpImage = bridge.imgMsgToCv(msg, "bgr8");
boatFront->origin = IPL_ORIGIN_TL; //sets image origin to top left corner
//Crop the image to the ROI
cvSetImageROI(boatFront, cvRect(0,0,boatFront->height/0.5,boatFront->width/1.83));
int X = boatFront->height;
int Y = boatFront->width;
/***********************************************************************/
//boat's edge distance from the camera. This is used for visual calibration
//to know the distance from the boat to the nearest obstacles.
//With respect to the mounted camera, distance is 21 inches (0.5334 m) side to side
//and 15 inches (0.381 m).
//float boatFrontDistance = 0.381; //distance in meters
//float boatSideDistance = 0.5334; //distance in meters
// These variables tell the distance from the center bottom of the image
// (the camera) to the square surrounding a the obstacle
float xObstacleDistance = 0.0;
float yObstacleDistance = 0.0;
float obstacleDistance = 0.0;
int pixelsNumber = 6; //number of pixels for an n x n matrix and # of neighbors
const int arraySize = pixelsNumber;
const int threeArraySize = pixelsNumber;
//if n gets changed, then the algorithm might have to be
//recalibrated. Try to keep it constant
//these variables are used for the k nearest neighbors
//int accuracy;
//reponses for each of the classifications
float responseWaterH, responseWaterS, responseWaterV;
float responseGroundH, responseGroundS, responseGroundV;
//float responseSkyH, responseSkyS, responseSkyV;
float averageHue = 0.0;
float averageSat = 0.0;
float averageVal = 0.0;
CvMat* trainClasses = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
CvMat* trainClasses2 = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
for (int i = 0; i < pixelsNumber/2; i++)
{
cvmSet(trainClasses, i,0,1);
cvmSet(trainClasses2, i,0,1);
}
for (int i = pixelsNumber/2; i < pixelsNumber; i++)
{
cvmSet(trainClasses, i,0,2);
cvmSet(trainClasses2, i,0,2);
}
//for (int i =0; i<pixelsNumber;i++)
//{
// cout << cvmGet(trainClasses,i,0);
// cout << cvmGet(trainClasses2,i,0);
//}
//CvMat sample = cvMat( 1, 2, CV_32FC1, _sample );
//used with the classifier
CvMat* nearestWaterH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestWaterS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestWaterV = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* nearestSkyH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* nearestSkyS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* nearestSkyV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//Distance
//CvMat* distanceWaterH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceWaterS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceWaterV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceGroundH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceGroundS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceGroundV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceSkyH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceSkyS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceSkyV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//these variables are use to traverse the picture by blocks of n x n pixels at
//a time.
//Index(0,0) does not exist, so make sure kj and ki start from 1 (in the
//right way, of course)
//x and y are the dimensions of the local patch of pixels
int x = (boatFront->height)/2.5 + pixelsNumber + 99;
int y = pixelsNumber-1;
int ix = 0;
int iy = 0;
int skyX = 0;
int skyY = 0;
//M controls the x axis (up and down); N controls the y axis (left and
//right)
int Mw = -550;
int Nw = 1300;
int Mg = -350;
int Ng = 700;
int row1 = 0;
int column1 = 0;
int row2 = 0;
int column2 = 0;
//ground sample
CvMat* groundTrainingHue = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* groundTrainingSat = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* groundTrainingVal = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//water sample
CvMat* waterTrainingHue = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* waterTrainingSat = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* waterTrainingVal = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//n x n sample patch taken from the picture
CvMat* sampleHue = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* sampleSat = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* sampleVal = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* resampleHue = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* resampleSat = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* resampleVal = cvCreateMat(arraySize,arraySize,CV_32FC1);
//sky training sample
CvMat* skyTrainingHue = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* skyTrainingSat = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* skyTrainingVal = cvCreateMat(arraySize,arraySize,CV_32FC1);
//initialize each matrix element to zero for ease of use
cvZero(groundTrainingHue);
cvZero(groundTrainingSat);
cvZero(groundTrainingVal);
cvZero(waterTrainingHue);
cvZero(waterTrainingSat);
cvZero(waterTrainingVal);
cvZero(sampleHue);
cvZero(sampleSat);
cvZero(sampleVal);
cvZero(resampleHue);
cvZero(resampleSat);
cvZero(resampleVal);
cvZero(skyTrainingHue);
cvZero(skyTrainingSat);
cvZero(skyTrainingVal);
//Stores the votes for each channel (whether it belongs to water or not
//1 is part of water, 0 not part of water
//if sum of votes is bigger than 1/2 the number of elements, then it belongs to water
int votesSum = 0;
int comparator[3]; //used when only three votes are needed
//int comparatorTwo [3][3]; //used when six votes are needed
//initial sum of votes is zero
//Error if initialize both matrices inside a single for loop. Dont know why
// for(int i = 0; i < 3; i++)
//{
//comparator[i] = 0;
// for(int j = 0; j < 3; j++)
// {
// comparatorTwo[i][j] = 0;
// }
//}
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
/***********************************************************************/
//Convert from RGB to HSV to control the brightness of the objects.
//work with reflexion
/*Sky recognition. Might be useful for detecting reflexion on the water. If
the sky is detected, and the reflection has the same characteristics of
something below the horizon, that "something" might be water. Assume sky
wont go below the horizon*/
//convert from RGB to HSV
cvCvtColor(boatFront, boatFront, CV_BGR2HSV);
cvCvtColor(backUpImage, backUpImage, CV_BGR2HSV);
HsvImage I(boatFront);
HsvImage IBackUp(backUpImage);
fprintf(stderr,"\n About to do Sky detection\n");
//Sky detection
for (int i=0; i<boatFront->height/3;i++)
{
for (int j=0; j<boatFront->width;j++)
{
//if something is bright enough, consider it sky and store the
//value. HSV values go from 0 to 180 ... RGB goes from 0 to 255
if (((I[i][j].v >= 180) && (I[i][j].s <= 16)))
// && ((I[i][j].h >=10)))) //&& (I[i][j].h <= 144))))
{
//The HSV values vary between 0 and 1
cvmSet(skyTrainingHue,skyX,skyY,I[i][j].h);
cvmSet(skyTrainingSat,skyX,skyY,I[i][j].s);
cvmSet(skyTrainingVal,skyX,skyY,I[i][j].v);
I[i][j].h = 0.3*180; //H (color)
I[i][j].s = 0.3*180; //S (color intensity)
I[i][j].v = 0.6*180; //V (brightness)
if (skyY == pixelsNumber-1)
{
if (skyX == pixelsNumber-1)
skyX = 1;
else
skyX = skyX + 1;
skyY = 1;
}
else
skyY = skyY + 1;
}
}
}
/***********************************************************************/
//offline input pictures. Samples of water properties are taken from these
//pictures to get a range of values for H, S, V that will be stored into a
//pre-defined classifier
IplImage* imageSample1 = cvLoadImage("bigObstacle.jpg");
cvSetImageROI(imageSample1, cvRect(0,0,imageSample1->height/0.5,imageSample1->width/1.83));
cvCvtColor(imageSample1, imageSample1, CV_BGR2HSV);
HsvImage I1(imageSample1);
IplImage* imageSample2 = cvLoadImage("bigObstacle2.jpg");
cvSetImageROI(imageSample2, cvRect(0,0,imageSample2->height/0.5,imageSample2->width/1.83));
cvCvtColor(imageSample2, imageSample2, CV_BGR2HSV);
HsvImage I2(imageSample2);
IplImage* imageSample3 = cvLoadImage("bigObstacle3.jpg");
cvSetImageROI(imageSample3, cvRect(0,0,imageSample3->height/0.5,imageSample3->width/1.83));
cvCvtColor(imageSample3, imageSample3, CV_BGR2HSV);
HsvImage I3(imageSample3);
IplImage* imageSample4 = cvLoadImage("river.jpg");
cvSetImageROI(imageSample4, cvRect(0,0,imageSample4->height/0.5,imageSample4->width/1.83));
cvCvtColor(imageSample4, imageSample4, CV_BGR2HSV);
HsvImage I4(imageSample4);
IplImage* imageSample5 = cvLoadImage("river2.jpg");
cvSetImageROI(imageSample5, cvRect(0,0,imageSample5->height/0.5,imageSample5->width/1.83));
cvCvtColor(imageSample5, imageSample5, CV_BGR2HSV);
HsvImage I5(imageSample5);
IplImage* imageSample6 = cvLoadImage("roundObstacle4.jpg");
cvSetImageROI(imageSample6, cvRect(0,0,imageSample6->height/0.5,imageSample6->width/1.83));
cvCvtColor(imageSample6, imageSample6, CV_BGR2HSV);
HsvImage I6(imageSample6);
IplImage* imageSample7 = cvLoadImage("farm.jpg");
cvSetImageROI(imageSample7, cvRect(0,0,imageSample7->height/0.5,imageSample7->width/1.83));
cvCvtColor(imageSample7, imageSample7, CV_BGR2HSV);
HsvImage I7(imageSample7);
IplImage* imageSample8 = cvLoadImage("bigObstacle4.jpg");
cvSetImageROI(imageSample8, cvRect(0,0,imageSample8->height/0.5,imageSample8->width/1.83));
cvCvtColor(imageSample8, imageSample8, CV_BGR2HSV);
HsvImage I8(imageSample8);
IplImage* imageSample9 = cvLoadImage("roundObstacle6.jpg");
cvSetImageROI(imageSample9, cvRect(0,0,imageSample9->height/0.5,imageSample9->width/1.83));
cvCvtColor(imageSample9, imageSample9, CV_BGR2HSV);
HsvImage I9(imageSample9);
IplImage* imageSample10 = cvLoadImage("roundObstacle.jpg");
cvSetImageROI(imageSample10, cvRect(0,0,imageSample10->height/0.5,imageSample10->width/1.83));
cvCvtColor(imageSample10, imageSample10, CV_BGR2HSV);
HsvImage I10(imageSample10);
fprintf(stderr,"\n Grab water samples\n");
//grab water samples from each picture
for (int i=0; i < threeArraySize; i++)
{
fprintf(stderr,"\n patch is pink (this is for me to know where the ground patch sample is\n");
for (int j=0; j < arraySize; j++)
{
row1 = ceil(X/1.2866)+ceil(X/5.237)+i+Mw;
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = ceil(Y/7.0755)+ceil(Y/21.01622)+j+Nw;
if (column1 > Y-1)
column1 = Y-1;
averageHue = (I1[row1][column1].h + I2[row1][column1].h + I3[row1][column1].h + I4[row1][column1].h + I5[row1][column1].h +
I6[row1][column1].h + I7[row1][column1].h + I8[row1][column1].h + I9[row1][column1].h + I10[row1][column1].h) / 10;
averageSat = (I1[row1][column1].s + I2[row1][column1].s + I3[row1][column1].s + I4[row1][column1].s + I5[row1][column1].s +
I6[row1][column1].s + I7[row1][column1].s + I8[row1][column1].s + I9[row1][column1].s + I10[row1][column1].s) / 10;
averageVal = (I1[row1][column1].v + I2[row1][column1].v + I3[row1][column1].v + I4[row1][column1].v + I5[row1][column1].v +
I6[row1][column1].v + I7[row1][column1].v + I8[row1][column1].v + I9[row1][column1].v + I10[row1][column1].v) / 10;
fprintf(stderr,"\n water patch sample (n X n matrix)\n");
cvmSet(waterTrainingHue,i,j,averageHue);
cvmSet(waterTrainingSat,i,j,averageSat);
cvmSet(waterTrainingVal,i,j,averageVal);
fprintf(stderr,"\n patch is red (this is for me to know where the ground patch sample is\n");
//I[row1][column1].h = 0;
//I[row1][column1].s = 255;
//I[row1][column1].v = 255;
}
}
fprintf(stderr,"\n Order water samples in ascending\n");
//order the water samples in ascending order on order to know a range
cvSort(waterTrainingHue, waterTrainingHue, CV_SORT_ASCENDING);
cvSort(waterTrainingSat, waterTrainingSat, CV_SORT_ASCENDING);
cvSort(waterTrainingVal, waterTrainingVal, CV_SORT_ASCENDING);
// find the maximum and minimum values in the array to create a range
int maxH = cvmGet(waterTrainingHue,0,0);
int maxS = cvmGet(waterTrainingSat,0,0);
int maxV = cvmGet(waterTrainingVal,0,0);
int minH = cvmGet(waterTrainingHue,0,0);
int minS = cvmGet(waterTrainingSat,0,0);
int minV = cvmGet(waterTrainingVal,0,0);
for (int i=0; i < threeArraySize; i++)
{
for (int j=0; j < arraySize; j++)
{
if (cvmGet(waterTrainingHue,i,j) > maxH)
maxH = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingSat,i,j) > maxS)
maxS = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingVal,i,j) > maxV)
maxV = cvmGet(waterTrainingVal,i,j);
if (cvmGet(waterTrainingHue,i,j) < minH)
minH = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingSat,i,j) < minS)
minS = cvmGet(waterTrainingSat,i,j);
if (cvmGet(waterTrainingVal,i,j) < minV)
minV = cvmGet(waterTrainingVal,i,j);
}
}
/***********************************************************************/
//Grab a random patch of water below the horizon and compare every other
//pixel against it
//The results of the water detection depend on where in the picture the
//training samples are located. Maybe adding more training samples will
//help improve this?
fprintf(stderr,"\n Random patch\n");
/*for (int i=0; i < threeArraySize; i++)
{
for (int j=0; j < arraySize; j++)
{
row2 = ceil(X/4.7291)+ceil(X/8.3176)+i+Mg;
column2 = ceil(Y/7.78378)+ceil(Y/16.54468)+j+Ng;
//ground patch sample (n X n matrix)
//Detecting the horizon in the picture might be an excellent visual aid to
//choose where (above the horizon) you can take a ground training(1:3*n,1:n)g sample
//from. The ground pixel sample can be at a constant distance from the
//horizon
cvmSet(groundTrainingHue,i,j,I[row2][column2].h);
cvmSet(groundTrainingSat,i,j,I[row2][column2].s);
cvmSet(groundTrainingVal,i,j,I[row2][column2].v);
//patch is red (this is for me to know where the ground patch sample is)
I[row2][column2].h = 60;
I[row2][column2].s = 180;
I[row2][column2].v = 90;
}
}
//order the water samples in ascending order on order to know a range
cvSort(groundTrainingHue, groundTrainingHue, CV_SORT_ASCENDING);
cvSort(groundTrainingSat, groundTrainingSat, CV_SORT_ASCENDING);
cvSort(groundTrainingVal, groundTrainingVal, CV_SORT_ASCENDING);
*/
// Main loop. It traverses through the picture
//skyX = 0;
//skyY = 0;
//The distance formula calculated by plotting points is given by:
/*********** distance = 0.0006994144*(1.011716711^x) *****************/
//cout << "Distance: " << distancePixels << endl;
while (x < boatFront->height/1.158)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i<pixelsNumber;i++)
{
cvmSet(sampleHue,0,i,I[x][y].h);
cvmSet(sampleSat,0,i,I[x][y].s);
cvmSet(sampleVal,0,i,I[x][y].v);
}
//Find the shortest distance between a pixel and the neighbors from each of
//the training samples (sort of inefficient, but might do the job...sometimes)
//if (ix == pixelsNumber-1)
//{
//HSV for water sample
// learn classifier
//CvKNearest knn(trainData, trainClasses, 0, false, itemsNumber);
//CvKNearest knnWaterHue(waterTrainingHue, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnWaterSat(waterTrainingSat, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnWaterVal(waterTrainingVal, trainClasses, 0, false, pixelsNumber);
//HSV for ground sample
//CvKNearest knnGroundHue(groundTrainingHue, trainClasses2, 0, false, pixelsNumber);
//CvKNearest knnGroundSat(groundTrainingSat, trainClasses2, 0, false, pixelsNumber);
//CvKNearest knnGroundVal(groundTrainingVal, trainClasses2, 0, false, pixelsNumber);
//HSV for sky sample
//if (cvmGet(skyTrainingHue,0,0)!=0.0 && cvmGet(skyTrainingSat,0,0)!=0.0 && cvmGet(skyTrainingVal,0,0)!=0.0)
//{
// CvKNearest knnSkyHue(skyTrainingHue, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnSkySat(skyTrainingSat, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnSkyVal(skyTrainingVal, trainClasses, 0, false, pixelsNumber);
//}
//scan nearest neighbors to each pixel
//responseWaterH = knnWaterHue.find_nearest(sampleHue,pixelsNumber,0,0,nearestWaterH,0);
//responseWaterS = knnWaterSat.find_nearest(sampleSat,pixelsNumber,0,0,nearestWaterS,0);
//responseWaterV = knnWaterVal.find_nearest(sampleVal,pixelsNumber,0,0,nearestWaterV,0);
//responseGroundH = knnGroundHue.find_nearest(sampleHue,pixelsNumber,0,0,nearestGroundH,0);
//responseGroundS = knnGroundSat.find_nearest(sampleSat,pixelsNumber,0,0,nearestGroundS,0);
//responseGroundV = knnGroundVal.find_nearest(sampleVal,pixelsNumber,0,0,nearestGroundV,0);
for (int i=0;i<pixelsNumber;i++)
{
for (int j=0;j<pixelsNumber;j++)
{
if ((minH <= cvmGet(sampleHue,0,j)) || (maxH >= cvmGet(sampleHue,0,j)))
//mark water samples as green
comparator[0] = 1;
else
comparator[0] = 0;
if (((minS <= cvmGet(sampleSat,0,j)) || (maxS <= cvmGet(sampleSat,0,j))))
//mark water samples as green
comparator[1] = 1;
else
comparator[1] = 0;
if ((minV <= cvmGet(sampleVal,0,j)) || (maxV <= cvmGet(sampleVal,0,j)))
//mark water samples as green
comparator[2] = 1;
else
comparator[2] = 0;
//count votes
for (int i3=0; i3 < 3; i3++)
votesSum = votesSum + comparator[i3];
//sky detection
if (votesSum > 1) //&& ((sampleSat[i][j] - sampleVal[i][j]) <= 0.1*180)
{
// classify pixel as water
I[x-pixelsNumber+i][y-pixelsNumber+j].h = 0;
I[x-pixelsNumber+i][y-pixelsNumber+j].s = 255;
I[x-pixelsNumber+i][y-pixelsNumber+j].v = 255;
}
votesSum = 0;
}
}
if (y < Y-1)
y = y + pixelsNumber-1;
if (y > Y-1)
y = Y-1;
else if (y == Y-1)
{
x = x + pixelsNumber-1;
y = pixelsNumber-1;
}
//ix = 0;
}
//traverse through the image one more time, divide the image in grids of
// 500x500 pixels, and see how many pixels of water are in each grid. If
// most of the pixels are labeled water, then mark all the other pixels
// as water as well
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
//int counter = 0;
int xDivisor = 20;
int yDivisor = 20;
votesSum = 0;
column1 = 0;
row1 = 0;
x = ceil(boatFront->height/2.5);
obstacleDistance = x;
y = 0;
int counter = 0;
while (x < boatFront->height/1.2)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i < ceil(boatFront->height/xDivisor); i++)
{
for(int j = 0; j < ceil(boatFront->width/yDivisor); j++)
{
cvmSet(resampleHue,i,j,I[x+i][y+j].h);
cvmSet(resampleSat,i,j,I[x+i][y+j].s);
cvmSet(resampleVal,i,j,I[x+i][y+j].v);
if(cvmGet(resampleHue,i,j)==0 && cvmGet(resampleSat,i,j)==255 && cvmGet(resampleVal,i,j)==255)
{
votesSum++;
}
}
}
if (votesSum > ((boatFront->height/xDivisor)*(boatFront->width/yDivisor)*(8.9/9)))
{
// if bigger than 4/5 the total number of pixels in a square, then consider the entire thing as water
// We might need to use other smaller quantities (like 5/6 maybe?)
for (int i = 0; i < ceil(boatFront->height/xDivisor);i++)
{
for (int j = 0; j < ceil(boatFront->width/yDivisor); j++)
{
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = y+j;
I[row1][column1].h = 0;
I[row1][column1].s = 255;
I[row1][column1].v = 255;
}
}
}
else
{
// If not water, eliminate all red pixels and turn those pixels
// back to the original color. These pixels shall, then, be marked
// as obstacles
for (int i = 0; i < ceil(boatFront->height/xDivisor);i++)
{
for (int j = 0; j < ceil(boatFront->width/yDivisor); j++)
{
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = y+j;
//the darker the color, the closer the object to the boat
I[row1][column1].h = 128;
I[row1][column1].s = 255;
I[row1][column1].v = 255 - counter;
//I[row1][column1].h = IBackUp[row1][column1].h;
//I[row1][column1].s = IBackUp[row1][column1].s;
//I[row1][column1].v = IBackUp[row1][column1].v;
//counter = counter + 20;
}
}
//The distance formula calculated by plotting points is given by:
/*********** distance = (1.76e-11)*pow(pixels,3.99) *****************/
/*********** pixel = (513.9332077469)pow(distance,0.240675506 *****************/
// Convert from pixel distance to normal distance in meters
if(obstacleDistance > sqrt(pow(xObstacleDistance,2) + pow(yObstacleDistance,2)))
{
// x,y coordinates of the obstacle
xObstacleDistance = (1.76e-11)*pow(((boatFront->height/xDivisor)+x)/2, 3.99) ;
yObstacleDistance = (1.76e-11)*pow(((boatFront->width/yDivisor)+y)/2, 3.99);
//xWaypoint_msg = xObstacleDistance;
//yWaypoint_msg = yObstacleDistance;
//publish position data
//waypoint_info_pub.publish(xWaypoint_msg);
//waypoint_info_pub.publish(yWaypoint_msg);
//ROS_INFO("Obstacle coordinates: X = %f meters, Y = %f meters", xObstacleDistance, yObstacleDistance);
obstacleDistance = sqrt(pow(xObstacleDistance,2) + pow(yObstacleDistance,2));
//ROS_INFO("Obstacle distance from: %f", obstacleDistance);
}
//cout << "Distance to Obstacle is: " << obstacleDistance << endl << endl;
}
y = y + boatFront->width/xDivisor;
if (y > Y-1)
{
x = x + boatFront->height/yDivisor;
y = 0;
counter = counter + 30;
}
votesSum = 0;
}
cvCvtColor(boatFront, boatFront, CV_HSV2BGR);
cvCvtColor(backUpImage, backUpImage, CV_HSV2BGR)
/**************************************************************************/
try
{
cvShowImage("Boat Front", boatFront);
}
catch (sensor_msgs::CvBridgeException& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
}
offs_t v810_disassembler::disassemble(std::ostream &stream, offs_t pc, const data_buffer &opcodes, const data_buffer ¶ms)
{
uint32_t flags = 0;
uint32_t opc,opc2;
unsigned size;
opc = opcodes.r16(pc);
opc2 = opcodes.r16(pc+2);
switch(opc>>10)
{
case 0x00: util::stream_format(stream,"MOV %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x01: util::stream_format(stream,"ADD %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x02: util::stream_format(stream,"SUB %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x03: util::stream_format(stream,"CMP %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x04: util::stream_format(stream,"SHL %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x05: util::stream_format(stream,"SHR %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x06: util::stream_format(stream,"JMP [%s]",GET1s(opc)); size=2; if ((opc&0x1f) == 31) flags = STEP_OUT; break;
case 0x07: util::stream_format(stream,"SAR %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x08: util::stream_format(stream,"MUL %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x09: util::stream_format(stream,"DIV %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0a: util::stream_format(stream,"MULU %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0b: util::stream_format(stream,"DIVU %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0c: util::stream_format(stream,"OR %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0d: util::stream_format(stream,"AND %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0e: util::stream_format(stream,"XOR %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0f: util::stream_format(stream,"NOT %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x10: util::stream_format(stream,"MOV %X,%s",I5(opc),GET2s(opc)); size=2; break;
case 0x11: util::stream_format(stream,"ADD %X,%s",I5(opc),GET2s(opc)); size=2; break;
case 0x12: util::stream_format(stream,"SETF %X,%s",I5(opc),GET2s(opc)); size=2; break;
case 0x13: util::stream_format(stream,"CMP %X,%s",I5(opc),GET2s(opc)); size=2; break;
case 0x14: util::stream_format(stream,"SHL %X,%s",UI5(opc),GET2s(opc)); size=2; break;
case 0x15: util::stream_format(stream,"SHR %X,%s",UI5(opc),GET2s(opc)); size=2; break;
case 0x16: util::stream_format(stream,"EI"); size=2; break;
case 0x17: util::stream_format(stream,"SAR %X,%s",UI5(opc),GET2s(opc)); size=2; break;
case 0x18: util::stream_format(stream,"TRAP %X",I5(opc)); size=2; break;
case 0x19: util::stream_format(stream,"RETI"); size=2; flags = STEP_OUT; break;
case 0x1a: util::stream_format(stream,"HALT"); size=2; break;
case 0x1b: util::stream_format(stream,"Unk 0x1B"); size=2; break;
case 0x1c: util::stream_format(stream,"LDSR %s,%s",GET2s(opc),GETRs(opc));size=2; break;
case 0x1d: util::stream_format(stream,"STSR %s,%s",GETRs(opc),GET2s(opc));size=2; break;
case 0x1e: util::stream_format(stream,"DI"); size=2; break;
case 0x1f:
switch(opc&0x1f)
{
case 0x00: util::stream_format(stream,"SCH0BSU"); break;
case 0x01: util::stream_format(stream,"SCH0BSD"); break;
case 0x02: util::stream_format(stream,"SCH1BSU"); break;
case 0x03: util::stream_format(stream,"SCH1BSD"); break;
case 0x04: util::stream_format(stream,"UnkS 4"); break;
case 0x05: util::stream_format(stream,"UnkS 5"); break;
case 0x06: util::stream_format(stream,"UnkS 6"); break;
case 0x08: util::stream_format(stream,"ORBSU"); break;
case 0x09: util::stream_format(stream,"ANDBSU"); break;
case 0x0a: util::stream_format(stream,"XORBSU"); break;
case 0x0b: util::stream_format(stream,"MOVBSU"); break;
case 0x0c: util::stream_format(stream,"ORNBSU"); break;
case 0x0d: util::stream_format(stream,"ANDNBSU"); break;
case 0x0e: util::stream_format(stream,"XORNBSU"); break;
case 0x0f: util::stream_format(stream,"NOTBSU"); break;
default: util::stream_format(stream,"UnkBS 0x%X",opc&0x1f); break;
}
size=2;
break;
case 0x20:
case 0x21:
case 0x22:
case 0x23:
case 0x24:
case 0x25:
case 0x26:
case 0x27: switch( (opc>>9) &0xf)
{
case 0x0: util::stream_format(stream,"BV %X",pc+D9(opc)); break;
case 0x1: util::stream_format(stream,"BL %X",pc+D9(opc)); break;
case 0x2: util::stream_format(stream,"BE %X",pc+D9(opc)); break;
case 0x3: util::stream_format(stream,"BNH %X",pc+D9(opc)); break;
case 0x4: util::stream_format(stream,"BN %X",pc+D9(opc)); break;
case 0x5: util::stream_format(stream,"BR %X",pc+D9(opc)); break;
case 0x6: util::stream_format(stream,"BLT %X",pc+D9(opc)); break;
case 0x7: util::stream_format(stream,"BLE %X",pc+D9(opc)); break;
case 0x8: util::stream_format(stream,"BNV %X",pc+D9(opc)); break;
case 0x9: util::stream_format(stream,"BNL %X",pc+D9(opc)); break;
case 0xa: util::stream_format(stream,"BNE %X",pc+D9(opc)); break;
case 0xb: util::stream_format(stream,"BH %X",pc+D9(opc)); break;
case 0xc: util::stream_format(stream,"BP %X",pc+D9(opc)); break;
case 0xd: util::stream_format(stream,"NOP"); break;
case 0xe: util::stream_format(stream,"BGE %X",pc+D9(opc)); break;
case 0xf: util::stream_format(stream,"BGT %X",pc+D9(opc)); break;
}
size=2;
break;
case 0x28: util::stream_format(stream,"MOVEA %X, %s, %s",I16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x29: util::stream_format(stream,"ADDI %X, %s, %s",I16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x2a: util::stream_format(stream,"JR %X",pc+D26(opc,opc2));size=4; break;
case 0x2b: util::stream_format(stream,"JAL %X",pc+D26(opc,opc2));size=4; flags = STEP_OVER; break;
case 0x2c: util::stream_format(stream,"ORI %X, %s, %s",UI16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x2d: util::stream_format(stream,"ANDI %X, %s, %s",UI16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x2e: util::stream_format(stream,"XORI %X, %s, %s",UI16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x2f: util::stream_format(stream,"MOVHI %X, %s, %s",UI16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x30: util::stream_format(stream,"LDB %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x31: util::stream_format(stream,"LDH %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x32: util::stream_format(stream,"Unk 0x32"); size=2; break;
case 0x33: util::stream_format(stream,"LDW %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x34: util::stream_format(stream,"STB %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
case 0x35: util::stream_format(stream,"STH %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
case 0x36: util::stream_format(stream,"Unk 0x36"); size=2; break;
case 0x37: util::stream_format(stream,"STW %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
case 0x38: util::stream_format(stream,"INB %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x39: util::stream_format(stream,"INH %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x3a: util::stream_format(stream,"CAXI %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x3b: util::stream_format(stream,"INW %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x3c: util::stream_format(stream,"OUTB %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
case 0x3d: util::stream_format(stream,"OUTH %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
case 0x3e:
switch((opc2&0xfc00)>>10)
{
case 0x0: util::stream_format(stream,"CMPF.S %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x2: util::stream_format(stream,"CVT.WS %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x3: util::stream_format(stream,"CVT.SW %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x4: util::stream_format(stream,"ADDF.S %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x5: util::stream_format(stream,"SUBF.S %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x6: util::stream_format(stream,"MULF.S %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x7: util::stream_format(stream,"DIVF.S %s, %s",GET1s(opc),GET2s(opc)); break;
case 0xb: util::stream_format(stream,"TRNC.SW %s, %s",GET1s(opc),GET2s(opc)); break;
default : util::stream_format(stream,"Unkf 0x%X",(opc2&0xfc00)>>10); break;
}
size=4;
break;
case 0x3f: util::stream_format(stream,"OUTW %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
default : size=2;
}
return size | flags | SUPPORTED;
}
static UINT32 opMOVi(void) // mov imm5,r2
{
SETREG(GET2,I5(OP));
return clkIF;
}
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
//bridge that will transform the message (image) from ROS code back to "image" code
sensor_msgs::CvBridge bridge;
fprintf(stderr, "\n call Back funtion \n");
//publish data (obstacle waypoints) back to the boat
//ros::NodeHandle n;
//std_msgs::Float32 xWaypoint_msg; // X coordinate obstacle message
//std_msgs::Float32 yWaypoint_msg; // Y coordinate obstacle message
//publish the waypoint data
//ros::Publisher waypoint_info_pub = n.advertise<std_msgs::Float32>("waypoint_info", 1000);
//ros::Publisher Ywaypoint_info_pub = n.advertise<std_msgs::Float32>("waypoint_info", 1000);
//std::stringstream ss;
/***********************************************************************/
//live image coming streamed straight from the boat's camera
IplImage* boatFront = bridge.imgMsgToCv(msg, "bgr8");
//The boat takes flipped images, so you need to flip them back to normal
cvFlip(boatFront, boatFront, 0);
IplImage* backUpImage = cvCloneImage(boatFront);
boatFront->origin = IPL_ORIGIN_TL; //sets image origin to top left corner
int X = boatFront->height;
int Y = boatFront->width;
//cout << "height " << X << endl;
//cout << "width " << Y << endl;
/*********************Image Filtering variables****************************/
//these images are used for segmenting objects from the overall background
//create a one channel image to convert from RGB to GRAY
IplImage* grayImage = cvCreateImage(cvGetSize(boatFront),IPL_DEPTH_8U,1);
//convert grayImage to binary (final step after converting from GRAY)
IplImage* bwImage = cvCreateImage(cvGetSize(grayImage),IPL_DEPTH_8U,1);
//variables used for the flood fill segmentation
CvPoint seed_point = cvPoint(boatFront->height/1.45,0); //not sure how this variable works
CvScalar color = CV_RGB(250,0,0);
CvMemStorage* grayStorage = NULL; //memory storage for contour sequence
CvSeq* contours = 0;
// get blobs and filter them using their area
//IplConvKernel* morphKernel = cvCreateStructuringElementEx(5, 5, 1, 1, CV_SHAPE_RECT, NULL);
//IplImage* original, *originalThr;
//IplImage* segmentated = cvCreateImage(cvGetSize(boatFront), 8, 1);
//unsigned int blobNumber = 0;
//IplImage* labelImg = cvCreateImage(cvGetSize(boatFront), IPL_DEPTH_LABEL, 1);
CvMoments moment;
/***********************************************************************/
//boat's edge distance from the camera. This is used for visual calibration
//to know the distance from the boat to the nearest obstacles.
//With respect to the mounted camera, distance is 21 inches (0.5334 m) side to side
//and 15 inches (0.381 m).
//float boatFrontDistance = 0.381; //distance in meters
//float boatSideDistance = 0.5334; //distance in meters
// These variables tell the distance from the center bottom of the image
// (the camera) to the square surrounding a the obstacle
float xObstacleDistance = 0.0;
float yObstacleDistance = 0.0;
float obstacleDistance = 0.0;
float obstacleHeading = 0.0;
int pixelsNumber = 50; //number of pixels for an n x n matrix and # of neighbors
const int arraySize = pixelsNumber;
const int threeArraySize = pixelsNumber;
//if n gets changed, then the algorithm might have to be
//recalibrated. Try to keep it constant
//these variables are used for the k nearest neighbors
//int accuracy;
//reponses for each of the classifications
float responseWaterH, responseWaterS, responseWaterV;
float responseGroundH, responseGroundS, responseGroundV;
float responseSkyH, responseSkyS, responseSkyV;
float averageHue = 0.0;
float averageSat = 0.0;
float averageVal = 0.0;
CvMat* trainClasses = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
CvMat* trainClasses2 = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
//CvMat sample = cvMat( 1, 2, CV_32FC1, _sample );
//used with the classifier
CvMat* trainClassesH = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
CvMat* trainClassesS = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
CvMat* trainClassesV = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
//CvMat* trainClasses2 = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
//CvMat sample = cvMat( 1, 2, CV_32FC1, _sample );
//used with the classifier
/*CvMat* nearestWaterH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestWaterS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestWaterV = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundV = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestSkyH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestSkyS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestSkyV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//Distance
CvMat* distanceWaterH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceWaterS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceWaterV = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceGroundH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceGroundS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceGroundV = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceSkyH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceSkyS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceSkyV = cvCreateMat(1, pixelsNumber, CV_32FC1); */
//these variables are use to traverse the picture by blocks of n x n pixels at
//a time.
//Index(0,0) does not exist, so make sure kj and ki start from 1 (in the
//right way, of course)
//x and y are the dimensions of the local patch of pixels
int x = (boatFront->height)/1.45;//(boatFront->height)/2.5 + 105;
int y = 0;
int skyX = 0;
int skyY = 0;
int row1 = 0;
int column1 = 0;
//these two variables are used in order to divide the grid in the
//resample segmentation part
int xDivisor = 200;
int yDivisor = 200;
//ground sample
//CvMat* groundTrainingHue = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//CvMat* groundTrainingSat = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//CvMat* groundTrainingVal = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//water sample
CvMat* waterTrainingHue = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* waterTrainingSat = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* waterTrainingVal = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//n x n sample patch taken from the picture
CvMat* sampleHue = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* sampleSat = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* sampleVal = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* resampleHue = cvCreateMat(boatFront->height/xDivisor,boatFront->width/yDivisor,CV_32FC1);
CvMat* resampleSat = cvCreateMat(boatFront->height/xDivisor,boatFront->width/yDivisor,CV_32FC1);
CvMat* resampleVal = cvCreateMat(boatFront->height/xDivisor,boatFront->width/yDivisor,CV_32FC1);
int xDiv = 20;
int yDiv = 20;
CvMat* resampleHue2 = cvCreateMat(boatFront->height/xDiv,boatFront->width/yDiv,CV_32FC1);
CvMat* resampleSat2 = cvCreateMat(boatFront->height/xDiv,boatFront->width/yDiv,CV_32FC1);
CvMat* resampleVal2 = cvCreateMat(boatFront->height/xDiv,boatFront->width/yDiv,CV_32FC1);
//sky training sample
CvMat* skyTrainingHue = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* skyTrainingSat = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* skyTrainingVal = cvCreateMat(arraySize,arraySize,CV_32FC1);
//initialize each matrix element to zero for ease of use
//cvZero(groundTrainingHue);
//cvZero(groundTrainingSat);
//cvZero(groundTrainingVal);
cvZero(waterTrainingHue);
cvZero(waterTrainingSat);
cvZero(waterTrainingVal);
cvZero(sampleHue);
cvZero(sampleSat);
cvZero(sampleVal);
cvZero(resampleHue);
cvZero(resampleSat);
cvZero(resampleVal);
cvZero(skyTrainingHue);
cvZero(skyTrainingSat);
cvZero(skyTrainingVal);
//Stores the votes for each channel (whether it belongs to water or not
//1 is part of water, 0 not part of water
//if sum of votes is bigger than 1/2 the number of elements, then it belongs to water
int votesSum = 0;
int comparator[3]; //used when only three votes are needed
//int comparatorTwo [3][3]; //used when six votes are needed
//initial sum of votes is zero
//Error if initialize both matrices inside a single for loop. Dont know why
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
/***********************************************************************/
//Convert from RGB to HSV to control the brightness of the objects.
//work with reflexion
/*Sky recognition. Might be useful for detecting reflexion on the water. If
the sky is detected, and the reflection has the same characteristics of
something below the horizon, that "something" might be water. Assume sky
wont go below the horizon
*/
//convert from RGB to HSV
cvCvtColor(boatFront, boatFront, CV_BGR2HSV);
cvCvtColor(backUpImage, backUpImage, CV_BGR2HSV);
HsvImage I(boatFront);
HsvImage IBackUp(backUpImage);
//Sky detection
/*
for (int i=0; i<boatFront->height;i++)
{
for (int j=0; j<boatFront->width;j++)
{
//if something is bright enough, consider it sky and store the
//value. HSV values go from 0 to 180 ... RGB goes from 0 to 255
if (((I[i][j].v >= 180) && (I[i][j].s <= 16)))
// && ((I[i][j].h >=10)))) //&& (I[i][j].h <= 144))))
{
//The HSV values vary between 0 and 1
cvmSet(skyTrainingHue,skyX,skyY,I[i][j].h);
cvmSet(skyTrainingSat,skyX,skyY,I[i][j].s);
cvmSet(skyTrainingVal,skyX,skyY,I[i][j].v);
//I[i][j].h = 0.3*180; //H (color)
//I[i][j].s = 0.3*180; //S (color intensity)
//I[i][j].v = 0.6*180; //V (brightness)
if (skyY == pixelsNumber-1)
{
if (skyX == pixelsNumber-1)
skyX = 1;
else
skyX = skyX + 1;
skyY = 1;
}
else
skyY = skyY + 1;
}
}
}
/***********************************************************************/
//offline input pictures. Samples of water properties are taken from these
//pictures to get a range of values for H, S, V that will be stored into a
//pre-defined classifier
IplImage* imageSample1 = cvLoadImage("20110805_032255.jpg");
cvSetImageROI(imageSample1, cvRect(0,0,imageSample1->height/0.5,imageSample1->width/1.83));
cvCvtColor(imageSample1, imageSample1, CV_BGR2HSV);
HsvImage I1(imageSample1);
IplImage* imageSample2 = cvLoadImage("20110805_032257.jpg");
cvCvtColor(imageSample2, imageSample2, CV_BGR2HSV);
HsvImage I2(imageSample2);
IplImage* imageSample3 = cvLoadImage("20110805_032259.jpg");
cvCvtColor(imageSample3, imageSample3, CV_BGR2HSV);
HsvImage I3(imageSample3);
IplImage* imageSample4 = cvLoadImage("20110805_032301.jpg");
cvCvtColor(imageSample4, imageSample4, CV_BGR2HSV);
HsvImage I4(imageSample4);
IplImage* imageSample5 = cvLoadImage("20110805_032303.jpg");
cvCvtColor(imageSample5, imageSample5, CV_BGR2HSV);
HsvImage I5(imageSample5);
IplImage* imageSample6 = cvLoadImage("20110805_032953.jpg");
cvCvtColor(imageSample6, imageSample6, CV_BGR2HSV);
HsvImage I6(imageSample6);
IplImage* imageSample7 = cvLoadImage("20110805_032955.jpg");
cvCvtColor(imageSample7, imageSample7, CV_BGR2HSV);
HsvImage I7(imageSample7);
IplImage* imageSample8 = cvLoadImage("20110805_032957.jpg");
cvCvtColor(imageSample8, imageSample8, CV_BGR2HSV);
HsvImage I8(imageSample8);
IplImage* imageSample9 = cvLoadImage("20110805_032959.jpg");
cvCvtColor(imageSample9, imageSample9, CV_BGR2HSV);
HsvImage I9(imageSample9);
IplImage* imageSample10 = cvLoadImage("20110805_033001.jpg");
cvCvtColor(imageSample10, imageSample10, CV_BGR2HSV);
HsvImage I10(imageSample10);
IplImage* imageSample11 = cvLoadImage("20110805_033009.jpg");
cvCvtColor(imageSample11, imageSample11, CV_BGR2HSV);
HsvImage I11(imageSample11);
IplImage* imageSample12 = cvLoadImage("20110805_033011.jpg");
cvCvtColor(imageSample12, imageSample12, CV_BGR2HSV);
HsvImage I12(imageSample12);
for (int i=0; i < threeArraySize; i++)
{
for (int j=0; j < arraySize; j++)
{
row1 = ceil(X/1.2866)+ceil(X/5.237)+i+ceil(-X/3.534545455) + ceil(X/4.8);
column1 = ceil(Y/7.0755)+ceil(Y/21.01622)+j+ceil(X/1.495384615);
averageHue = (I1[row1][column1].h + I2[row1][column1].h + I3[row1][column1].h + I4[row1][column1].h + I5[row1][column1].h +
I6[row1][column1].h + I7[row1][column1].h + I8[row1][column1].h + I9[row1][column1].h + I10[row1][column1].h + I11[row1][column1].h + I12[row1][column1].h) / 12;
averageSat = (I1[row1][column1].s + I2[row1][column1].s + I3[row1][column1].s + I4[row1][column1].s + I5[row1][column1].s +
I6[row1][column1].s + I7[row1][column1].s + I8[row1][column1].s + I9[row1][column1].s + I10[row1][column1].s + I11[row1][column1].s + I12[row1][column1].s) / 12;
averageVal = (I1[row1][column1].v + I2[row1][column1].v + I3[row1][column1].v + I4[row1][column1].v + I5[row1][column1].v +
I6[row1][column1].v + I7[row1][column1].v + I8[row1][column1].v + I9[row1][column1].v + I10[row1][column1].v + I11[row1][column1].v + I12[row1][column1].v) / 12;
//water patch sample (n X n matrix)
cvmSet(waterTrainingHue,i,j,averageHue);
cvmSet(waterTrainingSat,i,j,averageSat);
cvmSet(waterTrainingVal,i,j,averageVal);
//patch is red (this is for me to know where the ground patch sample is)
//I[row1][column1].h = 0;
//I[row1][column1].s = 255;
//I[row1][column1].v = 255;
}
}
//creating a training sample from the an image taken on the fly
row1 = 0;
column1 = 0;
for (int i=0; i<pixelsNumber; i++)
{
for (int j=0; j<pixelsNumber; j++)
{
row1 = ceil(X/1.2866)+ceil(X/5.237)+i+ceil(-X/3.534545455) + ceil(X/4.8);
column1 = ceil(Y/7.0755)+ceil(Y/21.01622)+j+ceil(X/1.495384615);
cvmSet(trainClassesH,i,0,I[row1][column1].h);
cvmSet(trainClassesS,i,0,I[row1][column1].s);
cvmSet(trainClassesV,i,0,I[row1][column1].v);
}
}
//order the water samples in ascending order on order to know a range
cvSort(waterTrainingHue, waterTrainingHue, CV_SORT_ASCENDING);
cvSort(waterTrainingSat, waterTrainingSat, CV_SORT_ASCENDING);
cvSort(waterTrainingVal, waterTrainingVal, CV_SORT_ASCENDING);
// find the maximum and minimum values in the array to create a range
int maxH = cvmGet(waterTrainingHue,0,0);
int maxS = cvmGet(waterTrainingSat,0,0);
int maxV = cvmGet(waterTrainingVal,0,0);
int minH = cvmGet(waterTrainingHue,0,0);
int minS = cvmGet(waterTrainingSat,0,0);
int minV = cvmGet(waterTrainingVal,0,0);
for (int i=0; i < threeArraySize; i++)
{
for (int j=0; j < arraySize; j++)
{
if (cvmGet(waterTrainingHue,i,j) > maxH)
maxH = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingSat,i,j) > maxS)
maxS = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingVal,i,j) > maxV)
maxV = cvmGet(waterTrainingVal,i,j);
if (cvmGet(waterTrainingHue,i,j) < minH)
minH = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingSat,i,j) < minS)
minS = cvmGet(waterTrainingSat,i,j);
if (cvmGet(waterTrainingVal,i,j) < minV)
minV = cvmGet(waterTrainingVal,i,j);
}
}
//cout << "Min Value in the range: " << endl;
//cout << minH << endl;
//cout << minS << endl;
//cout << minV << endl;
//cout << "Max Value in the range: " << endl;
//cout << maxH << endl;
//cout << maxS << endl;
//cout << maxV << endl << endl;
/*********** Main loop. It traverses through the picture**********/
/**********************************************************************/
//Ignore unused parts of the image and convert them to black
// for (int i=0; i<boatFront->height/1.45 - 1;i++)
//{
// for (int j=0; j<Y-1;j++)
// {
// I[i][j].h = 0;
// I[i][j].s = 0;
// I[i][j].v = 0;
// }
// }
/*********************************************************************
// Use nearest neighbors to increase accuracy
skyX = 0;
skyY = 0;
while (x < X-1)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i<6;i++)
{
column1 = y+i;
if (column1 > Y-1)
column1 = Y-1;
cvmSet(sampleHue,0,i,I[x][column1].h);
cvmSet(sampleSat,0,i,I[x][column1].s);
cvmSet(sampleVal,0,i,I[x][column1].v);
}
//Find the shortest distance between a pixel and the neighbors from each of
//the training samples (sort of inefficient, but might do the job...sometimes)
//HSV for water sample
// learn classifier
//CvKNearest knn(trainData, trainClasses, 0, false, itemsNumber);
CvKNearest knnWaterHue(waterTrainingHue, trainClassesH, 0, false, pixelsNumber);
CvKNearest knnWaterSat(waterTrainingSat, trainClassesS, 0, false, pixelsNumber);
CvKNearest knnWaterVal(waterTrainingVal, trainClassesV, 0, false, pixelsNumber);
//HSV for ground sample
//CvKNearest knnGroundHue(groundTrainingHue, trainClasses2, 0, false, pixelsNumber);
//CvKNearest knnGroundSat(groundTrainingSat, trainClasses2, 0, false, pixelsNumber);
//CvKNearest knnGroundVal(groundTrainingVal, trainClasses2, 0, false, pixelsNumber);
//HSV for sky sample
//if (cvmGet(skyTrainingHue,0,0)!=0.0 && cvmGet(skyTrainingSat,0,0)!=0.0 && cvmGet(skyTrainingVal,0,0)!=0.0)
//{
// CvKNearest knnSkyHue(skyTrainingHue, trainClasses, 0, false, pixelsNumber);
// CvKNearest knnSkySat(skyTrainingSat, trainClasses, 0, false, pixelsNumber);
// CvKNearest knnSkyVal(skyTrainingVal, trainClasses, 0, false, pixelsNumber);
//}
//scan nearest neighbors to each pixel
responseWaterH = knnWaterHue.find_nearest(sampleHue,pixelsNumber,0,0,nearestWaterH,0);
responseWaterS = knnWaterSat.find_nearest(sampleSat,pixelsNumber,0,0,nearestWaterS,0);
responseWaterV = knnWaterVal.find_nearest(sampleVal,pixelsNumber,0,0,nearestWaterV,0);
//responseGroundH = knnGroundHue.find_nearest(sampleHue,pixelsNumber,0,0,nearestGroundH,0);
//responseGroundS = knnGroundSat.find_nearest(sampleSat,pixelsNumber,0,0,nearestGroundS,0);
//responseGroundV = knnGroundVal.find_nearest(sampleVal,pixelsNumber,0,0,nearestGroundV,0);
//for (int i=0;i<pixelsNumber;i++)
//{
for (int j=0;j<pixelsNumber;j++)
{
if ((nearestWaterH->data.fl[j] == responseWaterH) )//&& (nearestWaterH->data.fl[j] == responseWaterH + 5))
// mark water samples as green
comparator[0] = 1;
else
comparator[0] = 0;
if ((nearestWaterS->data.fl[j] == responseWaterS) )//&& (nearestWaterS->data.fl[j] < responseWaterS + 5))
//mark water samples as green
comparator[1] = 1;
else
comparator[1] = 0;
if ((nearestWaterV->data.fl[j] == responseWaterV) )//&& (nearestWaterV->data.fl[j] < responseWaterV + 5))
//mark water samples as green
comparator[2] = 1;
else
comparator[2] = 0;
// similar sky pixels on the water
//count votes
for (int i3=0; i3 < 3; i3++)
votesSum = votesSum + comparator[i3];
if (votesSum > 1)
{
I[x][y-6+j].h = 0;
I[x][y-6+j].s = 255;
I[x][y-6+j].v = 255;
}
votesSum = 0;
}
}
if (y < Y-1)
//5 use to be equal to pixelsNumber-1.
y = y + 5;
if (y > Y-1)
y = Y-1;
else if (y == Y-1)
{
//5 use to be equal to pixelsNumber-1
x = x + 1;
y = 0;
}
// ix = 0;
}
/*********************************************************************/
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
//int counter = 0;
column1 = 0;
row1 = 0;
x = boatFront->height/1.45;
y = 0;
while (x < X-1)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i<6;i++)
{
column1 = y+i;
if (column1 > Y-1)
column1 = Y-1;
cvmSet(sampleHue,0,i,I[x][column1].h);
cvmSet(sampleSat,0,i,I[x][column1].s);
cvmSet(sampleVal,0,i,I[x][column1].v);
}
for (int i=0;i<6;i++)
{
for (int j=0;j<6;j++)
{
if ((minH < cvmGet(sampleHue,0,j)) && (maxH > cvmGet(sampleHue,0,j)))
//mark water samples as green
comparator[0] = 1;
else
comparator[0] = 0;
if ((minS < cvmGet(sampleSat,0,j)) && (maxS > cvmGet(sampleSat,0,j)))
//mark water samples as green
comparator[1] = 1;
else
comparator[1] = 0;
if ((minV < cvmGet(sampleVal,0,j)) && (maxV > cvmGet(sampleVal,0,j)))
//mark water samples as red
comparator[2] = 1;
else
comparator[2] = 0;
//count votes
for (int i3=0; i3 < 3; i3++)
votesSum = votesSum + comparator[i3];
if (votesSum > 1)
{
//use the known water samples as new training data
if((i<boatFront->height/xDivisor) && (j<boatFront->width/yDivisor))
{
cvmSet(resampleHue,i,j,cvmGet(sampleHue,0,j));
cvmSet(resampleSat,i,j,cvmGet(sampleSat,0,j));
cvmSet(resampleVal,i,j,cvmGet(sampleVal,0,j));
}
//6 use to be equal to pixelsNumber.
I[x][y-6+j].h = 0;
I[x][y-6+j].s = 255;
I[x][y-6+j].v = 255;
}
votesSum = 0;
}
}
if (y < Y-1)
//5 use to be equal to pixelsNumber-1.
y = y + 5;
if (y > Y-1)
y = Y-1;
else if (y == Y-1)
{
//5 use to be equal to pixelsNumber-1
x = x + 1;
y = 0;
}
//ix = 0;
}
/***************Deal with reflection*****************/
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
//int counter = 0;
votesSum = 0;
column1 = 0;
row1 = 0;
x = boatFront->height/1.45;
y = 0;
while (x < X-1)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i<6;i++)
{
column1 = y+i;
if (column1 > Y-1)
column1 = Y-1;
cvmSet(sampleHue,0,i,I[x][column1].h);
cvmSet(sampleSat,0,i,I[x][column1].s);
cvmSet(sampleVal,0,i,I[x][column1].v);
}
for (int i=0;i<6;i++)
{
for (int j=0;j<6;j++)
{
if ((minH < cvmGet(sampleHue,0,j)) && (maxH > cvmGet(sampleHue,0,j)))
//mark water samples as green
comparator[0] = 1;
else
comparator[0] = 0;
if ((0.8*255 > cvmGet(sampleSat,0,j)))// && (maxS < cvmGet(sampleSat,0,j)))
//mark water samples as green
comparator[1] = 1;
else
comparator[1] = 0;
if ((0.6*255 < cvmGet(sampleVal,0,j)))// || (maxV < cvmGet(sampleVal,0,j)))
//mark water samples as green
comparator[2] = 1;
else
comparator[2] = 0;
//count votes
for (int i3=0; i3 < 3; i3++)
votesSum = votesSum + comparator[i3];
if (votesSum > 1)
{
//use the known water samples as new training data
if((i<boatFront->height/xDivisor) && (j<boatFront->width/yDivisor))
{
cvmSet(resampleHue,i,j,cvmGet(sampleHue,0,j));
cvmSet(resampleSat,i,j,cvmGet(sampleSat,0,j));
cvmSet(resampleVal,i,j,cvmGet(sampleVal,0,j));
}
//6 use to be equal to pixelsNumber.
I[x][y-6+j].h = 0;
I[x][y-6+j].s = 255;
I[x][y-6+j].v = 255;
}
votesSum = 0;
}
}
if (y < Y-1)
//5 use to be equal to pixelsNumber-1.
y = y + 5;
if (y > Y-1)
y = Y-1;
else if (y == Y-1)
{
//5 use to be equal to pixelsNumber-1
x = x + 1;
y = 0;
}
//ix = 0;
}
/**********Resample the entire patch**********/
/*********find a new min and max for a new sample range*************/
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
//int counter = 0;
votesSum = 0;
column1 = 0;
row1 = 0;
x = boatFront->height/1.45;
y = 0;
maxH = cvmGet(resampleHue,0,0);
maxS = cvmGet(resampleSat,0,0);
maxV = cvmGet(resampleVal,0,0);
minH = cvmGet(resampleHue,0,0);
minS = cvmGet(resampleSat,0,0);
minV = cvmGet(resampleVal,0,0);
for (int i=0; i < boatFront->height/xDivisor; i++)
{
for (int j=0; j < boatFront->width/yDivisor; j++)
{
if (cvmGet(resampleHue,i,j) > maxH)
maxH = cvmGet(resampleHue,i,j);
if (cvmGet(resampleSat,i,j) > maxS)
maxS = cvmGet(resampleSat,i,j);
if (cvmGet(resampleVal,i,j) > maxV)
maxV = cvmGet(resampleVal,i,j);
if (cvmGet(resampleHue,i,j) < minH)
minH = cvmGet(resampleHue,i,j);
if (cvmGet(resampleSat,i,j) < minS)
minS = cvmGet(resampleSat,i,j);
if (cvmGet(resampleVal,i,j) < minV)
minV = cvmGet(resampleVal,i,j);
}
}
while (x < X-1)
{
for (int i=0;i<6;i++)
{
for (int j=0;j<6;j++)
{
if ((minH < I[x][y-6+j].h) && (maxH > I[x][y-6+j].h))
//mark water samples as red
I[x][y-6+j].h = 0;
else
comparator[0] = 0;
if ((minS < I[x][y-6+j].s) && (maxS > I[x][y-6+j].s))
//mark water samples as red
I[x][y-6+j].s = 255;
else
comparator[1] = 0;
if ((minV < I[x][y-6+j].v) && (maxV > I[x][y-6+j].v))
//mark water samples as red
I[x][y-6+j].v = 255;
}
}
if (y < Y-1)
//5 use to be equal to pixelsNumber-1.
y = y + 5;
if (y > Y-1)
y = Y-1;
else if (y == Y-1)
{
//5 use to be equal to pixelsNumber-1
x = x + 1;
y = 0;
}
}
//cout << "Sample data from current images" << endl;
//for (int i = 0; i<20;i++)
//{
// cout << "HUE: " << cvmGet(sampleHue,0,i) << endl;
// cout << "Saturation: " << cvmGet(sampleSat,0,i) << endl;
// cout << "Value: " << cvmGet(sampleVal,0,i) << endl;
//}
//traverse through the image one more time, divide the image in grids of
// 500x500 pixels, and see how many pixels of water are in each grid. If
// most of the pixels are labeled water, then mark all the other pixels
// as water as well
//int counter = 0;
votesSum = 0;
column1 = 0;
row1 = 0;
x = boatFront->height/1.45;
y = 0;
/***************Divide the picture in cells for filtering**********/
while (x < X-1)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i < boatFront->height/xDivisor; i++)
{
for(int j = 0; j < boatFront->width/yDivisor; j++)
{
cvmSet(resampleHue2,i,j,I[x+i][y+j].h);
cvmSet(resampleSat2,i,j,I[x+i][y+j].s);
cvmSet(resampleVal2,i,j,I[x+i][y+j].v);
if(cvmGet(resampleHue2,i,j)==0 && cvmGet(resampleSat2,i,j)==255 && cvmGet(resampleVal2,i,j)==255)
{
votesSum++;
}
}
}
if (votesSum > (((boatFront->height/xDivisor)*(boatFront->width/yDivisor))*(4/5)))
{
// if bigger than 4/5 the total number of pixels in a square, then consider the entire thing as water
// We might need to use other smaller quantities (like 5/6 maybe?)
for (int i = 0; i < boatFront->height/xDivisor;i++)
{
for (int j = 0; j < boatFront->width/yDivisor; j++)
{
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = y+j;
if (column1 > Y-1)
column1 = Y-1;
I[row1][column1].h = 0;
I[row1][column1].s = 255;
I[row1][column1].v = 255;
}
}
}
else
{
// If not water, eliminate all red pixels and turn those pixels
// back to the original color
for (int i = 0; i < boatFront->height/xDivisor;i++)
{
for (int j = 0; j < boatFront->width/yDivisor; j++)
{
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = y+j;
if (column1 > Y-1)
column1 = Y-1;
I[row1][column1].h = IBackUp[row1][column1].h;//255;//IBackUp[row1][column1].h;
I[row1][column1].s = IBackUp[row1][column1].s;//255;//IBackUp[row1][column1].s;
I[row1][column1].v = IBackUp[row1][column1].v;//255;//IBackUp[row1][column1].v;
}
}
}
y = y + boatFront->width/xDivisor;
if (y > Y-1)
{
x = x + boatFront->height/yDivisor;
y = 0;
}
votesSum = 0;
}
/********************Isolate obstacles************************/
votesSum = 0;
int paint = 0;
column1 = 0;
row1 = 0;
x = boatFront->height/1.45;
y = 0;
xDiv = 20;
yDiv = 20;
/***************Divide the picture in cells for filtering**********/
// Small pixel areas (noise) are going to be eliminated from the picture
// living only the big obstacles
while (x < X-2)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i < boatFront->height/xDiv; i++)
{
for(int j = 0; j < boatFront->width/yDiv; j++)
{
row1 = x + i;
if (row1 > X-2)
row1 = X-2;
column1 = y+j;
if (column1 > Y-1)
column1 = Y-1;
cvmSet(resampleHue2,i,j,I[row1][column1].h);
cvmSet(resampleSat2,i,j,I[row1][column1].s);
cvmSet(resampleVal2,i,j,I[row1][column1].v);
if(cvmGet(resampleHue2,i,j)==0 && cvmGet(resampleSat2,i,j)==255 && cvmGet(resampleVal2,i,j)==255)
{
votesSum++;
}
}
}
if (votesSum > (((boatFront->height/xDiv)*(boatFront->width/yDiv))*(4.5/5)))
{
// if bigger than 4/5 the total number of pixels in a square, then consider the entire thing as water
// We might need to use other smaller quantities (like 5/6 maybe?)
for (int i = 0; i < boatFront->height/xDiv;i++)
{
for (int j = 0; j < boatFront->width/yDiv; j++)
{
row1 = x + i;
if (row1 > X-2)
row1 = X-2;
column1 = y+j;
if (column1 > Y-1)
column1 = Y-1;
I[row1][column1].h = 0;
I[row1][column1].s = 255;
I[row1][column1].v = 255;
}
}
}
else
{
int count = 0;
// If not water, eliminate all red pixels and turn those pixels
// back to the original color
for (int i = 0; i < boatFront->height/xDiv;i++)
{
for (int j = 0; j < boatFront->width/yDiv; j++)
{
row1 = x + i;
if (row1 > X-2)
row1 = X-2;
column1 = y+j;
if (column1 > Y-1)
column1 = Y-1;
I[row1][column1].h = IBackUp[row1][column1].h;//255;
I[row1][column1].s = IBackUp[row1][column1].s;//255;
I[row1][column1].v = IBackUp[row1][column1].v;//255;
// count++;
}
}
}
y = y + boatFront->width/yDiv;
if (y > Y-1)
{
x = x + boatFront->height/xDiv;
if (x > X-2)
x = X-2;
y = 0;
}
votesSum = 0;
}
/****************Find Obstacles boundaries*********************************/
if( grayStorage == NULL )
{
grayStorage = cvCreateMemStorage(0);
}
else
{
cvClearMemStorage(grayStorage);
}
backUpImage = cvCloneImage(boatFront);
//convert from HSV to RGB
cvCvtColor(boatFront, boatFront, CV_HSV2BGR);
cvCvtColor(backUpImage, backUpImage, CV_HSV2BGR);
//do flood fill for obstacles
cvFloodFill( backUpImage, seed_point, color, cvScalarAll(255), cvScalarAll(2), NULL, 8, NULL);
//convert to to gray to do more obstacle segmentation
cvCvtColor(backUpImage, grayImage, CV_BGR2GRAY);
//convert to binary
cvThreshold(grayImage, bwImage, 100, 255, CV_THRESH_BINARY | CV_THRESH_OTSU);
//eliminate small unnecessary pixel areas
//bwImage is a pointer, so no need to reuse findCountours
int findCountours = bwareaopen_(bwImage, 100);
//find contours of obstacles in image
cvFindContours(bwImage, grayStorage, &contours);
cvZero( bwImage ); //redraw clean contours
for( CvSeq* c=contours; c!=NULL; c=c->h_next)
{
cvDrawContours(bwImage, c, cvScalarAll(255), cvScalarAll(255), 8);
cout << "Contour area: " << cvContourArea(c, CV_WHOLE_SEQ) << endl; //area in pixels
//find the x,y coordinate of the center of a contour
cvMoments(c, &moment, 0);
//centroid/moment of the contour/obstacle
//cout << "Contour center: " << moment.m10/moment.m00 << ", " << moment.m01/moment.m00 << endl;
//The distance formula calculated by plotting points is given by:
/*********** distance = 0.1622208546*pow(1.0186851612,pixels) *****************/
/*********** pixel = 87.0413255*pow(distance,0.4062956891) *****************/
//These formulas only work for 640X480 images
// x,y coordinates of the obstacle from the bottom center of the image
//Ignore everything less than 0.3 meters apart (anything too close to the boat)
//if ((X - (row1 -(boatFront->height/xDiv)/2)) > (87.0413255*pow(0.3,0.4062956891)))
//{
xObstacleDistance = 0.1622208546*pow(1.0186851612,X - (moment.m10/moment.m00));
if (xObstacleDistance == 0.0) //try to ignore obstacle that are too close
xObstacleDistance = 0.01; //robot shall tell operator if there is
//a problem with a close by obstacle
yObstacleDistance = 0.1622208546*pow(1.0186851612,Y/2 - (moment.m01/moment.m00));
//obstacle distance
obstacleDistance = sqrt(pow(xObstacleDistance,2) + pow(yObstacleDistance,2));
//obstacle heading
obstacleHeading = tan((yObstacleDistance/xObstacleDistance)*PI/180);
cout << "Obstacle polar coordinates: " << endl;
cout << "x: " << xObstacleDistance << " Y: " << yObstacleDistance << endl;
cout << "Distance (meters) " << obstacleDistance << endl;
cout << "Direction (degrees): " << obstacleHeading << endl << endl;
//}
}
/**************************************************************************/
try
{
//fprintf(stderr,"\n boatFront\n");
cvShowImage("Boat Front", boatFront);
//cvShowImage("Color Segment", backUpImage);
//cvShowImage("Obstacles", bwImage);
}
catch (sensor_msgs::CvBridgeException& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
}
