void Camera::processFrame()
{
this->capture.read(this->cameraImage);
for (unsigned int i = 0 ; i < this->objectsToTrack.size() ; i++)
{
cvtColor(this->cameraImage, this->HSVImage, COLOR_BGR2HSV);
inRange(this->HSVImage, this->objectsToTrack[i]->getHSVmin(),
this->objectsToTrack[i]->getHSVmax(), this->thresholdImage);
this->morphOps(this->thresholdImage);
this->trackFilteredObject(this->objectsToTrack[i], this->thresholdImage,
this->HSVImage, this->cameraImage);
if (this->head != NULL && i == this->focusObjIndex)
{
Joint *pan = head->getJoint(PAN);
Joint *tilt = head->getJoint(TILT);
float x = this->objectsToTrack[i]->getXPos();
float y = this->objectsToTrack[i]->getYPos();
float half_width = float(CAMERA_WIDTH) / 2.0;
float half_height = float(CAMERA_HEIGHT) / 2.0;
float pan_angle = ((x - half_width) / half_width)*(CAMERA_HORIZONTAL_FIELD/2.0);
float tilt_angle = ((y - half_height) / half_height)*(CAMERA_VERTICAL_FIELD/2.0);
this->focusPan = pan->getAngle() - pan_angle;
this->focusTilt = tilt->getAngle() - tilt_angle;
}
}
imshow("Dimitri's Vision", cameraImage);
}
void Camera::moveHeadToFocus()
{
Joint *pan = head->getJoint(PAN);
Joint *tilt = head->getJoint(TILT);
float deltaPan = this->focusPan - pan->getAngle();
float deltaTilt = this->focusTilt - tilt->getAngle();
deltaPan /= this->focusStepDiv*3.0/5.0;
deltaTilt /= this->focusStepDiv;
pan->setGoalAngle(pan->getAngle() + deltaPan);
tilt->setGoalAngle(tilt->getAngle() + deltaTilt);
}