calibTree.cpp

Go to the documentation of this file.

#include <youbot_overhead_vision/calibTree.h>

calibTree::calibTree()
{
  calibPoints = *(new vector<calib>());
  treeHead = NULL;
  idCount = 0;
}

calibTree::~calibTree()
{
  freeTree();
}

void calibTree::newCalibPoint(uint8_t red, uint8_t green, uint8_t blue, int camera, int calibType)
{
  calibPoints.push_back(*(new calib()));
  calibPoints[calibPoints.size() - 1].r = red;
  calibPoints[calibPoints.size() - 1].g = green;
  calibPoints[calibPoints.size() - 1].b = blue;
  calibPoints[calibPoints.size() - 1].camera = camera;
  calibPoints[calibPoints.size() - 1].calibType = calibType;
  calibPoints[calibPoints.size() - 1].redTreeLeftChild = NULL;
  calibPoints[calibPoints.size() - 1].redTreeLeftChild = NULL;
  idCount++;
  calibPoints[calibPoints.size() - 1].id = idCount;

}

int calibTree::getCalibPointsSize()
{
  return calibPoints.size();
}

int calibTree::getCalibPointColor(int index, int colorIndex)
{
  if (colorIndex == 0)
    return calibPoints[index].r;
  else if (colorIndex == 1)
    return calibPoints[index].g;
  else
    return calibPoints[index].b;
}

int calibTree::getCalibPointCamera(int index)
{
  return calibPoints[index].camera;
}

int calibTree::getCalibPointType(int index)
{
  return calibPoints[index].calibType;
}

void calibTree::removeCalibPoint(int index)
{
  if (getCalibPointsSize() > 1)
    calibPoints.erase(calibPoints.begin() + index);
  else
    calibPoints.pop_back();
}

void calibTree::createTree()
{
  if (getCalibPointsSize() > 0)
    createTreeRecursion(0, getCalibPointsSize());

}

void calibTree::createTreeRecursion(int start, int end)
{
  int range = end - start;
  if (range <= 3)
  {
    if (range >= 2)
    {
      addToTree(start + 1);
    }
    if (range == 3)
    {
      addToTree(start + 2);
    }
    addToTree(start);
  }
  else
  {
    int middleIndex = (end + start) / 2;
    addToTree(middleIndex);
    createTreeRecursion(start, middleIndex);
    createTreeRecursion(middleIndex + 1, end);
  }

}

int calibTree::parseCalibTree(int R, int G, int B, int camera, int calibAccuracy)
{
  return parseCalibTreeRecursion(treeHead, R, G, B, camera, calibAccuracy);
}

int calibTree::parseCalibTreeRecursion(calib * treeNode, int R, int G, int B, int camera, int calibAccuracy)
{
  if (treeNode)
  {
    //Calib Node's R value is in the interval.  As such, the other colors must be checked.  If the other values of
    //that node dont match the pixel colors (R, G, B), both children of that node must be checked.
    if (abs(treeNode->r - R) <= calibAccuracy)
    {
      if ((abs(treeNode->b - B) < calibAccuracy) && (abs(treeNode->g - G) < calibAccuracy)
          && (treeNode->camera == BOTH_CAMERAS || camera == treeNode->camera))
        return treeNode->calibType;
      else
      {
        int left = parseCalibTreeRecursion(treeNode->redTreeLeftChild, R, G, B, camera, calibAccuracy);
        return (
            (left != NOT_SET) ? left :
                (parseCalibTreeRecursion(treeNode->redTreeRightChild, R, G, B, camera, calibAccuracy)));
      }
    }
    //The calib Node's R value is greater than the pixel R value.  As such, the left side of the tree must be scanned
    else if ((treeNode->r - R) > 0)
    {
      return parseCalibTreeRecursion(treeNode->redTreeLeftChild, R, G, B, camera, calibAccuracy);
    }
    //The calib Node's R value is lesser than the pixel R value.  As such, the right side of the tree must be scanned
    else
    {
      return parseCalibTreeRecursion(treeNode->redTreeRightChild, R, G, B, camera, calibAccuracy);
    }

  }
  else //treenode is equal to NULL
  {
    return NOT_SET;
  }
}

void calibTree::orderCalibsByRed()
{
  if (calibPoints.size() > 0)
  {
    //Resets left and right child in preparation for the creation of the new tree
    calibPoints[0].redTreeLeftChild = NULL;
    calibPoints[0].redTreeLeftChild = NULL;
    //Order Calib Points by red using insertion sort:         
  }

  calib * temp = new calib();

  int i;
  for (int j = 1; j < (int)calibPoints.size(); j++)
  {
    temp->r = calibPoints[j].r;
    temp->g = calibPoints[j].g;
    temp->b = calibPoints[j].b;
    temp->camera = calibPoints[j].camera;
    temp->calibType = calibPoints[j].calibType;
    temp->id = calibPoints[j].id;

    i = j - 1;
    while (calibPoints[i].r > temp->r && i >= 0)
    {
      calibPoints[i + 1].r = calibPoints[i].r;
      calibPoints[i + 1].g = calibPoints[i].g;
      calibPoints[i + 1].b = calibPoints[i].b;
      calibPoints[i + 1].camera = calibPoints[i].camera;
      calibPoints[i + 1].calibType = calibPoints[i].calibType;
      calibPoints[i + 1].id = calibPoints[i].id;
      calibPoints[i + 1].redTreeLeftChild = NULL;
      calibPoints[i + 1].redTreeRightChild = NULL;

      i--;
    }
    calibPoints[i + 1].r = temp->r;
    calibPoints[i + 1].g = temp->g;
    calibPoints[i + 1].b = temp->b;
    calibPoints[i + 1].camera = temp->camera;
    calibPoints[i + 1].calibType = temp->calibType;
    calibPoints[i + 1].id = temp->id;
    calibPoints[i + 1].redTreeLeftChild = NULL;
    calibPoints[i + 1].redTreeRightChild = NULL;
  }
}

bool calibTree::removeLastNodeFromTree(int cameraShown)
{

  int size = getCalibPointsSize();
  if (size > 0)
  {
    int highestIndex = 0;
    for (int itt = 0; itt < size; itt++)
    {
      if (calibPoints[itt].id > calibPoints[highestIndex].id)
        highestIndex = itt;
    }

    if (removeLastNodeFromTreeRecursion(treeHead, getCalibPointColor(highestIndex, RED),
                                        getCalibPointColor(highestIndex, GREEN), getCalibPointColor(highestIndex, BLUE),
                                        calibPoints[highestIndex].id))
    {
      removeCalibPoint(highestIndex);
      idCount--;
      if (getCalibPointsSize() == 0)
      {
        freeTree();
      }
      return true;
    }

  }
  return false;
}

bool calibTree::removeLastNodeFromTreeRecursion(calib * nodePtr, int r, int g, int b, int id)
{
  if (!nodePtr)
    return false;

  //If the node being looked at is the one that needs to be removed, This removes it.  
  if (nodePtr->id == id)
  {
    //If the the node being removed has child nodes, one of the child nodes must be used to take the position of the node 
    //being destroyed in the tree. This child node then has to be removed to ensure there is no doubles in the tree.
    calib * replacementNode;
    if ((replacementNode = nodePtr->redTreeLeftChild))
    {
      while (replacementNode->redTreeRightChild)
      {
        replacementNode = replacementNode->redTreeRightChild;
      }
    }
    else if ((replacementNode = nodePtr->redTreeRightChild))
    {
      while (replacementNode->redTreeLeftChild)
      {
        replacementNode = replacementNode->redTreeLeftChild;
      }
    }
    else //Removes the node from the tree if the node has no children.
    {

      //Removes the link to the node from the parent node.
      if (nodePtr->redTreeParent)
      {
        calib * parent = nodePtr->redTreeParent;
        //If node being removed is the left child of its parent, remove left node. else, remove right node.
        if (parent->redTreeLeftChild && parent->redTreeLeftChild->r == r && parent->redTreeLeftChild->g == g
            && parent->redTreeLeftChild->b == b)
          parent->redTreeLeftChild = NULL;
        else
          parent->redTreeRightChild = NULL;
      }
      delete (nodePtr);

      return true;
    }

    //Replace Node Info with replacement Node Info
    nodePtr->r = replacementNode->r;
    nodePtr->g = replacementNode->g;
    nodePtr->b = replacementNode->b;
    nodePtr->camera = replacementNode->camera;
    nodePtr->calibType = replacementNode->calibType;
    nodePtr->id = replacementNode->id;

    //Remove Replacement Node
    return removeLastNodeFromTreeRecursion(replacementNode, replacementNode->r, replacementNode->g, replacementNode->b,
                                           replacementNode->id);
  }

  else //Searches tree for the node with values r, g, b
  {
    if (r < nodePtr->r && nodePtr->redTreeLeftChild)
      return removeLastNodeFromTreeRecursion(nodePtr->redTreeLeftChild, r, g, b, id);
    else if (r > nodePtr->r && nodePtr->redTreeRightChild)
      return removeLastNodeFromTreeRecursion(nodePtr->redTreeRightChild, r, g, b, id);
    else
    {
      bool foundRight = false, foundLeft = false;
      if (nodePtr->redTreeLeftChild)
      {
        foundLeft = removeLastNodeFromTreeRecursion(nodePtr->redTreeLeftChild, r, g, b, id);
      }
      if (nodePtr->redTreeRightChild)
      {
        foundRight = removeLastNodeFromTreeRecursion(nodePtr->redTreeRightChild, r, g, b, id);
      }
      return foundRight || foundLeft;
    }
  }

  return false;
}

void calibTree::freeTree()
{
  if (getCalibPointsSize() > 0)
    freeTreeRecursion(treeHead);
  else
    treeHead = NULL;
}

void calibTree::freeTreeRecursion(calib * node)
{
  if (node)
  {
    freeTreeRecursion(node->redTreeLeftChild);
    freeTreeRecursion(node->redTreeRightChild);
    delete (node);
  }
  treeHead = NULL;
}

void calibTree::addToTree(int calibNode)
{
  //If this is the first node found, assign the head of the trees to that node.
  if (!treeHead)
  {
    createTreeNode(calibNode, treeHead, NULL);

    treeHead = new calib();
    treeHead->r = calibPoints[calibNode].r;
    treeHead->g = calibPoints[calibNode].g;
    treeHead->b = calibPoints[calibNode].b;
    treeHead->camera = calibPoints[calibNode].camera;
    treeHead->calibType = calibPoints[calibNode].calibType;
    treeHead->id = calibPoints[calibNode].id;

    treeHead->redTreeLeftChild = NULL;
    treeHead->redTreeRightChild = NULL;
    treeHead->redTreeParent = NULL;
    return;
  }
  else
  {
    //Right now, the head of the tree is always the first node
    struct calib * redTreePtr = treeHead;

    //Adding nodes to all three trees
    bool nodeAddedFlag = false;
    while (!nodeAddedFlag)
    { //Node being added smaller than or equal to the node in tree.  Look at left child. 
      if (redTreePtr->r >= calibPoints[calibNode].r)
      {
        if (!redTreePtr->redTreeLeftChild)
        {
          createTreeNode(calibNode, redTreePtr->redTreeLeftChild, redTreePtr);
          nodeAddedFlag = true;
        }
        else
        {
          redTreePtr = redTreePtr->redTreeLeftChild;
        }
      }
      else //Node being added larger than the node in tree.  Look at right child.
      {
        if (!redTreePtr->redTreeRightChild)
        {
          createTreeNode(calibNode, redTreePtr->redTreeRightChild, redTreePtr);
          nodeAddedFlag = true;
        }
        else
        {
          redTreePtr = redTreePtr->redTreeRightChild;
        }
      }
    }
  }
}

void calibTree::createTreeNode(int vectorIndex, calib * nodePtr, calib * parentPtr)
{
  nodePtr = new calib();
  nodePtr->r = calibPoints[vectorIndex].r;
  nodePtr->g = calibPoints[vectorIndex].g;
  nodePtr->b = calibPoints[vectorIndex].b;
  nodePtr->camera = calibPoints[vectorIndex].camera;
  nodePtr->calibType = calibPoints[vectorIndex].calibType;
  nodePtr->id = calibPoints[vectorIndex].id;

  nodePtr->redTreeLeftChild = NULL;
  nodePtr->redTreeRightChild = NULL;
  nodePtr->redTreeParent = parentPtr;

  if (parentPtr)
  {
    if (parentPtr->r >= nodePtr->r)
      parentPtr->redTreeLeftChild = nodePtr;
    else
      parentPtr->redTreeRightChild = nodePtr;
  }
}

void calibTree::printTree()
{
  if (getCalibPointsSize() > 0)
    printTreeRecursion(treeHead, 0);
}

void calibTree::printTreeRecursion(calib * node, int level)
{
  if (level == 0)
    ROS_INFO("Printing Tree:");
  if (node)
  {
    ROS_INFO(
        "calibType: %d R %d G %d B %d Camera: %d ID: %d Level: %d Child pts: %p %p", node->calibType, node->r, node->g, node->b, node->camera, node->id, level, node->redTreeLeftChild, node->redTreeRightChild);

    printTreeRecursion(node->redTreeLeftChild, level + 1);
    printTreeRecursion(node->redTreeRightChild, level + 1);
  }
}