__LOG_T
ENTF90(SAME_TYPE_AS, same_type_as)
(void *ab, OBJECT_DESC *ad, void *bb, OBJECT_DESC *bd, __INT_T flag, ...)
{
OBJECT_DESC *t1 = ad, *t2 = bd;
TYPE_DESC *atd, *btd;
if (!ad || !bd)
return 0;
if (flag) {
va_list va;
int is_unl_poly = 0;
va_start(va, flag);
if (flag & (ARG1_PTR | ARG1_ALLOC)) {
OBJECT_DESC *vatd = va_arg(va, OBJECT_DESC *);
if (!((flag & ARG1_PTR) &&
ENTFTN(ASSOCIATED, associated)(ab, (F90_Desc *)ad, 0, 0)) &&
!I8(__fort_allocated)(ab)) {
t1 = vatd;
is_unl_poly |= t1->tag == __POLY && t1->baseTag == __POLY;
}
}
if (flag & (ARG2_PTR | ARG2_ALLOC)) {
OBJECT_DESC *vatd = va_arg(va, OBJECT_DESC *);
if (!((flag & ARG2_PTR) &&
ENTFTN(ASSOCIATED, associated)(bb, (F90_Desc *)bd, 0, 0)) &&
!I8(__fort_allocated)(bb)) {
t2 = vatd;
is_unl_poly |= t2->tag == __POLY && t2->baseTag == __POLY;
}
}
static bool swap3d_ghosted(Mesh* mesh) {
auto comm = mesh->comm();
auto edges_are_cands = mesh->get_array<I8>(EDGE, "candidate");
mesh->remove_tag(EDGE, "candidate");
auto cands2edges = collect_marked(edges_are_cands);
if (has_fixed_momentum_velocity(mesh)) {
auto keep_cands = filter_swap_momentum_velocity(mesh, cands2edges);
filter_swap(keep_cands, &cands2edges);
}
auto cand_quals = Reals();
auto cand_configs = Read<I8>();
swap3d_qualities(mesh, cands2edges, &cand_quals, &cand_configs);
auto edge_configs =
map_onto(cand_configs, cands2edges, mesh->nedges(), I8(-1), 1);
auto keep_cands = filter_swap_improve(mesh, cands2edges, cand_quals);
filter_swap(keep_cands, &cands2edges, &cand_quals);
if (comm->reduce_and(cands2edges.size() == 0)) return false;
edges_are_cands = mark_image(cands2edges, mesh->nedges());
auto edge_quals = map_onto(cand_quals, cands2edges, mesh->nedges(), -1.0, 1);
auto edges_are_keys = find_indset(mesh, EDGE, edge_quals, edges_are_cands);
Graph edges2cav_elems;
edges2cav_elems = mesh->ask_up(EDGE, mesh->dim());
mesh->add_tag(EDGE, "key", 1, OMEGA_H_DONT_TRANSFER, OMEGA_H_DONT_OUTPUT,
edges_are_keys);
mesh->add_tag(EDGE, "config", 1, OMEGA_H_DONT_TRANSFER, OMEGA_H_DONT_OUTPUT,
edge_configs);
auto keys2edges = collect_marked(edges_are_keys);
set_owners_by_indset(mesh, EDGE, keys2edges, edges2cav_elems);
return true;
}
void code2(int mode, LPSYMBOL s1, LPSYMBOL s2)
{
switch (mode) {
case AM_R8:
R8(s1->instr, s2->val8);
break;
case AM_R16:
R16(s1->instr, s2->val8);
break;
case AM_M8:
M8(s1->instr, s2->val8);
break;
case AM_M16:
M16(s1->instr, s2->val8);
break;
case AM_A8:
A8(s1->instr, s2->val16);
break;
case AM_A16:
A16(s1->instr, s2->val16);
break;
case AM_I16:
I16(s1->instr, s2->val16);
break;
case AM_I8:
I8(s1->instr, s2->val8);
break;
default:
break;
}
}
static void I8(__io_read)(fio_parm *z)
{
DECL_HDR_PTRS(ac);
char *adr, *buf;
int i, ioproc, owner, pc[MAXDIMS], str;
ac = z->ac;
adr = I8(__fort_local_address)(z->ab, ac, z->index);
str = z->str;
#if defined(DEBUG)
if (__fort_test & DEBUG_HFIO) {
printf("%d __io_read index=", GET_DIST_LCPU);
I8(__fort_show_index)(F90_RANK_G(ac), z->index);
printf(" cnt=%d str=%d adr=%x\n", z->cnt, str, adr);
}
#endif
if (LOCAL_MODE) {
if (!z->stat)
z->stat = z->f90io_rw(F90_KIND_G(ac), z->cnt, z->str * F90_LEN_G(ac), adr,
F90_LEN_G(ac));
return;
}
ioproc = GET_DIST_IOPROC;
buf = __fort_getgbuf(z->cnt * F90_LEN_G(ac));
if (GET_DIST_LCPU == ioproc) {
if (adr == NULL) {
adr = buf;
str = 1;
}
if (!z->stat)
z->stat = z->f90io_rw(F90_KIND_G(ac), z->cnt, str * F90_LEN_G(ac), adr,
F90_LEN_G(ac));
owner = I8(__fort_owner)(ac, z->index);
for (i = z->repl.ndim; --i >= 0;)
pc[i] = 0;
while (owner >= 0) {
if (owner != GET_DIST_LCPU)
__fort_rsendl(owner, adr, z->cnt, str, F90_KIND_G(ac), F90_LEN_G(ac));
owner = I8(__fort_next_owner)(ac, &z->repl, pc, owner);
}
} else if (adr != NULL)
__fort_rrecvl(ioproc, adr, z->cnt, str, F90_KIND_G(ac), F90_LEN_G(ac));
}
static void I8(__io_write)(fio_parm *z)
{
DECL_HDR_PTRS(ac);
char *adr, *buf;
int ioproc, owner, str;
ac = z->ac;
adr = I8(__fort_local_address)(z->ab, ac, z->index);
str = z->str;
#if defined(DEBUG)
if (__fort_test & DEBUG_HFIO) {
printf("%d __io_write index=", GET_DIST_LCPU);
I8(__fort_show_index)(F90_RANK_G(ac), z->index);
printf(" cnt=%d str=%d adr=%x\n", z->cnt, str, adr);
}
#endif
if (!z->stat)
z->stat = z->f90io_rw(F90_KIND_G(ac), z->cnt, str * F90_LEN_G(ac), adr,
F90_LEN_G(ac));
}
int I8(__fortio_main)(char *ab, /* base address */
F90_Desc *ac, /* array descriptor */
int rw, /* 0 => read, 1 => write */
int (*f90io_rw)()) /* f90io function */
{
int ioproc, size_of_kind;
fio_parm z;
z.stat = 0;
if (F90_TAG_G(ac) != __DESC) { /* scalar */
dtype kind = TYPEKIND(ac);
#if defined(DEBUG)
if (kind == __STR || kind == __DERIVED)
__fort_abort("__fortio_main: character or derived type not handled");
#endif
ioproc = GET_DIST_IOPROC;
size_of_kind = GET_DIST_SIZE_OF(kind);
if (LOCAL_MODE || GET_DIST_LCPU == ioproc)
z.stat = f90io_rw(kind, 1, 1, ab, size_of_kind);
if (!rw && !LOCAL_MODE) /* if global read... */
__fort_rbcstl(ioproc, ab, 1, 1, kind, size_of_kind);
return __fortio_stat_bcst(&z.stat);
}
if (F90_GSIZE_G(ac) <= 0)
return 0; /* zero-size array */
z.ab = ab + DIST_SCOFF_G(ac) * F90_LEN_G(ac);
z.ac = ac;
z.f90io_rw = f90io_rw;
z.fio_rw = rw ? I8(__io_write) : I8(__io_read);
if (!rw && !LOCAL_MODE) /* if global read... */
I8(__fort_describe_replication)(ac, &z.repl);
if (F90_RANK_G(ac) > 0)
I8(__fortio_loop)(&z, F90_RANK_G(ac));
else {
z.cnt = 1;
z.str = 1;
z.fio_rw(&z);
}
return __fortio_stat_bcst(&z.stat);
}
static bool probe_update_protocol(probe_t * probe)
{
size_t i, num_layers = probe_get_num_layers(probe);
layer_t * layer,
* prev_layer;
for (i = 0, prev_layer = NULL; i < num_layers; i++, prev_layer = layer) {
layer = probe_get_layer(probe, i);
if (layer->protocol && prev_layer) {
// Update 'protocol' field (if any)
layer_set_field_and_free(layer, I8("protocol", prev_layer->protocol->protocol));
}
}
return true;
}
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());
}
}
bool probe_set_protocols(probe_t * probe, const char * name1, ...)
{
// TODO A similar function should allow hooking into the layer structure
// and not at the top layer
va_list args, args2;
size_t packet_size, offset, segment_size;
const char * name;
layer_t * layer = NULL,
* prev_layer;
const protocol_t * protocol;
// Remove the former layer structure
probe_layers_clear(probe);
// Set up the new layer structure
va_start(args, name1);
// Allocate the buffer according to the layer structure
packet_size = 0;
va_copy(args2, args);
for (name = name1; name; name = va_arg(args2, char *)) {
if (!(protocol = protocol_search(name))) {
fprintf(stderr, "Cannot find %s protocol, known protocols are:", name);
protocols_dump();
goto ERR_PROTOCOL_SEARCH;
}
packet_size += protocol->write_default_header(NULL);
}
va_end(args2);
if (!(packet_resize(probe->packet, packet_size))) goto ERR_PACKET_RESIZE;
// Create each layer
offset = 0;
prev_layer = NULL;
for (name = name1; name; name = va_arg(args, char *)) {
// Associate protocol to the layer
if (!(protocol = protocol_search(name))) goto ERR_PROTOCOL_SEARCH2;
segment_size = protocol->write_default_header(packet_get_bytes(probe->packet) + offset);
if (!(layer = layer_create_from_segment(protocol, packet_get_bytes(probe->packet) + offset, segment_size))) {
fprintf(stderr, "Can't create segment for %s header\n", layer->protocol->name);
goto ERR_LAYER_CREATE;
}
// TODO layer_set_mask(layer, bitfield_get_mask(probe->bitfield) + offset);
// Update 'length' field (if any). It concerns IPv* and UDP, but not TCP or ICMPv*
layer_set_field_and_free(layer, I16("length", packet_size - offset));
// Update 'protocol' field of the previous inserted layer (if any)
if (prev_layer) {
if (!layer_set_field_and_free(prev_layer, I8("protocol", layer->protocol->protocol))) {
fprintf(stderr, "Can't set 'protocol' in %s header\n", layer->protocol->name);
goto ERR_SET_PROTOCOL;
}
}
offset += layer->segment_size;
if (!probe_push_layer(probe, layer)) {
fprintf(stderr, "Can't add protocol layer\n");
goto ERR_PUSH_LAYER;
}
prev_layer = layer;
}
va_end(args);
layer = NULL;
// Payload : initially empty
if (!probe_push_payload(probe, 0)) {
goto ERR_PUSH_PAYLOAD;
}
// Size and checksum are pending, they depend on payload
return true;
ERR_PUSH_PAYLOAD:
ERR_PUSH_LAYER:
ERR_SET_PROTOCOL:
if (layer) layer_free(layer);
ERR_LAYER_CREATE:
ERR_PROTOCOL_SEARCH2:
probe_layers_clear(probe);
ERR_PACKET_RESIZE:
ERR_PROTOCOL_SEARCH:
return false;
}
void ENTFTN(CSHIFTS, cshifts)(void *rb, /* result base */
void *ab, /* array base */
__INT_T *sb, /* shift base */
__INT_T *db, /* dimension */
F90_Desc *rs, /* result descriptor */
F90_Desc *as, /* array descriptor */
F90_Desc *ss, /* shift descriptor */
F90_Desc *ds) /* dim descriptor */
{
DECL_HDR_VARS(ac);
DECL_HDR_VARS(rc);
DECL_DIM_PTRS(ad);
DECL_DIM_PTRS(rd);
char *ap, *rp;
chdr *c1, *c2;
__INT_T aflags, albase, apbase, arepli, ascoff;
__INT_T rflags, rlbase, rpbase, rrepli, rscoff;
__INT_T aolb[MAXDIMS], aoub[MAXDIMS];
__INT_T rolb[MAXDIMS], roub[MAXDIMS];
__INT_T dim, extent, i, sabs, shift;
__INT_T al, au, rl, ru;
shift = *sb;
dim = *db;
#if defined(DEBUG)
if (__fort_test & DEBUG_CSHF) {
printf("%d r", GET_DIST_LCPU);
I8(__fort_show_section)(rs);
printf("@%x = CSHIFT(a", rb);
I8(__fort_show_section)(as);
printf("@%x, shift=%d, dim=%d)\n", ab, shift, dim);
}
#endif
/* compute the net positive (left) shift amount */
SET_DIM_PTRS(ad, as, dim - 1);
extent = F90_DPTR_EXTENT_G(ad);
if (extent < 0)
return;
sabs = shift % extent;
if (sabs < 0)
sabs += extent;
/* copy straight across if net shift amount is zero */
if (sabs == 0) {
rp = (char *)rb + DIST_SCOFF_G(rs) * F90_LEN_G(rs);
ap = (char *)ab + DIST_SCOFF_G(as) * F90_LEN_G(as);
c1 = I8(__fort_copy)(rp, ap, rs, as, NULL);
__fort_doit(c1);
__fort_frechn(c1);
return;
}
/* form section descriptors to describe shift. */
__DIST_INIT_SECTION(ac, F90_RANK_G(as), as);
__DIST_INIT_SECTION(rc, F90_RANK_G(as), rs);
for (i = 1; i <= F90_RANK_G(as); ++i) {
if (i == dim)
continue;
SET_DIM_PTRS(ad, as, i - 1);
I8(__fort_set_section)(ac, i, as, i, F90_DPTR_LBOUND_G(ad),
DPTR_UBOUND_G(ad), 1);
SET_DIM_PTRS(rd, rs, i - 1);
I8(__fort_set_section)(rc, i, rs, i, F90_DPTR_LBOUND_G(rd),
DPTR_UBOUND_G(rd), 1);
}
/* save descriptor fields affected by set/finish_section */
aflags = F90_FLAGS_G(ac);
albase = F90_LBASE_G(ac);
apbase = DIST_PBASE_G(ac);
arepli = DIST_REPLICATED_G(ac);
ascoff = DIST_SCOFF_G(ac);
for (i = F90_RANK_G(ac); --i >= 0;) {
aolb[i] = DIST_DIM_OLB_G(ac, i);
aoub[i] = DIST_DIM_OUB_G(ac, i);
}
rflags = F90_FLAGS_G(rc);
rlbase = F90_LBASE_G(rc);
rpbase = DIST_PBASE_G(rc);
rrepli = DIST_REPLICATED_G(rc);
rscoff = DIST_SCOFF_G(rc);
for (i = F90_RANK_G(rc); --i >= 0;) {
rolb[i] = DIST_DIM_OLB_G(rc, i);
roub[i] = DIST_DIM_OUB_G(rc, i);
}
/* move upper section to lower */
SET_DIM_PTRS(ad, as, dim - 1);
SET_DIM_PTRS(rd, rs, dim - 1);
I8(__fort_set_section)(ac, dim, as, dim, F90_DPTR_LBOUND_G(ad) + sabs,
DPTR_UBOUND_G(ad), 1);
I8(__fort_finish_section)((ac));
I8(__fort_set_section)(rc, dim, rs, dim, F90_DPTR_LBOUND_G(rd),
DPTR_UBOUND_G(rd) - sabs, 1);
I8(__fort_finish_section)((rc));
rp = (char *)rb + DIST_SCOFF_G(rc) * F90_LEN_G(rc);
ap = (char *)ab + DIST_SCOFF_G(ac) * F90_LEN_G(ac);
c1 = I8(__fort_copy)(rp, ap, rc, ac, NULL);
/* restore descriptor fields */
F90_FLAGS_P(ac, aflags);
F90_LBASE_P(ac, albase);
DIST_PBASE_P(ac, apbase);
DIST_REPLICATED_P(ac, arepli);
DIST_SCOFF_P(ac, ascoff);
for (i = F90_RANK_G(ac); --i >= 0;) {
DIST_DIM_OLB_P(ac, i, aolb[i]);
DIST_DIM_OUB_P(ac, i, aoub[i]);
}
DIST_CACHED_P(ac, 0);
F90_FLAGS_P(rc, rflags);
F90_LBASE_P(rc, rlbase);
DIST_PBASE_P(rc, rpbase);
DIST_REPLICATED_P(rc, rrepli);
DIST_SCOFF_P(rc, rscoff);
for (i = F90_RANK_G(rc); --i >= 0;) {
DIST_DIM_OLB_P(rc, i, rolb[i]);
DIST_DIM_OUB_P(rc, i, roub[i]);
}
DIST_CACHED_P(rc, 0);
/* move lower section to upper */
I8(__fort_set_section)(ac, dim, as, dim, F90_DPTR_LBOUND_G(ad),
DPTR_UBOUND_G(ad) - (extent - sabs), 1);
I8(__fort_finish_section)((ac));
I8(__fort_set_section)(rc, dim, rs, dim,
F90_DPTR_LBOUND_G(rd) + (extent - sabs),
DPTR_UBOUND_G(rd), 1);
I8(__fort_finish_section)((rc));
rp = (char *)rb + DIST_SCOFF_G(rc) * F90_LEN_G(rc);
ap = (char *)ab + DIST_SCOFF_G(ac) * F90_LEN_G(ac);
c2 = I8(__fort_copy)(rp, ap, rc, ac, NULL);
c1 = __fort_chain_em_up(c1, c2);
__fort_doit(c1);
__fort_frechn(c1);
}
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());
}
}