ImageForest.cpp

Go to the documentation of this file.

#include <face_contour_detector/ImageForest.h>

#include <map>
#include <vector>
#include <limits>
#include <face_contour_detector/Bgr8.h>
#include <ros/assert.h>

namespace face_contour_detector {

        ImageForest::Point::Point(int x, int y) : x(x), y(y) { }
        ImageForest::Point::Point(const Point& other) : x(other.x), y(other.y) { }

        ImageForest::Graph::Graph(): id(0), numPixels(0) {}
        ImageForest::Graph::Graph(const Graph& other): id(other.id), numPixels(other.numPixels), endPoints(other.endPoints), connectingPoints(other.connectingPoints) {}

        ImageForest::ImageForest() : m_width(0), m_height(0), m_lastId(0), m_values(0)  {

        }

        ImageForest::ImageForest(const cv::Mat& mat, unsigned char threshold) : m_width(0), m_height(0), m_lastId(0), m_values(0) {
                ReadFromImage(mat, threshold);
        }

        ImageForest::~ImageForest() {
                delete [] m_values;
        }

        void ImageForest::ReadFromImage(const cv::Mat& mat, unsigned char threshold) {
                assert (mat.cols > 0 && mat.rows > 0);
                m_width = mat.cols;
                m_height = mat.rows;
                if (m_values != 0) {
                        delete [] m_values;
                }
                m_lastId = 0;

                m_values = new int[m_width * m_height];

                //Write all 0 into the array
                for (int y = 0; y < m_height; y++) {
                        for(int x = 0; x < m_width; x++) {
                                ROS_ASSERT(y*m_width + x < m_height*m_width);
                                m_values[y*m_width + x] = 0;
                        }
                }

                //Find all the patches
                int m_lastPatchId = 0;
                for (int y = 0; y < mat.rows; y++) {
                        for(int x = 0; x < mat.cols; x++) {
                                if (mat.at<unsigned char>(y,x) >= threshold) {

                                        int foundId = 0;
                                        int maxY = y == m_height-1?0:1;
                                        int maxX = x == m_width-1?0:1;
                                        for (int y2 = y==0?0:-1; y2 <= maxY; y2++) {
                                                for (int x2 = x==0?0:-1; x2 <= maxX; x2++) {
                                                        ROS_ASSERT((y+y2)*m_width + (x+x2) < m_height*m_width);
                                                        if (m_values[(y+y2)*m_width + (x+x2)] != 0) {
                                                                foundId = m_values[(y+y2)*m_width + (x+x2)];
                                                                break;
                                                        }
                                                }
                                                if (foundId != 0)
                                                        break;
                                        }

                                        if (foundId != 0) {
                                                ROS_ASSERT(y*m_width + x < m_height*m_width);
                                                m_values[y*m_width + x] = foundId;
                                        } else {
                                                ROS_ASSERT(y*m_width + x < m_height*m_width);
                                                m_lastPatchId++;
                                                m_values[y*m_width + x] = m_lastPatchId;
                                        }
                                }
                        }
                }

                //Find the eq classes
                //set up eq classes map
                int* eqMap = new int[m_lastPatchId+1];
                for (int i = 0; i < m_lastPatchId+1; i++) {
                        ROS_ASSERT(i < m_lastPatchId+1);
                        eqMap[i] = 0;
                }
                //int m_lastId = 0;

                //finding eq classes
                for (int y = 0; y < m_height; y++) {
                        for(int x = 0; x < m_width; x++) {

                                if (mat.at<unsigned char>(y,x) >= threshold) {

                                        int mainValue = m_values[y*m_width + x];
                                        int maxY = y == m_height-1?0:1;
                                        int maxX = x == m_width-1?0:1;

                                        for (int y2 = y==0?0:-1; y2 <= maxY; y2++) {
                                                for (int x2 = x==0?0:-1; x2 <= maxX; x2++) {

                                                        int subValue = m_values[(y+y2)*m_width + (x+x2)];
                                                        if (subValue != 0 && subValue != mainValue) {
                                                                M_SetEqClass(eqMap, m_lastPatchId+1, subValue, mainValue);
                                                        }
                                                }
                                        }
                                }
                        }
                }

                //resetting the eq classes id's
                int* finalEqMap = new int[m_lastPatchId+1];
                for (int i = 0; i < m_lastPatchId+1; i++) {
                        ROS_ASSERT(i < m_lastPatchId+1);
                        finalEqMap[i] = 0;
                }

                int numEqClasses = 0;
                std::map<int, bool> exist;

                for (int i = 0; i < m_lastPatchId+1; i++) {
                        if (eqMap[i] == 0 || exist.find(eqMap[i]) == exist.end()) {
                                numEqClasses++;
                                if (eqMap[i] != 0) {
                                        exist[eqMap[i]] = true;
                                        for (int i2 = 0; i2 < m_lastPatchId+1; i2++) {
                                                if (eqMap[i] == eqMap[i2]) {
                                                        ROS_ASSERT(i2 < m_lastPatchId+1);
                                                        finalEqMap[i2] = numEqClasses;
                                                }
                                        }
                                } else {
                                        ROS_ASSERT(i < m_lastPatchId+1);
                                        finalEqMap[i] = numEqClasses;
                                }

                        }
                }

                //writting the new class id's to the value array
                for (int y = 0; y < m_height; y++) {
                        for(int x = 0; x < m_width; x++) {
                                ROS_ASSERT(y*m_width + x < m_width*m_height);
                                if (m_values[y*m_width + x] != 0) {
                                        m_values[y*m_width + x] = finalEqMap[m_values[y*m_width + x]];
                                }
                        }
                }

                m_count = numEqClasses;

                delete [] finalEqMap;
                delete [] eqMap;
        }



        cv::Mat ImageForest::GenerateImage() {
                ROS_ASSERT(m_width > 0 && m_height > 0 && m_values != 0);
                cv::Mat re = cv::Mat(m_height, m_width, CV_8UC3);
                GenerateImage(re);
                return re;
        }

        void ImageForest::GenerateImage(cv::Mat& mat) {
                ROS_ASSERT(mat.channels() == 3 &&
                                mat.cols == m_width &&
                                mat.rows == m_height);

                Bgr8 colors[6];
                colors[0].b = 255;
                colors[0].g = 0;
                colors[0].r = 0;

                colors[1].b = 0;
                colors[1].g = 255;
                colors[1].r = 0;

                colors[2].b = 0;
                colors[2].g = 0;
                colors[2].r = 255;

                colors[3].b = 255;
                colors[3].g = 255;
                colors[3].r = 0;

                colors[4].b = 0;
                colors[4].g = 255;
                colors[4].r = 255;

                colors[5].b = 255;
                colors[5].g = 0;
                colors[5].r = 255;

                //first run find eq classes
                for (int y = 0; y < m_height; y++) {
                        for (int x = 0; x < m_width; x++) {
                                if (m_values[m_width*y + x] != 0) {
                                        ROS_ASSERT(m_values[m_width*y + x]%6 < 6 && m_values[m_width*y + x]%6 >= 0);
                                        mat.at<Bgr8>(y,x) = colors[m_values[m_width*y + x]%6];
                                } else {
                                        mat.at<Bgr8>(y,x).b = 0;
                                        mat.at<Bgr8>(y,x).g = 0;
                                        mat.at<Bgr8>(y,x).r = 0;
                                }
                        }
                }
        }

        std::map<int, ImageForest::Graph> ImageForest::GetGraphs() {
                if (m_count == 0) {
                        return std::map<int,ImageForest::Graph>();
                }
                //run thorugh the picture and save an endpoint for all graphids
                std::map<int, ImageForest::Graph> graphs;
                //initiallize the data
                for (int graphId = 0; graphId < m_count; graphId++) {
                        graphs[graphId] = ImageForest::Graph();
                        graphs[graphId].id = graphId;
                        graphs[graphId].numPixels = 0;
                }
                //run through
                for (int y = 0; y < m_height; y++) {
                        for (int x = 0; x < m_width; x++) {
                                int graphId = GetGraphId(x,y);
                                if (graphId != 0) {
                                        //Add to the length
                                        graphs[graphId].numPixels = graphs[graphId].numPixels+1;
                                        //Check what kind of point this is
                                        int connectingNum = 0;
                                        bool found[9];
                                        for (int i = 0; i < 9; i++) {
                                                ROS_ASSERT(i < 9);
                                                found[i] = false;
                                        }
                                        int i = 0;
                                        for (int y2 = -1; y2 <= 1; y2++) {
                                                for (int x2 = -1; x2 <= 1; x2++) {
                                                        if (!(y2 == 0 && x2 == 0) &&
                                                                x+x2 >= 0 && x+x2 < m_width &&
                                                                y+y2 >= 0 && y+y2 < m_height) {

                                                                int neiGraphId = GetGraphId(x+x2,y+y2);
                                                                if (neiGraphId == graphId) {
                                                                        ROS_ASSERT(i < 9);
                                                                        found[i] = true;
                                                                        bool check = true;
                                                                        if (i == 1 ||
                                                                                i == 2 ||
                                                                                i == 7 ||
                                                                                i == 8) {
                                                                                ROS_ASSERT(i-1 < 9 && i-1 >= 0);
                                                                                check = !found[i-1];
                                                                        }
                                                                        if (i == 3 ||
                                                                                i == 5 ||
                                                                                i == 6 ||
                                                                                i == 8) {
                                                                                ROS_ASSERT(i-3 < 9 && i-3 >= 0);
                                                                                check = check && !found[i-3];
                                                                        }

                                                                        if (check) {
                                                                                connectingNum++;
                                                                        }
                                                                } else if (neiGraphId != 0) {
                                                                        //this should never happen, but just to be sure if there is not some kind of bug
                                                                        ROS_ASSERT(false);
                                                                }
                                                        }
                                                        i++;
                                                }
                                        }
                                        if (connectingNum <= 1 ) {
                                                graphs[graphId].endPoints.push_back(Point(x,y));
                                        } else if (connectingNum > 2) {
                                                graphs[graphId].connectingPoints.push_back(Point(x,y));
                                        }
                                }
                        }
                }

                return graphs;
        }

        double ImageForest::CallculateEdgyness() {
                ROS_ASSERT(m_values != 0);
                double re = 0;
                for (int y = 0; y < Height(); y++) {
                        for (int x = 0; x < Width(); x++) {
                                if (GetGraphId(x,y) != 0) {
                                        std::vector<ImageForest::Point> vecs;
                                        M_GetVectors(x, y, vecs);
                                        std::vector<ImageForest::Point>::iterator it;
                                        double currMin = std::numeric_limits<double>::infinity();
                                        for (it = vecs.begin(); it != vecs.end(); it++) {
                                                std::vector<ImageForest::Point>::iterator it2;
                                                double currValue = 0.0;
                                                for (it2 = vecs.begin(); it2 != vecs.end(); it2++) {

                                                        double angle = acos(
                                                                        (double(it->x*it2->x + it->y*it2->y))/
                                                                        (sqrt(double(it->x*it->x + it->y*it->y))*sqrt(double(it2->x*it2->x + it2->y*it2->y)))
                                                                );
                                                        //pi/4
                                                        if (angle > 0.1 && angle < (M_PI/4.0)+0.1) {
                                                                currValue += 3.0;
                                                        //pi/2
                                                        } else if (angle > (M_PI/4.0)+0.1 && angle < (M_PI/2.0)+0.1) {
                                                                currValue += 2.0;
                                                        //3*pi/4
                                                        } else if (angle > (M_PI/2.0)+0.1 && angle < (3.0*M_PI/4.0)+0.1) {
                                                                currValue += 1.0;
                                                        }
                                                }
                                                if (currValue < currMin) {
                                                        currMin = currValue;
                                                }
                                        }
                                        re += currMin;
                                }
                        }
                }
                return re;
        }

        double ImageForest::CallculateGraphMass() {
                double numGraphPixels = 0.0;
                for (int y = 0; y < Height(); y++) {
                        for (int x = 0; x < Width(); x++) {
                                if (GetGraphId(x,y) != 0) {
                                        numGraphPixels += 1.0;
                                }
                        }
                }
                /*std::cout<<"width*height="<<Width()*Height()<<std::endl;
                std::cout<<"numGraphPixels="<<numGraphPixels<<std::endl;
                std::cout<<"/="<<numGraphPixels / (double(Width()*Height())-numGraphPixels)<<std::endl;*/
                return numGraphPixels / (double(Width()*Height())-numGraphPixels);
        }

        void ImageForest::M_SetEqClass(int* eqMap, int size, int a, int b) {
                int target = 0;
                std::vector<int> combine;
                combine.push_back(a);
                combine.push_back(b);
                if (eqMap[a] != 0) {
                        target = eqMap[a];
                }
                if (eqMap[b] != 0 && eqMap[b] < target) {
                        target = eqMap[b];
                }

                if (eqMap[a] != 0 || eqMap[b] != 0) {
                        for (int i = 0; i < size; i++) {
                                if (a != i && b != i) {
                                        if (eqMap[a] != 0 && eqMap[a] == eqMap[i]) {
                                                combine.push_back(i);
                                                if (target == 0 || target > eqMap[a]) {
                                                        target = eqMap[a];
                                                }
                                        } else if (eqMap[b] != 0 && eqMap[b] == eqMap[i]) {
                                                combine.push_back(i);
                                                if (target == 0 || target > eqMap[b]) {
                                                        target = eqMap[b];
                                                }
                                        }
                                }
                        }
                }

                if (target == 0) {
                        m_lastId++,
                        target = m_lastId;
                }

                for(std::vector<int>::iterator it = combine.begin(); it < combine.end(); it++) {
                        ROS_ASSERT(*it < size && *it >= 0);
                        eqMap[*it] = target;
                }
        }
} //namespace face_contour_detector