image_calibration.cpp

Go to the documentation of this file.

#include <youbot_overhead_vision/image_calibration.h>
#include <sys/time.h>
#include <math.h>

using namespace std;
using namespace cv;

imageCalibration::imageCalibration()
{
  count = 0;

//These variable is red from a file.  IF the file is empty, zeroes will break the program.  As such, they are preset
  //As such, it is initialized to 1.
  blurringFactor = 1;
  calibAccuracy = 5;

  seq_num = 0;

  //Initializing marker linked list
  robotMarkersVect = *(new vector<markerPoints>());
  robotMarkersVect.push_back(*(new markerPoints()));
  robotMarkersVect[0].numPoints = 0;

  ros::NodeHandle nh;

  imgSub1 = nh.subscribe<sensor_msgs::Image>(OVERHEAD_NORTH, 0, &imageCalibration::imageCallback1, this);
  imgSub2 = nh.subscribe<sensor_msgs::Image>(OVERHEAD_SOUTH, 0, &imageCalibration::imageCallback2, this);

  pubImgCal = nh.advertise<sensor_msgs::Image>("/image_calibration/image_cal", 1);
  pubRobotPosition = nh.advertise<youbot_overhead_vision::PositionFromCamera>("/image_calibration/position_from_camera",
                                                                              1);

  //Gets the file locations of the calibration files from the param server

  nh.getParam("/image_calibration/runFromSimulation", useSim);

  nh.getParam("/image_calibration/floor_cal", filenames[FLOOR]);
  nh.getParam("/image_calibration/robot_lb_file", filenames[ROBOT_LEFT_BACK]);
  nh.getParam("/image_calibration/robot_rb_file", filenames[ROBOT_RIGHT_BACK]);
  nh.getParam("/image_calibration/robot_lf_file", filenames[ROBOT_LEFT_FRONT]);
  nh.getParam("/image_calibration/robot_rf_file", filenames[ROBOT_RIGHT_FRONT]);
  calTree = new calibTree();

  getCalibration(FLOOR, 255, 255, 255);
  getCalibration(ROBOT_RIGHT_BACK, 255, 0, 0);
  getCalibration(ROBOT_LEFT_BACK, 0, 255, 0);
  getCalibration(ROBOT_LEFT_FRONT, 0, 0, 255);
  getCalibration(ROBOT_RIGHT_FRONT, 0, 255, 255);

  string roomBoundaryFileName;
  nh.getParam("/image_calibration/room_boundary_map", roomBoundaryFileName);
  roomBoundary = cv::imread(roomBoundaryFileName);

  //Setting the colors of the NOT_SET calibration type to black.  In otherwords, pixels without a calibration will be black.
  colors[NOT_SET][0] = (uint8_t)0;
  colors[NOT_SET][1] = (uint8_t)0;
  colors[NOT_SET][2] = (uint8_t)0;

  lastFrameMidpointX = -SIDE_MARKER_DIST;
  lastFrameMidpointY = -SIDE_MARKER_DIST;

  if (useSim)
    pastObstImg = *(new cv::Mat(600 - SIM_OVERLAP, 400, CV_8UC3));
  else
    pastObstImg = *(new cv::Mat(600 - REAL_OVERLAP, 400, CV_8UC3));

  cv::rectangle(pastObstImg, cv::Point(0, 0), cv::Point(pastObstImg.cols, pastObstImg.rows), CV_RGB(255,255,255),
                CV_FILLED);
}

imageCalibration::~imageCalibration()
{
  freerobotVector();
}

void imageCalibration::getCalibration(int calibType, int calibR, int calibB, int calibG)
{

  string filename = filenames[calibType];
  colors[calibType][0] = calibR;
  colors[calibType][1] = calibG;
  colors[calibType][2] = calibB;

  ifstream calFile(filename.c_str());
  YAML::Parser parser(calFile);
  YAML::Node doc;
  parser.GetNextDocument(doc);

  if ((doc.size() > 0 && calibType != FLOOR) || doc.size() > 1)
  {
    int max = doc.size();

    //Getting Blurring factor and calibAccuracy parameters in the floor file
    if (calibType == FLOOR)
    {
      max -= 2;
      try
      {
        doc["blur"] >> blurringFactor;
        doc["calib_accuracy"] >> calibAccuracy;
      }
      catch (YAML::InvalidScalar &)
      {
        ROS_WARN("The floor file does not have a blurring or accuracy value.  Values are set to defaults");
      }
    }
    //Load up calibration structures with data from files.
    for (int i = 1; i <= max; i++)
    {
      string aPoint = "Point " + convertInt(i);

      int red, green, blue, camera;
      (*(doc.FindValue(aPoint.c_str()))->FindValue("rgb"))[0] >> red;
      (*(doc.FindValue(aPoint.c_str()))->FindValue("rgb"))[1] >> green;
      (*(doc.FindValue(aPoint.c_str()))->FindValue("rgb"))[2] >> blue;
      (*(doc.FindValue(aPoint.c_str()))->FindValue("camera")) >> camera;
      calTree->newCalibPoint((uint8_t)red, (uint8_t)green, (uint8_t)blue, camera, calibType);

    }
    calTree->orderCalibsByRed();
    calTree->createTree();
  }
  else if (calibType == FLOOR)
    ROS_INFO("Floor file did not have proper format. No blur/calibration variables found.");

}

void imageCalibration::processImage()
{

  vector<int> ImageXCoords;
  vector<int> ImageYCoords;
  resetForNextFrame();
  ros::NodeHandle nh;

  //Filling up vector with 4 NOT_SET ints
  for (int x = 0; x < 4; x++)
  {
    ImageXCoords.push_back(NOT_SET);
    ImageYCoords.push_back(NOT_SET);
  }

  //Copy the current raw_Img into the output_img for calibration

  cv_bridge::CvImagePtr * temp1 = new cv_bridge::CvImagePtr(new cv_bridge::CvImage());
  cv_bridge::CvImagePtr * temp2 = new cv_bridge::CvImagePtr(new cv_bridge::CvImage());
  output_Img1 = *(temp1);
  output_Img2 = *(temp2);

  while (raw_Img1->encoding != ENCODING && raw_Img2->encoding != ENCODING)
    cout << "Images Not Loaded" << endl;

  raw_Img1->image.copyTo(output_Img1->image);
  raw_Img2->image.copyTo(output_Img2->image);

  //Making a big combined version of raw_Img1 & raw_Img2
  cv::Mat * out1 = (new cv::Mat(output_Img1->image.rows / 2, output_Img1->image.cols / 2, output_Img1->image.type()));
  cv::Mat * out2 = (new cv::Mat(output_Img2->image.rows / 2, output_Img2->image.cols / 2, output_Img2->image.type()));

  cv::resize(output_Img1->image, *out1, out1->size(), cv::INTER_NEAREST);
  cv::resize(output_Img2->image, *out2, out2->size(), cv::INTER_NEAREST);
  output_Img1->image = (output_Img1->image)(cv::Rect(0, 0, out1->cols, out1->rows));
  output_Img2->image = (output_Img2->image)(cv::Rect(0, 0, out2->cols, out2->rows));

  cv::Mat * cvImg;
  if (useSim)
    cvImg = new cv::Mat(out1->rows * 2 - SIM_OVERLAP, out1->cols, out1->type());
  else
    cvImg = new cv::Mat(out1->rows * 2 - REAL_OVERLAP, out1->cols, out1->type());

  cv::Mat roiCvImgBot, roiCvImgTop, roiOut2;
  roiCvImgTop = (*cvImg)(cv::Rect(0, 0, out1->cols, out1->rows));
  if (useSim)
  {
    roiCvImgBot = (*cvImg)(cv::Rect(0, out1->rows - SIM_OVERLAP / 2, out2->cols, out2->rows - SIM_OVERLAP / 2));
    roiOut2 = (*out2)(cv::Rect(0, SIM_OVERLAP / 2, out2->cols, out2->rows - SIM_OVERLAP / 2));
  }
  else
  {
    roiCvImgBot = (*cvImg)(cv::Rect(0, out1->rows - REAL_OVERLAP / 2, out2->cols, out2->rows - REAL_OVERLAP / 2));
    roiOut2 = (*out2)(cv::Rect(0, REAL_OVERLAP / 2, out2->cols, out2->rows - REAL_OVERLAP / 2));
  }

  cv::Mat roiOut1 = (*out1)(cv::Rect(0, 0, out1->cols, out1->rows));

  roiOut1.copyTo(roiCvImgTop);
  roiOut2.copyTo(roiCvImgBot);

  (*cvImg).copyTo(output_Img1->image);

  output_Img1->encoding = ENCODING;
  output_Img1->header.seq = seq_num++;
  output_Img1->header.stamp = ros::Time::now();
  output_Img1->header.frame_id = FRAME_ID;

  //Applies calib 

  applyCalib();

  addClusters(&ImageXCoords, &ImageYCoords);

  //Determines how many robot marker points were found.
  int pointsFound = 0;
  for (int i = 0; i < (int)ImageXCoords.size(); i++)
  {
    if (ImageXCoords[i] != NOT_SET)
      pointsFound++;
  }

  //If between 2 or three points are found, it is possible to extrapolate the position of the missing points.
  if (pointsFound == 2 || pointsFound == 3)
    extrapolateMarkers(&ImageXCoords, &ImageYCoords);

  // If the robot is found, publish its position, and remove it from the image.
  if (ImageXCoords[0] != NOT_SET && ImageXCoords[1] != NOT_SET && ImageXCoords[2] != NOT_SET
      && ImageXCoords[3] != NOT_SET)
  {

    double heading = atan2((double)ImageYCoords[ROBOT_LEFT_FRONT] - ImageYCoords[ROBOT_LEFT_BACK],
                           ImageXCoords[ROBOT_LEFT_FRONT] - ImageXCoords[ROBOT_LEFT_BACK]);

    youbot_overhead_vision::PositionFromCamera pos;
    pos.heading = heading;
    pos.midpointX = (ImageXCoords[ROBOT_LEFT_FRONT] + ImageXCoords[ROBOT_RIGHT_BACK]) / 2;
    pos.midpointY = (ImageYCoords[ROBOT_LEFT_FRONT] + ImageYCoords[ROBOT_RIGHT_BACK]) / 2;

    //Save midpoint of the robot in the frame if the robot was found. To be used next frame to determine more accurately which points are the real robot marker points. If the robot isnt found, the values are set negative enough so that they will not affect the probability calculations
    lastFrameMidpointX = pos.midpointX;
    lastFrameMidpointY = pos.midpointY;

    //Expands box and removes robot if all points have been found.
    removeRobot(ImageXCoords, ImageYCoords, pos.midpointX, pos.midpointY);

    pos.midpointX *= 2;
    pos.midpointY *= 2;
    pubRobotPosition.publish(pos);
  }

  //Resize image for publishing
  cv::resize(output_Img1->image, output_Img1->image,
             cv::Size(output_Img1->image.cols / 2.0, output_Img1->image.rows / 2.0));
  //Add the room boundaries to the image
  cv::bitwise_and(roomBoundary, output_Img1->image, output_Img1->image);
  //Publishes calibrated image for the map server.
  pubImgCal.publish(output_Img1->toImageMsg());

  delete temp1;
  delete temp2;
  resetForNextFrame();
  delete cvImg;
  delete out1;
  delete out2;
}

void imageCalibration::imageCallback1(const sensor_msgs::ImageConstPtr& image_msg)
{
  ros::Time time = image_msg->header.stamp;
  raw_Img1 = cv_bridge::toCvCopy(image_msg, ENCODING);

  // set the encoding
  raw_Img1->encoding = ENCODING;
  // set and update the sequence number
  raw_Img1->header.seq = seq_num++;
  // get the current timestamp
  raw_Img1->header.stamp = ros::Time::now();
  // set the frame ID
  raw_Img1->header.frame_id = FRAME_ID;
}

void imageCalibration::imageCallback2(const sensor_msgs::ImageConstPtr& image_msg)
{
  ros::Time time = image_msg->header.stamp;

  raw_Img2 = cv_bridge::toCvCopy(image_msg, ENCODING);

  // set the encoding
  raw_Img2->encoding = ENCODING;
  // set and update the sequence number
  raw_Img2->header.seq = seq_num++;
  // get the current timestamp
  raw_Img2->header.stamp = ros::Time::now();
  // set the frame ID
  raw_Img2->header.frame_id = FRAME_ID;
}

void imageCalibration::applyCalib()
{
  const cv::Mat& image = output_Img1->image;

  cv::Mat roiCvImgBot, roiCvImgTop;
  if (useSim)
  {
    roiCvImgBot = image(
        cv::Rect(0, output_Img2->image.rows - SIM_OVERLAP / 2, image.cols, output_Img2->image.rows - SIM_OVERLAP / 2));
    roiCvImgTop = image(cv::Rect(0, 0, image.cols, output_Img2->image.rows - SIM_OVERLAP / 2));
  }
  else
  {
    roiCvImgBot = image(
        cv::Rect(0, output_Img2->image.rows - REAL_OVERLAP / 2, image.cols,
                 output_Img2->image.rows - REAL_OVERLAP / 2));
    roiCvImgTop = image(cv::Rect(0, 0, image.cols, output_Img2->image.rows - REAL_OVERLAP / 2));
  }

  cv::blur(image, image, cv::Size(blurringFactor, blurringFactor), cv::Point(-1, -1), cv::BORDER_DEFAULT);

  cv::Mat * imagePtr;
  //Apply The calibrations on both cameras.
  for (int camera = 1; camera <= 2; camera++)
  {
    if (camera == NORTH_CAMERA)
      imagePtr = &roiCvImgTop;
    else
      imagePtr = &roiCvImgBot;

    int numBytes = imagePtr->step * imagePtr->rows;
    // applies calibration file on each pixel
    for (int pixelItt = 0; pixelItt < numBytes; pixelItt += 3)
    {
      //This call looks for applicable calibrations

      int calibType = calTree->parseCalibTree((int)imagePtr->data[pixelItt], (int)imagePtr->data[pixelItt + 1],
                                              (int)imagePtr->data[pixelItt + 2], camera, calibAccuracy);

      imagePtr->data[pixelItt] = (uint8_t)colors[calibType][0];
      imagePtr->data[pixelItt + 1] = (uint8_t)colors[calibType][1];
      imagePtr->data[pixelItt + 2] = (uint8_t)colors[calibType][2];
      //If the calib type is smaller than the floor or "NOT_SET", it must be one of the robot markers.  These points are stored using the MarkerPoint function.
      if (calibType < FLOOR)
      {
        int pointX = pixelItt / (output_Img1->image.step);
        int pointY = pixelItt - pointX * (output_Img1->image.step);

        if (camera == SOUTH_CAMERA)
        {
          pointX += roiCvImgTop.rows;
        }

        pointY /= 3;
        addMarkerPoint(pointX, pointY, calibType);
      }
    }
  }

  //The morphologyEx(dilate +erode) eliminates pixels that are by themselves.  dilate reduces black zones by 1 pixel in each direction, erode increases them back their original position.  Large obstacles remain unchanged, small obstacles disappear
  int erosion_size = 1;
  cv::Mat element = getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1),
                                          cv::Point(erosion_size, erosion_size));
  morphologyEx(image, image, cv::MORPH_CLOSE, element);
}

void imageCalibration::addMarkerPoint(int pointX, int pointY, int calibType)
{

  for (int markerItt = 0; markerItt < (int)robotMarkersVect.size(); markerItt++)
  {
    //Getting the x,y coordinates of a point.

    //If the point fits in one of the clusters, add it in, and recalculate cluster 
    if (robotMarkersVect[markerItt].markerType
        == calibType&& abs(pointX - robotMarkersVect[markerItt].avgX) < MARKER_CLUSTER_RADIUS && abs(pointY - robotMarkersVect[markerItt].avgY) < MARKER_CLUSTER_RADIUS){

    //Calculating New ave

    robotMarkersVect[markerItt].avgX = (robotMarkersVect[markerItt].avgX * robotMarkersVect[markerItt].numPoints + pointX) /(robotMarkersVect[markerItt].numPoints + 1);
    robotMarkersVect[markerItt].avgY = (robotMarkersVect[markerItt].avgY * robotMarkersVect[markerItt].numPoints + pointY) /(robotMarkersVect[markerItt].numPoints + 1);

    robotMarkersVect[markerItt].numPoints++;
    return;
  }
  //If the point does not fit in any found clusters, create a new cluster
  else if(robotMarkersVect[markerItt].numPoints == 0)
  {

    robotMarkersVect[markerItt].numPoints ++;
    robotMarkersVect[markerItt].avgX = pointX;
    robotMarkersVect[markerItt].avgY = pointY;
    robotMarkersVect[markerItt].markerType = calibType;

    //Initializing new point.
    robotMarkersVect.push_back(*(new markerPoints()));
    robotMarkersVect[markerItt + 1].numPoints = 0;
    robotMarkersVect[markerItt + 1].avgX = (-1 * MARKER_CLUSTER_RADIUS - 1);
    robotMarkersVect[markerItt + 1].avgY = (-1 * MARKER_CLUSTER_RADIUS - 1);
    robotMarkersVect[markerItt + 1].markerType = NOT_SET;
    return;
  }
}
}

void imageCalibration::addClusters(vector<int> *ImageXCoords, vector<int> *ImageYCoords)
{

  //This loop removes small clusters
  for (int itt = 0; itt < (int)robotMarkersVect.size(); itt++)
  {
    if (robotMarkersVect[itt].numPoints < MIN_MARKER_PIXELS)
    {
      robotMarkersVect.erase(robotMarkersVect.begin() + itt);
      itt--;
    }

  }

  //Holds the marker that has the highest probability to be the real marker in the list, where [x][0] - index, [x][1]- probability
  int highestProb[4][2];
  for (int clearItt = 0; clearItt < 4; clearItt++)
  {
    highestProb[clearItt][0] = -1;
    highestProb[clearItt][1] = -1;

  }

  //Calculates Probabilities

  for (int markerItt = 0; markerItt < (int)robotMarkersVect.size(); markerItt++)
  {
    int probCount = 0;
    for (int markerColor = 0; markerColor < FLOOR; markerColor++)
    {
      bool probIncreased = false;

      //The probability counter should only be increased once per color, since if a cluster is in range of two different markers of the same color, one of them has to be wrong.
      for (int comparisonItt = 0; (!probIncreased) && comparisonItt < (int)robotMarkersVect.size(); comparisonItt++)
      {
        if (robotMarkersVect[comparisonItt].markerType == markerColor)
        {
          int dist = pow(
              pow((double)robotMarkersVect[markerItt].avgX - robotMarkersVect[comparisonItt].avgX, 2)
                  + pow((double)robotMarkersVect[markerItt].avgY - robotMarkersVect[comparisonItt].avgY, 2),
              .5);

          //If the two point's distance to each other is close to one of the robot distances (sides, front or diagonal), there is more chance the point is a robot marker
          if (abs(dist - SIDE_MARKER_DIST) < MARKER_ACCURACY || abs(dist - FRONT_MARKER_DIST) < MARKER_ACCURACY
              || abs(dist - DIAG_MARKER_DIST) < MARKER_ACCURACY)
          {
            probCount++;
            probIncreased = true;
          }
        }
      }
    }

    //If the point is close to the right distance away from the midpoint of the robot as calculated on the last frame, there is a higher chance that the point is a robot marker.
    int distToMid = pow(
        pow((double)robotMarkersVect[markerItt].avgX - lastFrameMidpointX, 2)
            + pow((double)robotMarkersVect[markerItt].avgY - lastFrameMidpointY, 2),
        .5);
    if (abs(distToMid - DIAG_MARKER_DIST / 2) < MARKER_ACCURACY)
    {
      probCount = probCount + 2;
    }

    //If the point has the highest probability of being the marker, record it.
    int markerType = robotMarkersVect[markerItt].markerType;

    if (highestProb[markerType][1] < probCount
        || (highestProb[markerType][1] == probCount
            && robotMarkersVect[highestProb[markerType][0]].numPoints < robotMarkersVect[markerItt].numPoints))
    {
      highestProb[markerType][0] = markerItt;
      highestProb[markerType][1] = probCount;
    }
  }

  //Highest Probability Sanity Checking.
  //Checking that the distance between the points with the highest probability actually match the dimensions of the robot.
  bool pointRemoved = false;
  for (int i = 0; i < 4 && (!pointRemoved); i++)
  {
    for (int j = i + 1; j < 4 && (!pointRemoved); j++)
    {
      if (highestProb[j][1] >= 1 && highestProb[i][1] >= 1)
      {
        int dist = pow(
            pow((double)robotMarkersVect[highestProb[i][0]].avgX - robotMarkersVect[highestProb[j][0]].avgX, 2)
                + pow((double)robotMarkersVect[highestProb[i][0]].avgY - robotMarkersVect[highestProb[j][0]].avgY, 2),
            .5);
        //Checking Side Dist, Front dist, and diag dist

        if ((j == (i + 2) % 4 && abs(dist - SIDE_MARKER_DIST) > MARKER_ACCURACY)
            || (j == (5 - i) % 4 && abs(dist - FRONT_MARKER_DIST) > MARKER_ACCURACY)
            || (j == (3 - i) % 4 && abs(dist - DIAG_MARKER_DIST) > MARKER_ACCURACY))
        {
          int distiToMid = pow(
              pow((double)robotMarkersVect[highestProb[i][0]].avgX - lastFrameMidpointX, 2)
                  + pow((double)robotMarkersVect[highestProb[i][0]].avgY - lastFrameMidpointY, 2),
              .5);
          int distjToMid = pow(
              pow((double)robotMarkersVect[highestProb[j][0]].avgX - lastFrameMidpointX, 2)
                  + pow((double)robotMarkersVect[highestProb[j][0]].avgY - lastFrameMidpointY, 2),
              .5);
          //The marker less likely to be a real marker is removed
          if (abs(distiToMid - DIAG_MARKER_DIST / 2) > abs(distjToMid - DIAG_MARKER_DIST / 2))
          {
            robotMarkersVect.erase(robotMarkersVect.begin() + highestProb[i][0]);
            pointRemoved = true;
          }
          else
          {
            robotMarkersVect.erase(robotMarkersVect.begin() + highestProb[j][0]);
            pointRemoved = true;
          }
        }
      }
    }
  }

  //If a point was removed, recalls the function. Otherwise, adds the marker point cluster with the highest probability of being the robot marker if it is above a certain threshold.
  if (pointRemoved)
  {
    addClusters(ImageXCoords, ImageYCoords);
    return;
  }
  else
  {
    int threshold = 1;
    for (int itt = 0; itt < 4; itt++)
    {
      if (highestProb[itt][0] > -1 && highestProb[itt][1] >= threshold)
      {
        (*ImageXCoords)[itt] = robotMarkersVect[highestProb[itt][0]].avgX;
        (*ImageYCoords)[itt] = robotMarkersVect[highestProb[itt][0]].avgY;
      }
      else
      {
        (*ImageXCoords)[itt] = NOT_SET;
        (*ImageYCoords)[itt] = NOT_SET;
      }
    }

  }
}

bool imageCalibration::extrapolateMarkers(vector<int> *ImageXCoords, vector<int> *ImageYCoords)
{

  vector<int> * xCoords = ImageXCoords;
  vector<int> * yCoords = ImageYCoords;

  double heading;
  //If the known edge is the front 
  if ((*xCoords)[ROBOT_LEFT_FRONT] != NOT_SET && (*xCoords)[ROBOT_RIGHT_FRONT] != NOT_SET)
  {
    heading = atan2((double)(*yCoords)[ROBOT_LEFT_FRONT] - (*yCoords)[ROBOT_RIGHT_FRONT],
                    (*xCoords)[ROBOT_LEFT_FRONT] - (*xCoords)[ROBOT_RIGHT_FRONT]) - PI / 2;

    if ((*xCoords)[ROBOT_LEFT_BACK] == NOT_SET)
    {
      (*xCoords)[ROBOT_LEFT_BACK] = (*xCoords)[ROBOT_LEFT_FRONT] - SIDE_MARKER_DIST * cos(heading);
      (*yCoords)[ROBOT_LEFT_BACK] = (*yCoords)[ROBOT_LEFT_FRONT] - SIDE_MARKER_DIST * sin(heading);
    }
    if ((*xCoords)[ROBOT_RIGHT_BACK] == NOT_SET)
    {
      (*xCoords)[ROBOT_RIGHT_BACK] = (*xCoords)[ROBOT_RIGHT_FRONT] - SIDE_MARKER_DIST * cos(heading);
      (*yCoords)[ROBOT_RIGHT_BACK] = (*yCoords)[ROBOT_RIGHT_FRONT] - SIDE_MARKER_DIST * sin(heading);
    }
    return true;
  }
  //if the known edge is the back
  else if ((*xCoords)[ROBOT_LEFT_BACK] != NOT_SET && (*xCoords)[ROBOT_RIGHT_BACK] != NOT_SET)
  {
    heading = atan2((double)(*yCoords)[ROBOT_LEFT_BACK] - (*yCoords)[ROBOT_RIGHT_BACK],
                    (*xCoords)[ROBOT_LEFT_BACK] - (*xCoords)[ROBOT_RIGHT_BACK]) - PI / 2;

    if ((*xCoords)[ROBOT_LEFT_FRONT] == NOT_SET)
    {
      (*xCoords)[ROBOT_LEFT_FRONT] = (*xCoords)[ROBOT_LEFT_BACK] + SIDE_MARKER_DIST * cos(heading);
      (*yCoords)[ROBOT_LEFT_FRONT] = (*yCoords)[ROBOT_LEFT_BACK] + SIDE_MARKER_DIST * sin(heading);
    }
    if ((*xCoords)[ROBOT_RIGHT_FRONT] == NOT_SET)
    {
      (*xCoords)[ROBOT_RIGHT_FRONT] = (*xCoords)[ROBOT_RIGHT_BACK] + SIDE_MARKER_DIST * cos(heading);
      (*yCoords)[ROBOT_RIGHT_FRONT] = (*yCoords)[ROBOT_RIGHT_BACK] + SIDE_MARKER_DIST * sin(heading);
    }
    return true;
  }
  //If the known edge is the left side
  else if ((*xCoords)[ROBOT_LEFT_FRONT] != NOT_SET && (*xCoords)[ROBOT_LEFT_BACK] != NOT_SET)
  {
    heading = atan2((double)(*yCoords)[ROBOT_LEFT_FRONT] - (*yCoords)[ROBOT_LEFT_BACK],
                    (*xCoords)[ROBOT_LEFT_FRONT] - (*xCoords)[ROBOT_LEFT_BACK]);
    if ((*xCoords)[ROBOT_RIGHT_FRONT] == NOT_SET)
    {
      (*xCoords)[ROBOT_RIGHT_FRONT] = (*xCoords)[ROBOT_LEFT_FRONT] + FRONT_MARKER_DIST * cos(heading - PI / 2);
      (*yCoords)[ROBOT_RIGHT_FRONT] = (*yCoords)[ROBOT_LEFT_FRONT] + FRONT_MARKER_DIST * sin(heading - PI / 2);
    }
    if ((*xCoords)[ROBOT_RIGHT_BACK] == NOT_SET)
    {
      (*xCoords)[ROBOT_RIGHT_BACK] = (*xCoords)[ROBOT_LEFT_BACK] + FRONT_MARKER_DIST * cos(heading - PI / 2);
      (*yCoords)[ROBOT_RIGHT_BACK] = (*yCoords)[ROBOT_LEFT_BACK] + FRONT_MARKER_DIST * sin(heading - PI / 2);
    }
    return true;
  }
  //If the known edge is the right side
  else if ((*xCoords)[ROBOT_RIGHT_FRONT] != NOT_SET && (*xCoords)[ROBOT_RIGHT_BACK] != NOT_SET)
  {
    heading = atan2((double)(*yCoords)[ROBOT_RIGHT_FRONT] - (*yCoords)[ROBOT_RIGHT_BACK],
                    (*xCoords)[ROBOT_RIGHT_FRONT] - (*xCoords)[ROBOT_RIGHT_BACK]);
    if ((*xCoords)[ROBOT_LEFT_FRONT] == NOT_SET)
    {
      (*xCoords)[ROBOT_LEFT_FRONT] = (*xCoords)[ROBOT_RIGHT_FRONT] + FRONT_MARKER_DIST * cos(heading + PI / 2);
      (*yCoords)[ROBOT_LEFT_FRONT] = (*yCoords)[ROBOT_RIGHT_FRONT] + FRONT_MARKER_DIST * sin(heading + PI / 2);
    }
    if ((*xCoords)[ROBOT_LEFT_BACK] == NOT_SET)
    {
      (*xCoords)[ROBOT_LEFT_BACK] = (*xCoords)[ROBOT_RIGHT_BACK] + FRONT_MARKER_DIST * cos(heading + PI / 2);
      (*yCoords)[ROBOT_LEFT_BACK] = (*yCoords)[ROBOT_RIGHT_BACK] + FRONT_MARKER_DIST * sin(heading + PI / 2);
    }
    return true;
  }
  //If the known edge is a diagonal
  else if ((*xCoords)[ROBOT_LEFT_FRONT] != NOT_SET && (*xCoords)[ROBOT_RIGHT_BACK] != NOT_SET)
  {
    heading = atan2((double)(*yCoords)[ROBOT_LEFT_FRONT] - (*yCoords)[ROBOT_RIGHT_BACK],
                    (*xCoords)[ROBOT_LEFT_FRONT] - (*xCoords)[ROBOT_RIGHT_BACK])
        - atan((double)FRONT_MARKER_DIST / (double)SIDE_MARKER_DIST);
    if ((*xCoords)[ROBOT_RIGHT_FRONT] == NOT_SET)
    {
      (*xCoords)[ROBOT_RIGHT_FRONT] = (*xCoords)[ROBOT_RIGHT_BACK] + SIDE_MARKER_DIST * cos(heading);
      (*yCoords)[ROBOT_RIGHT_FRONT] = (*yCoords)[ROBOT_RIGHT_BACK] + SIDE_MARKER_DIST * sin(heading);
    }
    if ((*xCoords)[ROBOT_LEFT_BACK] == NOT_SET)
    {
      (*xCoords)[ROBOT_LEFT_BACK] = (*xCoords)[ROBOT_LEFT_FRONT] - SIDE_MARKER_DIST * cos(heading);
      (*yCoords)[ROBOT_LEFT_BACK] = (*yCoords)[ROBOT_LEFT_FRONT] - SIDE_MARKER_DIST * sin(heading);
    }
    return true;
  }
  else if ((*xCoords)[ROBOT_RIGHT_FRONT] != NOT_SET && (*xCoords)[ROBOT_LEFT_BACK] != NOT_SET)
  {
    heading = atan2((double)((*yCoords)[ROBOT_RIGHT_FRONT] - (*yCoords)[ROBOT_LEFT_BACK]),
                    ((*xCoords)[ROBOT_RIGHT_FRONT] - (*xCoords)[ROBOT_LEFT_BACK]))
        + atan((double)FRONT_MARKER_DIST / (double)SIDE_MARKER_DIST);

    if ((*xCoords)[ROBOT_LEFT_FRONT] == NOT_SET)
    {
      (*xCoords)[ROBOT_LEFT_FRONT] = (*xCoords)[ROBOT_LEFT_BACK] + SIDE_MARKER_DIST * cos(heading);
      (*yCoords)[ROBOT_LEFT_FRONT] = (*yCoords)[ROBOT_LEFT_BACK] + SIDE_MARKER_DIST * sin(heading);
    }
    if ((*xCoords)[ROBOT_RIGHT_BACK] == NOT_SET)
    {
      (*xCoords)[ROBOT_RIGHT_BACK] = (*xCoords)[ROBOT_RIGHT_FRONT] - SIDE_MARKER_DIST * cos(heading);
      (*yCoords)[ROBOT_RIGHT_BACK] = (*yCoords)[ROBOT_RIGHT_FRONT] - SIDE_MARKER_DIST * sin(heading);
    }
    return true;
  }
  else
  {
    return false;
  }

}

void imageCalibration::removeRobot(vector<int> xCoords, vector<int> yCoords, int xpos, int ypos)
{
  cv::Mat cvImg = output_Img1->image;
  int minDstIndex;

  vector<int> outerxCoords;
  vector<int> outeryCoords;

  float heading = atan2((double)yCoords[ROBOT_LEFT_FRONT] - yCoords[ROBOT_LEFT_BACK],
                        xCoords[ROBOT_LEFT_FRONT] - xCoords[ROBOT_LEFT_BACK]);
  float theta;
  float l;

  //Front Left point
  theta = heading + atan((float)(LEFT_EXTENDED / 3.0) / (float)(FRONT_EXTENDED / 3.0));
  l = sqrt(pow((LEFT_EXTENDED / 3.0), 2) + pow((FRONT_EXTENDED / 3.0), 2));
  outerxCoords.push_back((int)(yCoords[ROBOT_LEFT_FRONT] + l * sin(theta)));
  outeryCoords.push_back((int)(xCoords[ROBOT_LEFT_FRONT] + l * cos(theta)));

  //Front Right point
  theta = heading - atan((float)(RIGHT_EXTENDED / 3.0) / (float)(FRONT_EXTENDED / 3.0));
  l = sqrt(pow((RIGHT_EXTENDED / 3.0), 2) + pow((FRONT_EXTENDED / 3.0), 2));
  outerxCoords.push_back((int)(yCoords[ROBOT_RIGHT_FRONT] + l * sin(theta)));
  outeryCoords.push_back((int)(xCoords[ROBOT_RIGHT_FRONT] + l * cos(theta)));

  //Back Left point
  theta = heading + 3.14159 - atan((float)(LEFT_EXTENDED / 3.0) / (float)(BACK_EXTENDED / 3.0));
  l = sqrt(pow((LEFT_EXTENDED / 3.0), 2) + pow((BACK_EXTENDED / 3.0), 2));
  outerxCoords.push_back((int)(yCoords[ROBOT_LEFT_BACK] + l * sin(theta)));
  outeryCoords.push_back((int)(xCoords[ROBOT_LEFT_BACK] + l * cos(theta)));

  //Back Right point
  theta = heading + 3.14159 + atan((float)(RIGHT_EXTENDED / 3.0) / (float)(BACK_EXTENDED / 3.0));
  l = sqrt(pow((RIGHT_EXTENDED / 3.0), 2) + pow((BACK_EXTENDED / 3.0), 2));
  outerxCoords.push_back((int)(yCoords[ROBOT_RIGHT_BACK] + l * sin(theta)));
  outeryCoords.push_back((int)(xCoords[ROBOT_RIGHT_BACK] + l * cos(theta)));

  cv::Point points[4];
  points[0].x = outerxCoords[0];
  points[0].y = outeryCoords[0];
  points[1].x = outerxCoords[1];
  points[1].y = outeryCoords[1];
  points[2].x = outerxCoords[3];
  points[2].y = outeryCoords[3];
  points[3].x = outerxCoords[2];
  points[3].y = outeryCoords[2];

  cv::fillConvexPoly(cvImg, points, 4, cv::Scalar(0, 0, 0));

  //Detect robot using edge detection
  vector<cv::RotatedRect> minRect = findBoundingBox(cvImg, xpos, ypos, &minDstIndex);

  if (minRect.size() > 0)
  {
    applyBoundingBox(&cvImg, minRect[minDstIndex]);
  }
  //if robot edge detection failed, create the bounding box based on the
  //position and heading of the robot
  else
  {
    outerxCoords.clear();
    outeryCoords.clear();

    //Front Left point
    theta = heading + atan((float)LEFT_EXTENDED / (float)FRONT_EXTENDED);
    l = sqrt(pow((double)LEFT_EXTENDED, 2) + pow((double)FRONT_EXTENDED, 2));
    outerxCoords.push_back((int)(yCoords[ROBOT_LEFT_FRONT] + l * sin(theta)));
    outeryCoords.push_back((int)(xCoords[ROBOT_LEFT_FRONT] + l * cos(theta)));

    //Front Right point
    theta = heading - atan((float)RIGHT_EXTENDED / (float)FRONT_EXTENDED);
    l = sqrt(pow((double)RIGHT_EXTENDED, 2) + pow((double)FRONT_EXTENDED, 2));
    outerxCoords.push_back((int)(yCoords[ROBOT_RIGHT_FRONT] + l * sin(theta)));
    outeryCoords.push_back((int)(xCoords[ROBOT_RIGHT_FRONT] + l * cos(theta)));

    //Back Left point
    theta = heading + 3.14159 - atan((float)LEFT_EXTENDED / (float)BACK_EXTENDED);
    l = sqrt(pow((double)LEFT_EXTENDED, 2) + pow((double)BACK_EXTENDED, 2));
    outerxCoords.push_back((int)(yCoords[ROBOT_LEFT_BACK] + l * sin(theta)));
    outeryCoords.push_back((int)(xCoords[ROBOT_LEFT_BACK] + l * cos(theta)));

    //Back Right point
    theta = heading + 3.14159 + atan((float)RIGHT_EXTENDED / (float)BACK_EXTENDED);
    l = sqrt(pow((double)RIGHT_EXTENDED, 2) + pow((double)BACK_EXTENDED, 2));
    outerxCoords.push_back((int)(yCoords[ROBOT_RIGHT_BACK] + l * sin(theta)));
    outeryCoords.push_back((int)(xCoords[ROBOT_RIGHT_BACK] + l * cos(theta)));

    cv::Point points[4];
    points[0].x = outerxCoords[0];
    points[0].y = outeryCoords[0];
    points[1].x = outerxCoords[1];
    points[1].y = outeryCoords[1];
    points[2].x = outerxCoords[3];
    points[2].y = outeryCoords[3];
    points[3].x = outerxCoords[2];
    points[3].y = outeryCoords[2];

    //Filled a specific color so we know what points to avoid.
    cv::fillConvexPoly(cvImg, points, 4, cv::Scalar(251, 144, 2));

    int numBytes = cvImg.step * cvImg.rows;

    /*Goes through every pixel.  If that pixel is not in the area being removed, that pixel is updated in the pastObstImg image as it contains current obstacle information.  If it is in the area where the robot is (with rgb 251, 144, 2), then the pixel value for pastObstImg is used as it contains old obstacle information for that pixel.  While not as good as current information, it is better than having no information at all in the area*/
    for (int pixelItt = 0; pixelItt < numBytes; pixelItt += 3)
    {
      if (cvImg.data[pixelItt] != 251 && cvImg.data[pixelItt + 1] != 144 && cvImg.data[pixelItt + 2] != 2)
      {
        pastObstImg.data[pixelItt] = cvImg.data[pixelItt];
        pastObstImg.data[pixelItt + 1] = cvImg.data[pixelItt + 1];
        pastObstImg.data[pixelItt + 2] = cvImg.data[pixelItt + 2];
      }
      else
      {
        cvImg.data[pixelItt] = pastObstImg.data[pixelItt];
        cvImg.data[pixelItt + 1] = pastObstImg.data[pixelItt + 1];
        cvImg.data[pixelItt + 2] = pastObstImg.data[pixelItt + 2];
      }
    }
  }

}

vector<cv::RotatedRect> imageCalibration::findBoundingBox(cv::Mat image, int xpos, int ypos, int *minDstIndex)
{
  cv::Mat imgCopy;

  cv::cvtColor(image, imgCopy, CV_BGR2GRAY);
  cv::blur(imgCopy, imgCopy, cv::Size(3, 3));

  vector<vector<cv::Point> > contours;

  // Detect edges using Threshold
  cv::threshold(imgCopy, imgCopy, 60, 255, cv::THRESH_BINARY);

  cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3), cv::Point(1, 1));
  cv::morphologyEx(imgCopy, imgCopy, cv::MORPH_CLOSE, element);
  element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3), cv::Point(1, 1));
  cv::morphologyEx(imgCopy, imgCopy, cv::MORPH_OPEN, element);

  // Find contours
  cv::findContours(imgCopy, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0));

  // Find the rotated rectangles to bound each contour
  vector<cv::RotatedRect> minRect(contours.size());

  for (unsigned int i = 0; i < contours.size(); i++)
  {
    minRect[i] = cv::minAreaRect(cv::Mat(contours[i]));
  }

  //Filter bounding boxes by size and distance from robot center
  unsigned int i = 0;
  float minDst = 999999;
  int minIndex = 0;
  while (i < minRect.size())
  {
    float width = minRect[i].size.width;
    float length = minRect[i].size.height;

    if (length < width)
    {
      float temp = width;
      width = length;
      length = temp;
    }

    int xbox = minRect[i].center.y;
    int ybox = minRect[i].center.x;

    float dst = sqrt(pow((double)xpos - xbox, 2) + pow((double)ypos - ybox, 2));

    if (!(width > MIN_BOUND_WIDTH && width < MAX_BOUND_WIDTH && length > MIN_BOUND_LENGTH && length < MAX_BOUND_LENGTH)
        || dst > DST_BOUND_THRESH)
      minRect.erase(minRect.begin() + i);
    else
    {
      if (dst < minDst)
      {
        minDst = dst;
        minIndex = i;
      }
      i++;
    }
  }
  *minDstIndex = minIndex;

  return minRect;
}

void imageCalibration::applyBoundingBox(cv::Mat *image, cv::RotatedRect rect)
{
  cv::Point2f rectPoints[4];
  rect.size.width += 8;
  rect.size.height += 8;
  rect.points(rectPoints);
  cv::Point rectPointsConverted[4];
  for (int i = 0; i < 4; i++)
  {
    rectPointsConverted[i].x = (int)rectPoints[i].x;
    rectPointsConverted[i].y = (int)rectPoints[i].y;
  }
  cv::fillConvexPoly(*image, rectPointsConverted, 4, cv::Scalar(255, 255, 255));
}

string imageCalibration::convertInt(int number)
{
  stringstream ss; //create a stringstream
  ss << number; //add number to the stream
  return ss.str(); //return a string with the contents of the stream
}

void imageCalibration::resetForNextFrame()
{
  //free the robotMarkersHead linked list
  freerobotVector();
  //Initializing marker linked list
  robotMarkersVect = *(new vector<markerPoints>());
  robotMarkersVect.push_back(*(new markerPoints()));
  robotMarkersVect[0].numPoints = 0;
}

void imageCalibration::freerobotVector()
{
  for (int x = 0; x < (int)robotMarkersVect.size(); x++)
  {
    robotMarkersVect.erase(robotMarkersVect.begin() + x);
  }
}

int main(int argc, char** argv)
{
  // Initialize ROS
  ros::init(argc, argv, "image_calibration");

  imageCalibration * dispPtr = new imageCalibration();

  ros::Rate loop_rate(20);
  while (ros::ok())
  {
    if (dispPtr->raw_Img1 != NULL && dispPtr->raw_Img2 != NULL)
    {
      dispPtr->processImage();
    }
    ros::spinOnce();
    loop_rate.sleep();
  }

  return (0);
}