MaskBlur.cpp

Go to the documentation of this file.

#include <face_contour_detector/filters/MaskBlur.h>

#include <limits>
#include <ros/ros.h>
#include <ros/assert.h>
#include <cmath>

namespace face_contour_detector {

        namespace filters {

                MaskBlur::MaskBlur(int blurWidth, face_contour_detector::Bgr8 blurColor) : m_blurWidth(blurWidth), m_blurColor(blurColor), m_maskPtr(0) { }
                void MaskBlur::SetMask(const cv::Mat& mask, unsigned char maskTrue, unsigned char maskFalse) {
                        ROS_ASSERT(mask.type() == CV_8UC1);
                        m_maskPtr = &mask;
                        m_maskTrue = maskTrue;
                        m_maskFalse = maskFalse;
                }
                void MaskBlur::Apply(const cv::Mat& input, cv::Mat& output) const {
                        ROS_ASSERT(m_maskPtr->cols == input.cols && m_maskPtr->rows == input.rows);
                        ROS_ASSERT(m_maskPtr->cols == output.cols && m_maskPtr->rows == output.rows);
                        ROS_ASSERT(input.type() == CV_8UC3);
                        ROS_ASSERT(output.type() == CV_8UC3);
                        ROS_ASSERT(m_maskPtr != 0);
                        for (int y = 0; y < input.rows; y++) {
                                for (int x = 0; x < input.cols; x++) {

                                        if (m_maskPtr->at<unsigned char>(y,x) == m_maskFalse) {
                                                //scan the surounding
                                                double minDistance = std::numeric_limits<double>::infinity();
                                                Bgr8 closestColor;
                                                for (int dy = m_blurWidth*-1; dy <= m_blurWidth; dy++) {
                                                        for (int dx = m_blurWidth*-1; dx <= m_blurWidth; dx++) {
                                                                int newY = y + dy;
                                                                int newX = x + dx;
                                                                if (newY >= 0 && newY < output.rows &&
                                                                                newX >= 0 && newX < output.cols) {

                                                                        if (m_maskPtr->at<unsigned char>(newY,newX) == m_maskTrue) {
                                                                                double distance = sqrt(double(dx*dx + dy*dy));
                                                                                if (distance < minDistance) {
                                                                                        minDistance = distance;
                                                                                        closestColor = input.at<Bgr8>(newY,newX);
                                                                                }
                                                                        }
                                                                }
                                                        }
                                                }
                                                if (minDistance == std::numeric_limits<double>::infinity()) {
                                                        //out of range -> setting to new value
                                                        output.at<Bgr8>(y,x) = m_blurColor;
                                                } else {
                                                        //weightet adding of the values
                                                        double maxDist = sqrt(double(m_blurWidth*m_blurWidth*2));
                                                        Bgr8 re;
                                                        re.b = 0;
                                                        re.g = 0;
                                                        re.r = 0;
                                                        //ROS_INFO("blurColor is (%u, %u, %u)", m_blurColor.b, m_blurColor.g, m_blurColor.r);
                                                        /*double reDoubleB = std::floor((double(input.at<Bgr8>(y,x).b)*(maxDist-minDistance) + double(m_blurColor.b)*minDistance)/maxDist);
                                                        double reDoubleG = std::floor((double(input.at<Bgr8>(y,x).g)*(maxDist-minDistance) + double(m_blurColor.g)*minDistance)/maxDist);
                                                        double reDoubleR = std::floor((double(input.at<Bgr8>(y,x).r)*(maxDist-minDistance) + double(m_blurColor.r)*minDistance)/maxDist);*/
                                                        double reDoubleB = std::floor((double(closestColor.b)*(maxDist-minDistance) + double(m_blurColor.b)*minDistance)/maxDist);
                                                        double reDoubleG = std::floor((double(closestColor.g)*(maxDist-minDistance) + double(m_blurColor.g)*minDistance)/maxDist);
                                                        double reDoubleR = std::floor((double(closestColor.r)*(maxDist-minDistance) + double(m_blurColor.r)*minDistance)/maxDist);

                                                        //this is super ineffective
                                                        ROS_ASSERT(reDoubleB >= 0.0 && reDoubleB <= 255.0);
                                                        ROS_ASSERT(reDoubleG >= 0.0 && reDoubleG <= 255.0);
                                                        ROS_ASSERT(reDoubleR >= 0.0 && reDoubleR <= 255.0);
                                                        unsigned char iC = 0;
                                                        for (int i = 0; i <= 255; i++) {
                                                                if (reDoubleB == double(i)) {
                                                                        re.b = iC;
                                                                }
                                                                if (reDoubleG == double(i)) {
                                                                        re.g = iC;
                                                                }
                                                                if (reDoubleR == double(i)) {
                                                                        re.r = iC;
                                                                }
                                                                iC++;
                                                        }
                                                        output.at<Bgr8>(y,x) = re;
                                                }

                                        } else if (m_maskPtr->at<unsigned char>(y,x) == m_maskTrue) {
                                                output.at<Bgr8>(y,x) = input.at<Bgr8>(y,x);
                                        } else {
                                                Bgr8 tmp;
                                                tmp.b = 255;
                                                tmp.g = 0;
                                                tmp.r = 0;
                                                output.at<Bgr8>(y,x) = tmp;
                                                ROS_ERROR("The value in the mask(at x=%i,y=%i) in neither true (%u) or false (%u) it is %u",x,y,m_maskTrue,m_maskFalse,m_maskPtr->at<unsigned char>(y,x));
                                        }

                                }
                        }
                }

        } //namespace filters

} //namespace face_contour_detector