Beispiel #1
0
__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;
      }
    }
Beispiel #2
0
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;
}
Beispiel #3
0
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;
	}
}
Beispiel #4
0
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));
}
Beispiel #5
0
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));
}
Beispiel #6
0
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);
}
Beispiel #7
0
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;
}
Beispiel #8
0
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());
  }
}
Beispiel #9
0
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;
}
Beispiel #10
0
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());
  }
}