FaceFind.cpp

Go to the documentation of this file.

#include <FaceFind.h>
#include <Global.h>

// cv image bridge
#include <cv_bridge/CvBridge.h>
#include <opencv2/highgui/highgui.hpp>

//laser and point cloud
#include <pcl/io/pcd_io.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/image_encodings.h>
#include <pr2_msgs/SetPeriodicCmd.h>

//moving the head
#include <actionlib/client/simple_action_client.h>
#include <pr2_controllers_msgs/PointHeadAction.h>
#include <cv_bridge/cv_bridge.h>

typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient; 

// colors
typedef cv::Vec<uchar, 3> Vec3b;
cv::Vec3b const BLACK = Vec3b(0, 0, 0); 
cv::Vec3b const WHITE = Vec3b(255, 255, 255); 
cv::Vec3b const BLUE = Vec3b(255, 0, 0); 
cv::Vec3b const GREEN = Vec3b(0, 255, 0); 

FaceFind::FaceFind() {
        facesQueue_ = std::deque<FaceWrapper>();

        model_ = image_geometry::PinholeCameraModel();
        prosilicamodel_ = image_geometry::PinholeCameraModel();

        headBusy_ = false;
        mouseClicked_ = 0;
        mirror_ = false;
        mirroredMaskExists_ = false;
        drawingCorners_ = 0;

        lastCameraImage_ = ros::Time(0);
}

// --------------------------------- begin of callback functions --------------------------------- //

void FaceFind::ProsilicaCBAll(const sensor_msgs::Image::ConstPtr &image) {

        try {

        cv::Mat cv_image_ = cv_bridge::toCvCopy(image,
                          sensor_msgs::image_encodings::BGR8)->image;

//        sensor_msgs::CvBridge newbridge;
//              cv::Mat cv_image_(newbridge.imgMsgToCv(image, "bgr8"));
                storedProsilicaImage_ = cv_image_.clone();
        } catch (...) {
                ROS_ERROR("error in prosilica image receiving, image ignored");
                return;
        }
}

void FaceFind::ImageCBAll(const sensor_msgs::Image::ConstPtr &limage) {

        try {
//              sensor_msgs::CvBridge newbridge;
//              cv::Mat cv_image_left_(newbridge.imgMsgToCv(limage, "bgr8"));

        cv::Mat cv_image_left_ = cv_bridge::toCvCopy(limage,
                          sensor_msgs::image_encodings::BGR8)->image;
                try {
                        //** save image for face positions function to work on */
                        if (limage->header.stamp - lastCameraImage_ >= ros::Duration(0)) {
                                storedImage_ = cv_image_left_.clone();
                                lastCameraImage_ = limage->header.stamp;
                        }
                } catch (...) {
                        return;
                }

        } catch (...) {
                ROS_ERROR("error in wide_stereo image receiving, ignoring image");
                return;
        }

        //** read faces from deque and check if the faces are too old (in this case delete them) */
        for (uint index = 0; index < facesQueue_.size(); ++index) {

                ros::Time begin = ros::Time::now();
                ros::Duration age = begin - facesQueue_[index].GetTime();

                //** retrieve a face position */
                geometry_msgs::Point p = facesQueue_[index].GetPosition();
                cv::Point3d frame_pos = cv::Point3d(p.x, p.y, p.z);

                //** test if face position is too old */
                if (age.sec >= 2 && !headBusy_) {

                        facesQueue_.erase(facesQueue_.begin() + index);
                        index--;

                } else {

                        //** convert the face position to a screen pixel coordinate */
                        cv::Point2d pixel_pos;
                        pixel_pos = model_.project3dToPixel(frame_pos);

                        //** norm portrait size */
                        int sizex = 35 / frame_pos.z * 1.6; //determined by science.
                        int sizey = 45 / frame_pos.z * 1.6;

                        //** edge color depending on if the mouse cursor is in the face or not */
                        CvScalar edgeColor;
                        if (CheckFaceBox((int) pixel_pos.x - sizex,
                                        (int) pixel_pos.y - sizey, (int) pixel_pos.x + sizex,
                                        (int) pixel_pos.y + sizey)) {
                                edgeColor = CV_RGB(0, 0, 150);

                                if (mouseClicked_ != 0) {
                                        StartPhoto(index);
                                }

                        } else
                                edgeColor = CV_RGB(0, 0, 0);

                        //** visualize face in image */
                        cv::rectangle(
                                        storedImage_,
                                        cv::Point((int) pixel_pos.x - sizex,
                                                        (int) pixel_pos.y - sizey),
                                        cv::Point((int) pixel_pos.x + sizex,
                                                        (int) pixel_pos.y + sizey), edgeColor, 2);

                }
        }

        //display image
        Q_EMIT newCameraImage(cvMat2QImage(storedImage_, 0));

}

void FaceFind::FacePosCBAll(
                const people_msgs::PositionMeasurement::ConstPtr &position) {
        facesQueue_.push_back(FaceWrapper(position->pos, position->header.stamp));

}

void FaceFind::CameraModelCBAll(
                const sensor_msgs::CameraInfoConstPtr& cam_info) {
        model_.fromCameraInfo(cam_info);

}

void FaceFind::ProsilicaCameraModelCBAll(
                const sensor_msgs::CameraInfoConstPtr& cam_info) {
        prosilicamodel_.fromCameraInfo(cam_info);

}

bool FaceFind::StatusMessageCBAll(
                portrait_robot_msgs::alubsc_status_msg::Request &req,
                portrait_robot_msgs::alubsc_status_msg::Response &res) {
        Q_EMIT newStatusMessage((int) req.ID, QString(req.message.c_str()));

        return true;
}

void FaceFind::PhotoFromFile(const char* filename1, const char* filename2) {
        storedPhoto_ = cv::imread(filename1, 1); //force 3-channel
        cv::Mat resizedPhoto, resizedMask;
        cv::resize(storedPhoto_, resizedPhoto, cv::Size(PORTRAIT_X, PORTRAIT_Y));

        if (mirror_) {
                storedMirroredMaskImage_ = cv::imread(filename2, 1); //force 3-channel
                cv::resize(storedMirroredMaskImage_, resizedMask,
                                cv::Size(PORTRAIT_X, PORTRAIT_Y));
        } else {
                storedMaskImage_ = cv::imread(filename2, 1); //force 3-channel
                cv::resize(storedMaskImage_, resizedMask,
                                cv::Size(PORTRAIT_X, PORTRAIT_Y));
        }

        Q_EMIT
        newPhoto(cvMat2QImage(resizedPhoto, 1));
        Q_EMIT
        newMask(cvMat2QImage(resizedMask, 2));
        Q_EMIT activateButton(2);
}
void FaceFind::EdgeImageFromFile(const char* filename) {
        edgeImage_ = cv::imread(filename, 1); //force 3-channel
        cv::Mat resizedImage;
        cv::resize(edgeImage_, resizedImage, cv::Size(PORTRAIT_X, PORTRAIT_Y));
        Q_EMIT
        newMask(cvMat2QImage(resizedImage, 3));
        Q_EMIT activateButton(3);
}

bool FaceFind::EdgeImageCBAll(
                portrait_robot_msgs::alubsc_node_instr::Request &req,
                portrait_robot_msgs::alubsc_node_instr::Response &res) {
        if (req.images.size() > 0) {
                sensor_msgs::Image& image = req.images.at(0);

                try {
                        sensor_msgs::CvBridge newbridge;

                        if (!newbridge.fromImage(image, "bgr8"))
                                throw sensor_msgs::CvBridgeException(
                                                "Conversion to OpenCV image failed");
                        else {
                                edgeImage_ = cv::Mat(newbridge.toIpl(), true);
                                cv::Mat resizedImage;
                                cv::resize(edgeImage_, resizedImage,
                                                cv::Size(PORTRAIT_X, PORTRAIT_Y));
                                Q_EMIT
                                newMask(cvMat2QImage(resizedImage, 3));
                                Q_EMIT
                                activateButton(3);
                                cv::imwrite("last_edge_image.png", edgeImage_);
                                res.success = true;
                                return true;
                        }
                } catch (...) {
                        ROS_ERROR("ERROR: Could not convert the edge image for receiving.");
                        Q_EMIT
                        setGUIStatus(
                                        "ERROR: Could not convert the edge image for receiving.");
                        res.success = false;
                        return false;
                }
        } else {
                res.success = false;
                return false;
        }
}

void FaceFind::PenButton() {
        portrait_robot_msgs::alubsc_node_instr srv;
        srv.request.abort = false;

        if (pen_gripper_client.exists()) {
                pen_gripper_client.call(srv);
                Q_EMIT activateButton(1);
        } else
                Q_EMIT newStatusMessage(1,
                                QString("<font color=red>Client is not ready.</font>"));
}

void FaceFind::EdgeButton() {
        portrait_robot_msgs::alubsc_node_instr srv;
        srv.request.abort = false;
        std::vector<sensor_msgs::Image> imageArray(2);
        IplImage image1 = storedPhoto_;
        IplImage image2;
        if (mirror_)
                image2 = storedMirroredMaskImage_;
        else
                image2 = storedMaskImage_;

        cv::imwrite("last_photo.png", cv::Mat(&image1));
        cv::imwrite("last_mask.png", cv::Mat(&image2));
        try {
                sensor_msgs::CvBridge newbridge;

                imageArray.at(0) = *(newbridge.cvToImgMsg(&image1));
                imageArray.at(1) = *(newbridge.cvToImgMsg(&image2));

                srv.request.images = imageArray;

                if (contour_detector_client.exists()) {
                        contour_detector_client.call(srv);
                } else
                        Q_EMIT newStatusMessage(2,
                                        QString("<font color=red>Client is not ready.</font>"));
        } catch (...) {
                ROS_ERROR("ERROR: Could not convert the photo and mask image.");
                Q_EMIT
                setGUIStatus("ERROR: Could not convert the photo and mask image.");
                return;
        }
}

void FaceFind::CornerButton() {
        portrait_robot_msgs::alubsc_node_instr srv;
        srv.request.abort = false;

        switch (drawingCorners_) {
        case 0:
                if (corner_client.exists()) {
                        corner_client.call(srv);
                        ++drawingCorners_;
                } else
                        Q_EMIT newStatusMessage(3,
                                        QString("<font color=red>Client is not ready.</font>"));
                break;

        case 1:
                if (corner_client.exists()) {
                        corner_client.call(srv);
                        ++drawingCorners_;
                } else
                        Q_EMIT newStatusMessage(3,
                                        QString("<font color=red>Client is not ready.</font>"));
                break;

        case 2:
                if (corner_client.exists()) {
                        corner_client.call(srv);
                        ++drawingCorners_;
                } else
                        Q_EMIT newStatusMessage(3,
                                        QString("<font color=red>Client is not ready.</font>"));
                break;

        case 3:
                if (corner_client.exists()) {
                        corner_client.call(srv);
                        drawingCorners_ = 0;
                        Q_EMIT
                        newStatusMessage(
                                        3,
                                        QString(
                                                        "<font color=red>Please make sure that the robot has a pen before you start painting.</font>"));
                        Q_EMIT activateButton(4);
                } else
                        Q_EMIT newStatusMessage(3,
                                        QString("<font color=red>Client is not ready.</font>"));
                break;

        default:
                Q_EMIT setGUIStatus(QString("ERROR: Wrong parameter!"));
                break;
        }
}

void FaceFind::PaintButton() {
        portrait_robot_msgs::alubsc_node_instr srv;
        srv.request.abort = false;

        if (drawingCorners_ == 0) {
                std::vector<sensor_msgs::Image> imageArray(1);
                IplImage image1 = edgeImage_;
                try {
                        sensor_msgs::CvBridge newbridge;

                        imageArray.at(0) = *(newbridge.cvToImgMsg(&image1));

                        srv.request.images = imageArray;

                        if (painter_client.exists()) {
                                painter_client.call(srv);
                        } else
                                Q_EMIT newStatusMessage(3,
                                                QString("<font color=red>Client is not ready.</font>"));
                } catch (...) {
                        ROS_ERROR("ERROR: Could not convert the edge image for sending.");
                        Q_EMIT
                        setGUIStatus("ERROR: Could not convert edge image for sending.");
                        return;
                }
        } else {
                Q_EMIT setGUIStatus(QString("ERROR: Wrong parameter!"));
        }
}

void FaceFind::HeadButton() {
        Q_EMIT setGUIInfo("Turning the head.");

        //the goal message we will be sending
        pr2_controllers_msgs::PointHeadGoal goal;

        //the target point, expressed in the requested frame
        geometry_msgs::PointStamped point;
        point.header.frame_id = "/torso_lift_link";
        point.point.x = 2;
        point.point.y = 0;
        point.point.z = 0.5;
        goal.target = point;

        goal.pointing_frame = "high_def_optical_frame";

        //take at least 0.5 seconds to get there
        goal.min_duration = ros::Duration(0.5);

        //and go no faster than 1 rad/s
        goal.max_velocity = 1.0;

        PointHeadClient* point_head_client_ = new PointHeadClient(
                        "/head_traj_controller/point_head_action", true);
        while (!point_head_client_->waitForServer(ros::Duration(5.0))) {
                //wait
        }

        //send the goal
        point_head_client_->sendGoal(goal);

        //wait for it to get there (abort after 2 secs to prevent getting stuck)
        point_head_client_->waitForResult(ros::Duration(2));
}

void FaceFind::AbortButton() {
        portrait_robot_msgs::alubsc_node_instr srv;
        srv.request.abort = true;

        if (contour_detector_client.exists())
                contour_detector_client.call(srv);

        if (painter_client.exists())
                painter_client.call(srv);
        drawingCorners_ = 0;

        if (corner_client.exists())
                corner_client.call(srv);

        Q_EMIT setGUIStatus(QString("Aborted the application."));
}

// --------------------------------- end of callback functions --------------------------------- //

void FaceFind::StartPhoto(uint index) {

        int myclicked = mouseClicked_;
        mouseClicked_ = 0;

        if (facesQueue_.size() == 0) {
                Q_EMIT setGUIStatus("There was no face found. Please try again.");
                return;
        } else if (index >= facesQueue_.size()) {
                Q_EMIT setGUIStatus(
                                "The face is not available any more. Please try again.");
                return;
        }

        if (!headBusy_) {

                headBusy_ = true;

                if (myclicked == 1) {

                        cv::Point2d pixel_pos;
                        geometry_msgs::Point p = facesQueue_[index].GetPosition();
                        cv::Point3d frame_pos = cv::Point3d(p.x, p.y, p.z);

                        AdjustLaser(p);

                        TurnHead(frame_pos);

                } else if (myclicked == 2) {

                        TakePhoto(index);

                }

                headBusy_ = false;

        }

}

void FaceFind::TakePhoto(uint index) {
        // if 'false', portraits will be extracted from the wide_stereo images
        // but we only use the prosilica, so it's true anyway (other users may prefer otherwise)
        bool prosilica = true;

        cv::Mat currentImage;
        cv::Point2d pixel_pos;
        geometry_msgs::Point p = facesQueue_[index].GetPosition();
        cv::Point3d frame_pos = cv::Point3d(p.x, p.y, p.z);

        tf::TransformListener tf;

        // if the prosilica is to be used, convert the point into prosilica frame
        if (prosilica) {
                try {
                        geometry_msgs::PointStamped ps;
                        geometry_msgs::PointStamped ps2;
                        ps.header.frame_id = "wide_stereo_optical_frame";
                        ps.point = p;
                        ros::Time time = ros::Time(0);
                        tf.waitForTransform("high_def_optical_frame",
                                        "/wide_stereo_optical_frame", time, ros::Duration(1.5));
                        tf.transformPoint("high_def_optical_frame", time, ps, "base_link",
                                        ps2);
                        frame_pos = cv::Point3d(ps2.point.x, ps2.point.y, ps2.point.z);
                } catch (...) {
                        ROS_ERROR("error transforming");
                        Q_EMIT
                        setGUIStatus("* Error in the transformation! *");
                        return;
                }
        }

        // for wide.stereo images it is model_ and storedImage_, for prosilica it is prosilicamodel_ and storedProsilicaImage_
        if (prosilica) {
                pixel_pos = prosilicamodel_.project3dToPixel(frame_pos);
        } else {
                pixel_pos = model_.project3dToPixel(frame_pos);
        }
        currentImage = (
                        prosilica ? storedProsilicaImage_.clone() : storedImage_.clone());

        //** norm portrait size */
        int sizex = 35 / facesQueue_[index].GetPosition().z * (prosilica ? 9 : 1.6);
        int sizey = 45 / facesQueue_[index].GetPosition().z * (prosilica ? 9 : 1.6);

        //the portrait portion of the image
        CvRect roi;
        roi.x = pixel_pos.x - sizex;
        roi.y = pixel_pos.y - (sizey * 1.2);
        roi.width = 2 * sizex;
        roi.height = 2 * sizey;

        pcl::PointCloud<pcl::PointXYZ> storedPointCloud2;
        pcl::PointCloud<pcl::PointXYZ> newPointCloud;

        try {

                sensor_msgs::PointCloud2 cloud2_msg;
                laser_assembler::AssembleScans srv;
                srv.request.begin = ros::Time::now() - ros::Duration(5, 0); /*first number: assemble scanlines back to when in the past?*/
                srv.request.end = ros::Time::now();

                if (laserscanner_client.call(srv)) {

                        sensor_msgs::convertPointCloudToPointCloud2(srv.response.cloud,
                                        cloud2_msg);
                        pcl::fromROSMsg(cloud2_msg, storedPointCloud2);

                } else {
                        ROS_ERROR("error pointcloud");
                        Q_EMIT setGUIStatus("* Error in the pointcloud! *");
                }

                tf::StampedTransform transform;
                try {

                        ros::Time time = ros::Time::now();

                        /* NOTE: there are also "laser_tilt_link" and "laser_tilt_mount_link", but laser_assember
                         * launchfile indicated that laser scan data is converted to base_link frame in accumulation/pointcloud assembly */
                        if (prosilica) {
                                tf.waitForTransform("/high_def_optical_frame", "/base_link",
                                                time, ros::Duration(0.5));
                                tf.lookupTransform("/high_def_optical_frame", "/base_link",
                                                time, transform);
                        } else {
                                tf.waitForTransform("/wide_stereo_optical_frame", "/base_link",
                                                time, ros::Duration(0.5));
                                tf.lookupTransform("/wide_stereo_optical_frame", "/base_link",
                                                time, transform);
                        }

                } catch (tf::TransformException ex) {
                        ROS_ERROR("%s", ex.what());
                        Q_EMIT
                        setGUIStatus(tr(ex.what()));
                        return;
                } catch (...) {
                        ROS_ERROR("error in transform acquisition");
                        Q_EMIT
                        setGUIStatus("* Error in the transform acquisition! *");
                        return;
                }

                try {
                        pcl_ros::transformPointCloud(storedPointCloud2, newPointCloud,
                                        transform);
                } catch (...) {
                        ROS_ERROR("error in transforming");
                        Q_EMIT
                        setGUIStatus("* Error in the pointcloud transformation! *");
                        return;
                }

        } catch (cv::Exception& e) {

                ROS_ERROR("%s", e.what());
                Q_EMIT
                setGUIStatus(tr(e.what()));
                return;
        }

        catch (...) {
                ROS_ERROR("real error in transforming");
                Q_EMIT setGUIStatus(
                                "* Error (big one) in the pointcloud transformation! *");
        }

        if (roi.x < 0 || roi.y < 0 || roi.x + roi.width >= currentImage.cols
                        || roi.y + roi.height >= currentImage.rows) {

                Q_EMIT setGUIStatus("* The image values breach the border! *");

        } else {

                storedPhoto_ = currentImage(roi);

                //a mask image
                storedMaskImage_ = storedPhoto_.clone();
                storedMaskImage_ = CV_RGB(255, 255, 255);

                std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ> >::iterator iter;
                for (iter = newPointCloud.begin(); iter != newPointCloud.end();
                                ++iter) {
                        try {

                                cv::Point2d pixel;
                                pcl::PointXYZ p = *iter;
                                cv::Point3d point = cv::Point3d(p.x, p.y, p.z);

                                //wide: model_, prosilica: prosilicamodel_
                                if (prosilica) {
                                        pixel = prosilicamodel_.project3dToPixel(point);
                                } else {
                                        pixel = model_.project3dToPixel(point);
                                }

                                //if the projected pixel is outside the portrait, do nothing
                                if (pixel.x < roi.x + 2 || pixel.x >= roi.x + roi.width - 2
                                                || pixel.y < roi.y + 2
                                                || pixel.y >= roi.y + roi.height - 2) {

                                        continue;

                                } else {

                                        //blacken all points outside an epsilon-environment along z axis
                                        if (point.z >= frame_pos.z - 0.5
                                                        && point.z < frame_pos.z + 0.5) {
                                                cv::Point p = cv::Point(pixel.x - roi.x,
                                                                pixel.y - roi.y);

                                                cv::line(storedMaskImage_, p, p, CV_RGB(0, 0, 0));
                                        }

                                }

                        } catch (...) {
                                ROS_ERROR("ERROR: error in pointcloud stuff");
                                Q_EMIT
                                setGUIStatus("* Error in pointcloud! *");
                                return;
                        }
                }

                MakeMask(storedMaskImage_);
                mirroredMaskExists_ = false;

                cv::Mat resizedPhoto, resizedMask;

                cv::resize(storedPhoto_, resizedPhoto,
                                cv::Size(PORTRAIT_X, PORTRAIT_Y));

                if (mirror_) {
                        MirrorPointCloud();

                        cv::resize(storedMirroredMaskImage_, resizedMask,
                                        cv::Size(PORTRAIT_X, PORTRAIT_Y));
                } else {
                        cv::resize(storedMaskImage_, resizedMask,
                                        cv::Size(PORTRAIT_X, PORTRAIT_Y));
                }

                Q_EMIT
                newPhoto(cvMat2QImage(resizedPhoto, 1));
                Q_EMIT
                newMask(cvMat2QImage(resizedMask, 2));
                Q_EMIT
                setGUIInfo("The photo was successfully taken.");
                Q_EMIT activateButton(2);
        }
}

void FaceFind::TurnHead(cv::Point3d facecenter) {
        Q_EMIT setGUIInfo("Turning the head.");

        //the goal message we will be sending
        pr2_controllers_msgs::PointHeadGoal goal;

        //the target point, expressed in the requested frame
        geometry_msgs::PointStamped point;
        point.header.frame_id = "/wide_stereo_optical_frame";
        point.point.x = facecenter.x;
        point.point.y = facecenter.y;
        point.point.z = facecenter.z;
        goal.target = point;

        //we are pointing the high-def camera frame
        //(pointing_axis defaults to X-axis)
        goal.pointing_frame = "high_def_optical_frame";

        //take at least 0.5 seconds to get there
        goal.min_duration = ros::Duration(0.5);

        //and go no faster than 1 rad/s
        goal.max_velocity = 1.0;

        PointHeadClient* point_head_client_ = new PointHeadClient(
                        "/head_traj_controller/point_head_action", true);
        while (!point_head_client_->waitForServer(ros::Duration(5.0))) {
                //wait
        }

        //send the goal
        point_head_client_->sendGoal(goal);

        //wait for it to get there (abort after 2 secs to prevent getting stuck)
        point_head_client_->waitForResult(ros::Duration(2));
}

void FaceFind::MakeMask(cv::Mat& maskImage) {
        cv::Mat_<cv::Vec3b>& copy = (cv::Mat_<cv::Vec3b>&) maskImage;

        for (int x = 0; x < copy.cols; ++x) {
                for (int y = 0; y < copy.rows; ++y) {

                        bool found = false;
                        int blobsize = 8 * copy.cols / 350;

                        for (int deltax = -blobsize; deltax < blobsize; ++deltax) {
                                if (found)
                                        break;

                                for (int deltay = -blobsize; deltay < blobsize; ++deltay) {
                                        if (found)
                                                break;

                                        //diamond blob shape
                                        //if(abs(deltax)+abs(deltay) > blobsize) continue;
                                        //circular blob shape
                                        if (deltax * deltax + deltay * deltay > blobsize * blobsize)
                                                continue;

                                        if (x + deltax < 0 || x + deltax >= copy.cols
                                                        || y + deltay < 0 || y + deltay >= copy.rows)
                                                continue;
                                        if (copy(y + deltay, x + deltax) == BLACK)
                                                found = true;
                                }
                        }

                        if (found)
                                copy(y, x) = (copy(y, x) == BLACK ? BLACK : BLUE);
                }
        }

        //cluster in all four directions
        Cluster(copy, 0, copy.cols, 1, 0, copy.rows, 1);
        Cluster(copy, copy.cols - 1, -1, -1, 0, copy.rows, 1);
        Cluster(copy, 0, copy.cols, 1, copy.rows - 1, -1, -1);
        Cluster(copy, copy.cols - 1, -1, -1, copy.rows - 1, -1, -1);

        //set mask to black/white
        for (int x = 0; x < copy.cols; ++x) {
                for (int y = 0; y < copy.rows; ++y) {
                        if (copy(y, x) != WHITE)
                                copy(y, x) = BLACK;
                }
        }
}

void FaceFind::Cluster(cv::Mat_<cv::Vec3b>& copy, int n_x, int border_x,
                int inc_x, int n_y, int border_y, int inc_y) {
        for (int x = n_x; x != border_x; x += inc_x) {
                for (int y = n_y; y != border_y; y += inc_y) {

                        if (copy(y, x) != WHITE)
                                continue;

                        int found = 0;
                        int maximum = 0;
                        int blobsize = 10 * copy.cols / 350;

                        for (int deltax = -blobsize; deltax < blobsize; ++deltax) {
                                for (int deltay = -blobsize; deltay < blobsize; ++deltay) {

                                        //circular blob shape
                                        if (deltax * deltax + deltay * deltay > blobsize * blobsize)
                                                continue;

                                        if (x + deltax < 0 || x + deltax >= copy.cols
                                                        || y + deltay < 0 || y + deltay >= copy.rows)
                                                continue;

                                        ++maximum;
                                        if (copy(y + deltay, x + deltax) != WHITE)
                                                ++found;

                                }
                        }
                        if (found >= 0.7 * maximum)
                                copy(y, x) = GREEN;
                }
        }
}

void FaceFind::AdjustLaser(geometry_msgs::Point p) {
        Q_EMIT setGUIInfo("Adjusting the laser.");

        pr2_msgs::SetPeriodicCmd srv;
        pr2_msgs::PeriodicCmd pc;
        pc.profile = "linear";
        pc.period = 0.5;
        pc.amplitude = 0.;
        pc.offset = 0.;
        srv.request.command = pc;

        try {
                if (lasertilt_client.call(srv)) {

                } else {
                        ROS_ERROR(
                                        (laserscanner_client.exists()?"but scanner client exists":"and scanner client doesn't exist"));
                }
        } catch (...) {
                ROS_ERROR("error halting laser scanner");
                Q_EMIT
                setGUIStatus("* Error in the laser scanner (1)! *");
                return;
        }

        cv::Point3d pos;

        try {
                tf::TransformListener tf;
                geometry_msgs::PointStamped ps;
                geometry_msgs::PointStamped ps2;
                ps.header.frame_id = "wide_stereo_optical_frame";
                ps.point = p;
                ros::Time time = ros::Time(0);
                tf.waitForTransform("/laser_tilt_mount_link",
                                "/wide_stereo_optical_frame", time, ros::Duration(1.5));
                tf.transformPoint("/laser_tilt_mount_link", time, ps, "base_link", ps2);
                pos = cv::Point3d(ps2.point.x, ps2.point.y, ps2.point.z);
        } catch (...) {
                ROS_ERROR("error transforming");
                Q_EMIT
                setGUIStatus("* Error in the laser scanner transformation! *");
                return;
        }

        float tangens_alpha = pos.z / pos.x;
        float alpha = acos(tangens_alpha);
        float amplitude;
        if (pos.x < 1) {
                amplitude = 0.2;
        } else {
                amplitude = 0.2 / pos.x;
        }

        pc.profile = "linear";
        pc.period = 5;
        pc.amplitude = amplitude;
        pc.offset = -(1.57 - alpha);
        srv.request.command = pc;
        try {
                if (lasertilt_client.call(srv)) {

                } else {
                        ROS_ERROR(
                                        (laserscanner_client.exists()?"but scanner client exists2":"and scanner client doesn't exist2"));
                }
        } catch (...) {
                ROS_ERROR("error starting laser scanner");
                Q_EMIT
                setGUIStatus("* Error in the laser scanner (2)! *");
                return;
        }
}

void FaceFind::ReceiveMouseEvent(int x, int y, int clicked) {
        lastMousePosition_ = cv::Point(x, y);

        mouseClicked_ = clicked;
}

bool FaceFind::CheckFaceBox(int x1, int y1, int x2, int y2) {
        return (x1 < lastMousePosition_.x && y1 < lastMousePosition_.y
                        && x2 > lastMousePosition_.x && y2 > lastMousePosition_.y);
}

void FaceFind::Mirror() {
        mirror_ = !mirror_;

        // if no mask image exists, return
        if (storedMaskImage_.total() == 0)
                return;

        cv::Mat newResizedMaskImage;

        if (mirror_) {
                MirrorPointCloud();

                cv::resize(storedMirroredMaskImage_, newResizedMaskImage,
                                cv::Size(PORTRAIT_X, PORTRAIT_Y));
        } else {
                cv::resize(storedMaskImage_, newResizedMaskImage,
                                cv::Size(PORTRAIT_X, PORTRAIT_Y));
        }

        Q_EMIT newMask(cvMat2QImage(newResizedMaskImage, 2));
}

void FaceFind::MirrorPointCloud() {
        // if no mask image exists, return
        if (storedMaskImage_.total() == 0)
                return;

        if (!mirroredMaskExists_) {
                cv::Mat_<cv::Vec3b>& copy = (cv::Mat_<cv::Vec3b>&) storedMaskImage_;
                storedMirroredMaskImage_ = storedMaskImage_.clone();
                cv::Point p;

                for (int x = 0; x < storedMirroredMaskImage_.cols; ++x) {
                        for (int y = 0; y < storedMirroredMaskImage_.rows; ++y) {
                                if ((copy(y, x) == BLACK)
                                                && (copy(y, storedMaskImage_.cols - x - 1) != BLACK)) {
                                        p = cv::Point(storedMirroredMaskImage_.cols - x - 1, y);
                                        cv::line(storedMirroredMaskImage_, p, p, CV_RGB(0, 0, 0));
                                }
                        }
                }

                mirroredMaskExists_ = true;
        }
}

QImage FaceFind::cvMat2QImage(const cv::Mat& image, unsigned int idx) {
        try {
                if (rgbaBuffer_.size() <= idx) {
                        rgbaBuffer_.resize(idx + 1);
                }

                if (image.rows != rgbaBuffer_[idx].rows
                                || image.cols != rgbaBuffer_[idx].cols) {
                        rgbaBuffer_[idx] = cv::Mat(image.rows, image.cols, CV_8UC4);
                }
                cv::Mat alpha(image.rows, image.cols, CV_8UC1, cv::Scalar(255));
                cv::Mat in[] = { image, alpha };

                int from_to[] = { 0, 0, 1, 1, 2, 2, 1, 3 };
                mixChannels(in, 2, &rgbaBuffer_[idx], 1, from_to, 4);
        } catch (...) {
                ROS_ERROR("error in cvMat2QImage");
                return QImage();
        }

        return QImage((unsigned char *) (rgbaBuffer_[idx].data),
                        rgbaBuffer_[idx].cols, rgbaBuffer_[idx].rows, rgbaBuffer_[idx].step,
                        QImage::Format_RGB32);
}