EdgeConnectorGraphBased.cpp

Go to the documentation of this file.

#include <face_contour_detector/filters/EdgeConnectorGraphBased.h>
#include <string>
#include <face_contour_detector/ImageForest.h>
#include <map>
#include <vector>
#include <opencv/cv.h>
#include <ros/assert.h>

namespace face_contour_detector {

        namespace filters {

                EdgeConnectorGraphBased::EdgeConnectorGraphBased() {
                        ResetParameters();
                }

                std::vector<Parameter> EdgeConnectorGraphBased::GetParameters() {
                        std::vector<Parameter> re;
                        std::string name("searchRadius");
                        re.push_back(Parameter(name, &m_searchRadius, 2, 50));
                        return re;
                }

                void EdgeConnectorGraphBased::Apply(const cv::Mat& input, cv::Mat& result) {
                        ROS_ASSERT(input.channels() == 1);

                        ImageForest forest(input, 200);
                        std::map<int, ImageForest::Graph> graphs = forest.GetGraphs();

                        result = input;

                        for (int i = 1; i <= forest.NumGraphs(); i++) {
                                std::vector<ImageForest::Point>::iterator itPoints;
                                for (itPoints = graphs[i].endPoints.begin(); itPoints != graphs[i].endPoints.end(); itPoints++) {
                                        ImageForest::Point found(-1,-1);
                                        for (int dy = -m_searchRadius; dy <= m_searchRadius; dy++) {
                                                for (int dx = -m_searchRadius; dx <= m_searchRadius; dx++) {
                                                        int x = dx + itPoints->x;
                                                        int y = dy + itPoints->y;
                                                        if (x >= 0 && x < input.cols && y >= 0 && y < input.rows) {
                                                                int otherId = forest.GetGraphId(x,y);
                                                                if (otherId > i) {
                                                                        if (found.x == -1 ||
                                                                                itPoints->Distance(ImageForest::Point(x,y)) < itPoints->Distance(found)) {

                                                                                found = ImageForest::Point(x,y);
                                                                        }
                                                                }
                                                        }
                                                }
                                        }
                                        if (found.x != -1) {
                                                M_Connect(result, itPoints->x, itPoints->y, found.x, found.y);
                                        }
                                }
                        }
                }

                void EdgeConnectorGraphBased::ResetParameters() {
                        m_searchRadius = 4;
                        m_connectValue = 255;
                }

                const std::string& EdgeConnectorGraphBased::GetFilterName() {
                        return m_filterName;
                }

                void EdgeConnectorGraphBased::M_Connect(cv::Mat& image, int x1, int y1, int x2, int y2) {
                                ROS_ASSERT(x1 < image.cols && x1 >= 0);
                                ROS_ASSERT(x2 < image.cols && x2 >= 0);
                                ROS_ASSERT(y1 < image.rows && y1 >= 0);
                                ROS_ASSERT(y2 < image.rows && y2 >= 0);
                                //std::cout<<"connect ("<<x1<<", "<<y1<<") to ("<<x2<<", "<<y2<<")"<<std::endl;
                                //image.at<unsigned char>(y1,x1) = 170;
                                //image.at<unsigned char>(y2,x2) = 170;
                                double dx = x2-x1; //=c
                                double dy = y2-y1; //=a
                                //std::cout<<"dx="<<dx<<" dy="<<dy<<std::endl;
                                //special cases
                                //only y direction
                                if (dx == 0) {
                                        if (dy > 0) {
                                                for (int i = 1; i < dy; i++) {
                                                        image.at<unsigned char>(y1+i, x1) = m_connectValue;
                                                }
                                        } else if (dy < 0) {
                                                for (int i = -1; i > dy; i--) {
                                                        image.at<unsigned char>(y1+i, x1) = m_connectValue;
                                                }
                                        }
                                        return;
                                }
                                //only x direction
                                if (dy == 0) {
                                        if (dx > 0) {
                                                for (int i = 1; i < dx; i++) {
                                                        image.at<unsigned char>(y1, x1+i) = m_connectValue;
                                                }
                                        } else if (dx < 0) {
                                                for (int i = -1; i > dx; i--) {
                                                        image.at<unsigned char>(y1, x1+i) = m_connectValue;
                                                }
                                        }
                                        return;
                                }
                                //same stepsize in x and y direction
                                if (dx == dy) {
                                        if (dx > 0) {
                                                for (int i = 1; i < dx; i++) {
                                                        image.at<unsigned char>(y1+1, x1+i) = m_connectValue;
                                                }
                                        } else if (dx < 0) {
                                                for (int i = -1; i > dx; i--) {
                                                        image.at<unsigned char>(y1+1, x1+1) = m_connectValue;
                                                }
                                        }
                                        return;
                                }
                                //non special cases
                                //                C
                                //              / |
                                //    A - B
                                double b = sqrt(dx*dx + dy*dy);
                                double alpha = acos((b*b + dx*dx - dy*dy)/(2.0*std::abs(b)*std::abs(dx)));
                                double sinAlpha = sin(alpha);
                                double sinGamma = sin(M_PI/2.0 - alpha);
                                //x-direction
                                if (dx > 0.0) {
                                        for (double currX = double(x1)+0.5; currX < x2; currX += 1.0) {
                                                double currY = (currX-double(x1))*sinAlpha/sinGamma;
                                                if (dy < 0) {
                                                        currY *= -1;
                                                }
                                                currY += y1;
                                                ROS_ASSERT(currY >= 0 && currY < image.rows);
                                                image.at<unsigned char>(int(round(currY)), int(ceil(currX))) = m_connectValue;
                                        }
                                } else if (dx < 0.0) {
                                        for (double currX = double(x1)-0.5; currX > x2; currX -= 1.0) {
                                                double currY = (double(x1)-currX)*sinAlpha/sinGamma;
                                                if (dy < 0) {
                                                        currY *= -1.0;
                                                }
                                                currY += y1;
                                                ROS_ASSERT(currY >= 0 && currY < image.rows);
                                                image.at<unsigned char>(int(round(currY)), int(floor(currX))) = m_connectValue;
                                        }
                                }
                                //y-direction
                                if (dy > 0.0) {
                                        for (double currY = double(y1)+0.5; currY < y2; currY += 1.0) {
                                                double currX = (currY-double(y1))*sinGamma/sinAlpha;
                                                if (dx < 0) {
                                                        currX *= -1.0;
                                                }
                                                currX += x1;
                                                ROS_ASSERT(currX >= 0 && currX < image.cols);
                                                image.at<unsigned char>(int(ceil(currY)), int(round(currX))) = m_connectValue;
                                        }
                                } else if (dy < 0.0) {
                                        for (double currY = double(y1)-0.5; currY > y2; currY -= 1.0) {
                                                double currX = (double(y1)-currY)*sinGamma/sinAlpha;
                                                if (dx < 0) {
                                                        currX *= -1.0;
                                                }
                                                currX += x1;
                                                ROS_ASSERT(currX >= 0 && currX < image.cols);
                                                image.at<unsigned char>(int(floor(currY)), int(round(currX))) = m_connectValue;
                                        }
                                }
                        }

                //static
                std::string EdgeConnectorGraphBased::m_filterName = std::string("EdgeConnectorGraphBased");
        } //namespace filters

} //namespace face_contour_detector